[wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850)

Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.

This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.

To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.

Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

.github/workflows/sanitizers.yml

@@ -29,11 +29,11 @@ jobs:
             ctest-env: ""
             ctest-flags: ""
     name: "${{ matrix.name }}"
-    runs-on: ubuntu-22.04
-    container: wpilib/roborio-cross-ubuntu:2025-22.04
+    runs-on: ubuntu-24.04
+    container: wpilib/roborio-cross-ubuntu:2025-24.04
     steps:
       - name: Install Dependencies
-        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java clang-14 libprotobuf-dev protobuf-compiler ninja-build
+        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-17 libprotobuf-dev protobuf-compiler ninja-build
 
       - name: Install sccache
         uses: mozilla-actions/sccache-action@v0.0.5
@@ -41,7 +41,7 @@ jobs:
       - uses: actions/checkout@v4
 
       - name: configure
-        run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
+        run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-17 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-17 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
         env:
           SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}

CODE_OF_CONDUCT.md

@@ -1,69 +1,47 @@
-# Contributor Covenant Code of Conduct
+# Contributor Community Code of Conduct
 
 ## Our Pledge
 
-We as members, contributors, and leaders pledge to make participation in our
-community a harassment-free experience for everyone, regardless of age, body
-size, visible or invisible disability, ethnicity, sex characteristics, gender
-identity and expression, level of experience, education, socio-economic status,
-nationality, personal appearance, race, religion, or sexual identity
-and orientation.
+As members, contributors, and leaders, we commit to fostering a community where everyone feels safe, respected, and valued. We are dedicated to ensuring that participation in this community is harassment-free, inclusive, and welcoming, regardless of age, body type, abilities (visible or invisible), ethnicity, gender identity or expression, sexual orientation, socioeconomic background, education, nationality, personal appearance, race, or religion.
+
+Above all, we pledge to act with integrity, kindness, and empathy—striving to be not only good participants but also good humans.
 
-We pledge to act and interact in ways that contribute to an open, welcoming,
-diverse, inclusive, and healthy community.
 
 ## Our Standards
 
-Examples of behavior that contributes to a positive environment for our
-community include:
+Positive and respectful behavior is essential to creating a thriving community. This includes:
 
-* Exhibiting Gracious Professionalism® at all times. Gracious Professionalism
+* Practicing **Gracious Professionalism®** at all times. Gracious Professionalism
   is a way of doing things that encourages high-quality work, emphasizes the
   value of others, and respects individuals and the community.
-* Demonstrating empathy and kindness toward other people
-* Being respectful of differing opinions, viewpoints, and experiences
-* Giving and gracefully accepting constructive feedback
-* Accepting responsibility and apologizing to those affected by our mistakes,
-  and learning from the experience
-* Focusing on what is best not just for us as individuals, but for the
-  overall community
+* Showing empathy, kindness, and patience.
+* Respecting diverse perspectives and experiences.
+* Giving and receiving constructive feedback with openness and humility.
+* Owning mistakes, apologizing when necessary, and learning from them.
+* Prioritizing the well-being and success of the entire community over individual interests.
 
-Examples of unacceptable behavior include:
+Unacceptable behavior include:
 
-* The use of sexualized language or imagery, and sexual attention or
-  advances of any kind
+* Using sexualized language, imagery, or making inappropriate advances
 * Trolling, insulting or derogatory comments, and personal or political attacks
-* Public or private harassment
-* Publishing others' private information, such as a physical or email
-  address, without their explicit permission
-* Other conduct which could reasonably be considered inappropriate in a
-  professional setting
+* Harassment in any form, whether public or private.
+* Sharing private information (e.g., email or physical addresses) without explicit consent.
+
+* Any behavior that is unprofessional, harmful, or exclusionary.
 
 ## Enforcement Responsibilities
 
-Community leaders are responsible for clarifying and enforcing our standards of
-acceptable behavior and will take appropriate and fair corrective action in
-response to any behavior that they deem inappropriate, threatening, offensive,
-or harmful.
-
-Community leaders have the right and responsibility to remove, edit, or reject
-comments, commits, code, wiki edits, issues, and other contributions that are
-not aligned to this Code of Conduct, and will communicate reasons for moderation
-decisions when appropriate.
+Community leaders are responsible for maintaining these standards and will take appropriate action to address any behavior deemed harmful, threatening, or inappropriate. Actions may include removing content, issuing warnings, or, when necessary, banning individuals. Moderation decisions will be communicated transparently where appropriate.
 
 ## Scope
 
-This Code of Conduct applies within all community spaces, and also applies when
-an individual is officially representing the community in public spaces.
-Examples of representing our community include using an official e-mail address,
-posting via an official social media account, or acting as an appointed
-representative at an online or offline event.
+This Code of Conduct applies to all community spaces, events, and instances where individuals represent the community (e.g., official email accounts, social media posts, or in-person/virtual events).
 
 ## Enforcement
 
 Instances of abusive, harassing, or otherwise unacceptable behavior may be
 reported to the community leaders responsible for enforcement at
-[conduct@wpilib.org](mailto:conduct@wpilib.org).
+[wpilib@wpi.edu](mailto:wpilib@wpi.edu).
 All complaints will be reviewed and investigated promptly and fairly.
 
 All community leaders are obligated to respect the privacy and security of the
@@ -115,6 +93,9 @@ individual, or aggression toward or disparagement of classes of individuals.
 **Consequence**: A permanent ban from any sort of public interaction within
 the community.
 
+## A Note on Kindness
+Building a community isn’t just about rules—it’s about connection. Every interaction is an opportunity to be understanding, compassionate, and supportive. Being a good human is the key to our ethos.
+
 ## Attribution
 
 This Code of Conduct is adapted from the [Contributor Covenant][homepage],

CONTRIBUTING.md

@@ -80,7 +80,7 @@ xₖ₊₁ = Axₖ + Buₖ
 
 Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered.
 
-During the build season, breaking changes or other changes intended for the next season can be created as a pull request against the development branch of WPILib. After the season is over, the changes in the development branch will be merged into main.
+Particularly large and/or breaking changes should be targeted to the 2027 branch, which targets the [SystemCore Robot Controller](https://community.firstinspires.org/introducing-the-future-mobile-robot-controller). The intent is minimize changes for 2026, to allow development to focus on preparing for 2027.
 
 ### Merge Process
 

README.md

@@ -136,6 +136,9 @@ If you have installed the FRC Toolchain to a directory other than the default, o
 
 Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes.
 
+> [!NOTE]
+> The `/format` action has been temporarily disabled. The individual formatting commands can be run locally as shown below. Alternately, the Lint and Format action for a PR will upload a patch file that can be downloaded and applied manually.
+
 #### wpiformat
 
 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms.

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java

@@ -269,12 +269,18 @@ public class AnnotationProcessor extends AbstractProcessor {
       return false;
     }
 
-    processingEnv
-        .getMessager()
-        .printMessage(
-            Diagnostic.Kind.NOTE,
-            "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
-            element);
+    var classConfig = element.getEnclosingElement().getAnnotation(Logged.class);
+
+    if (classConfig == null || classConfig.warnForNonLoggableTypes()) {
+      // Not loggable and not explicitly opted out of logging; print a warning message
+      processingEnv
+          .getMessager()
+          .printMessage(
+              Diagnostic.Kind.NOTE,
+              "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type",
+              element);
+    }
+
     return true;
   }
 

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java

@@ -71,6 +71,11 @@ public class LoggerGenerator {
         public Naming defaultNaming() {
           return Naming.USE_CODE_NAME;
         }
+
+        @Override
+        public boolean warnForNonLoggableTypes() {
+          return false;
+        }
       };
 
   public LoggerGenerator(ProcessingEnvironment processingEnv, List<ElementHandler> handlers) {

epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/AnnotationProcessorTest.java

@@ -1772,7 +1772,7 @@ class AnnotationProcessorTest {
         """
         package edu.wpi.first.epilogue;
 
-        @Logged
+        @Logged(warnForNonLoggableTypes = true)
         class Example {
           Throwable t;
         }

epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java

@@ -124,4 +124,12 @@ public @interface Logged {
    *     for all logged fields and methods in an annotated class
    */
   Naming defaultNaming() default Naming.USE_CODE_NAME;
+
+  /**
+   * Class-level only: if {@link #strategy()} is {@link Strategy#OPT_OUT}, this can be used to quiet
+   * the warnings that are printed for non-loggable fields and methods detected within the class.
+   *
+   * @return true if warnings should be printed, or false if warnings should not be printed
+   */
+  boolean warnForNonLoggableTypes() default false;
 }

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -224,8 +224,8 @@ class ObjectInfo {
 
 class FieldInfo {
  public:
-  static constexpr auto kDefaultWidth = 16.541052_m;
-  static constexpr auto kDefaultHeight = 8.211_m;
+  static constexpr auto kDefaultWidth = 17.5483_m;
+  static constexpr auto kDefaultHeight = 8.0519_m;
 
   explicit FieldInfo(Storage& storage);
 
@@ -345,7 +345,7 @@ static bool InputPose(frc::Pose2d* pose) {
 }
 
 FieldInfo::FieldInfo(Storage& storage)
-    : m_builtin{storage.GetString("builtin", "2024 Crescendo")},
+    : m_builtin{storage.GetString("builtin", "2025 Reefscape")},
       m_filename{storage.GetString("image")},
       m_width{storage.GetFloat("width", kDefaultWidth.to<float>())},
       m_height{storage.GetFloat("height", kDefaultHeight.to<float>())},

shared/ceres.gradle

@@ -4,7 +4,6 @@ nativeUtils {
             groupId = "edu.wpi.first.thirdparty.frc2024.ceres"
             artifactId = "ceres-cpp"
             headerClassifier = "headers"
-            sourceClassifier = "sources"
             ext = "zip"
             version = '2.2-3'
             targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms)

sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp

@@ -8,6 +8,7 @@
 
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
+#include <frc/system/plant/LinearSystemId.h>
 #include <units/acceleration.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
@@ -18,6 +19,7 @@ using namespace sysid;
 
 using Kv_t = decltype(1_V / 1_mps);
 using Ka_t = decltype(1_V / 1_mps_sq);
+using Matrix1d = Eigen::Matrix<double, 1, 1>;
 
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
@@ -26,39 +28,32 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
     return {0.0, 0.0};
   }
 
-  // If acceleration requires no effort, velocity becomes an input for position
-  // control. We choose an appropriate model in this case to avoid numerical
+  // If acceleration for position control requires no effort, velocity becomes
+  // an input. We choose an appropriate model in this case to avoid numerical
   // instabilities in the LQR.
-  if (Ka > 1E-7) {
-    // Create a position system from our feedforward gains.
-    frc::LinearSystem<2, 1, 1> system{
-        frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
-        frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
-        frc::Matrixd<1, 1>{0.0}};
-    // Create an LQR with 2 states to control -- position and velocity.
-    frc::LinearQuadraticRegulator<2, 1> controller{
-        system, {params.qp, params.qv}, {params.r}, preset.period};
-    // Compensate for any latency from sensor measurements, filtering, etc.
+  if (std::abs(Ka) < 1e-7) {
+    // System has position state and velocity input
+    frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0},
+                                      Matrix1d{1.0}, Matrix1d{0.0}};
+
+    frc::LinearQuadraticRegulator<1, 1> controller{
+        system, {params.qp}, {params.r}, preset.period};
     controller.LatencyCompensate(system, preset.period,
                                  preset.measurementDelay);
 
-    return {
-        controller.K(0, 0) * preset.outputConversionFactor,
-        controller.K(0, 1) * preset.outputConversionFactor /
-            (preset.normalized ? 1 : units::second_t{preset.period}.value())};
+    return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
   }
 
-  // This is our special model to avoid instabilities in the LQR.
-  auto system = frc::LinearSystem<1, 1, 1>(
-      Eigen::Matrix<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
-      Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{0.0});
-  // Create an LQR with one state -- position.
-  frc::LinearQuadraticRegulator<1, 1> controller{
-      system, {params.qp}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
+  auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
+
+  frc::LinearQuadraticRegulator<2, 1> controller{
+      system, {params.qp, params.qv}, {params.r}, preset.period};
   controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
-  return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
+  return {controller.K(0, 0) * preset.outputConversionFactor,
+          controller.K(0, 1) * preset.outputConversionFactor /
+              (preset.normalized ? 1 : units::second_t{preset.period}.value())};
 }
 
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
@@ -69,20 +64,16 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
   }
 
   // If acceleration for velocity control requires no effort, the feedback
-  // control gains approach zero. We special-case it here because numerical
-  // instabilities arise in LQR otherwise.
-  if (Ka < 1E-7) {
+  // control gains approach zero. We special-case it here to avoid numerical
+  // instabilities in LQR.
+  if (std::abs(Ka) < 1E-7) {
     return {0.0, 0.0};
   }
 
-  // Create a velocity system from our feedforward gains.
-  frc::LinearSystem<1, 1, 1> system{
-      frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
-      frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
-  // Create an LQR controller with 1 state -- velocity.
+  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      Kv_t{Kv}, Ka_t{Ka});
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qv}, {params.r}, preset.period};
-  // Compensate for any latency from sensor measurements, filtering, etc.
   controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
   return {controller.K(0, 0) * preset.outputConversionFactor /

sysid/src/main/native/include/sysid/analysis/FilteringUtils.h

@@ -41,7 +41,7 @@ class InvalidDataError : public std::exception {
    */
   explicit InvalidDataError(std::string_view message) {
     m_message = fmt::format(
-        "{}. Please verify that your units and data is reasonable and then "
+        "{} Please verify that your units and data is reasonable and then "
         "adjust your velocity threshold, test duration, and/or window size to "
         "try to fix this issue.",
         message);

sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp

@@ -7,7 +7,7 @@
 #include "sysid/analysis/FeedbackAnalysis.h"
 #include "sysid/analysis/FeedbackControllerPreset.h"
 
-TEST(FeedbackAnalysisTest, Velocity1) {
+TEST(FeedbackAnalysisTest, VelocitySystem1) {
   auto Kv = 3.060;
   auto Ka = 0.327;
 
@@ -20,7 +20,7 @@ TEST(FeedbackAnalysisTest, Velocity1) {
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
 
-TEST(FeedbackAnalysisTest, Velocity2) {
+TEST(FeedbackAnalysisTest, VelocitySystem2) {
   auto Kv = 0.0693;
   auto Ka = 0.1170;
 
@@ -33,6 +33,19 @@ TEST(FeedbackAnalysisTest, Velocity2) {
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
 
+TEST(FeedbackAnalysisTest, VelocitySystemWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 0.0;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 0.00, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, VelocityConversion) {
   auto Kv = 0.0693;
   auto Ka = 0.1170;
@@ -117,6 +130,19 @@ TEST(FeedbackAnalysisTest, Position) {
   EXPECT_NEAR(Kd, 2.48, 0.05);
 }
 
+TEST(FeedbackAnalysisTest, PositionWithSmallKa) {
+  auto Kv = 3.060;
+  auto Ka = 1e-10;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 19.97, 0.05);
+  EXPECT_NEAR(Kd, 0.00, 0.05);
+}
+
 TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
   auto Kv = 3.060;
   auto Ka = 0.327;

wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h

@@ -52,11 +52,11 @@ class LinearSystemSim {
    * @param dt The time between updates.
    */
   void Update(units::second_t dt) {
-    // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
-    // Bu_k
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ +
+    // Buₖ.
     m_x = UpdateX(m_x, m_u, dt);
 
-    // y = Cx + Du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.CalculateY(m_x, m_u);
 
     // Add noise. If the user did not pass a noise vector to the
@@ -115,7 +115,14 @@ class LinearSystemSim {
    *
    * @param state The new state.
    */
-  void SetState(const Vectord<States>& state) { m_x = state; }
+  void SetState(const Vectord<States>& state) {
+    m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.CalculateY(m_x, m_u);
+  }
 
  protected:
   /**

wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp

@@ -44,6 +44,15 @@ TEST(ElevatorSimTest, StateSpaceSim) {
   EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().value(), 0.2);
 }
 
+TEST(ElevatorSimTest, InitialState) {
+  constexpr auto startingHeight = 0.5_m;
+  frc::sim::ElevatorSim sim(frc::DCMotor::KrakenX60(2), 20, 8_kg, 0.1_m, 0_m,
+                            1_m, true, startingHeight, {0.01, 0.0});
+
+  EXPECT_DOUBLE_EQ(startingHeight.value(), sim.GetPosition().value());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}
+
 TEST(ElevatorSimTest, MinMax) {
   frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
                             0_m, 1_m, true, 0_m, {0.01});

wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp

@@ -21,3 +21,12 @@ TEST(SingleJointedArmTest, Disabled) {
   // The arm should swing down.
   EXPECT_NEAR(sim.GetAngle().value(), -std::numbers::pi / 2, 0.01);
 }
+
+TEST(SingleJointedArmTest, InitialState) {
+  constexpr auto startingAngle = 45_deg;
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::KrakenX60(2), 125, 3_kg_sq_m,
+                                    30_in, 0_deg, 90_deg, true, startingAngle);
+
+  EXPECT_EQ(startingAngle, sim.GetAngle());
+  EXPECT_DOUBLE_EQ(0, sim.GetVelocity().value());
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java

@@ -75,10 +75,10 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    * @param dtSeconds The time between updates.
    */
   public void update(double dtSeconds) {
-    // Update X. By default, this is the linear system dynamics X = Ax + Bu
+    // Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ + Buₖ.
     m_x = updateX(m_x, m_u, dtSeconds);
 
-    // y = cx + du
+    // yₖ = Cxₖ + Duₖ
     m_y = m_plant.calculateY(m_x, m_u);
 
     // Add measurement noise.
@@ -164,6 +164,11 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
    */
   public void setState(Matrix<States, N1> state) {
     m_x = state;
+
+    // Update the output to reflect the new state.
+    //
+    //   yₖ = Cxₖ + Duₖ
+    m_y = m_plant.calculateY(m_x, m_u);
   }
 
   /**

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java

@@ -63,6 +63,17 @@ class ElevatorSimTest {
     }
   }
 
+  @Test
+  void testInitialState() {
+    double startingHeightMeters = 0.5;
+    var sim =
+        new ElevatorSim(
+            DCMotor.getKrakenX60(2), 20, 8.0, 0.1, 0.0, 1.0, true, startingHeightMeters, 0.01, 0.0);
+
+    assertEquals(startingHeightMeters, sim.getPositionMeters());
+    assertEquals(0, sim.getVelocityMetersPerSecond());
+  }
+
   @Test
   void testMinMax() {
     var sim =

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java

@@ -12,28 +12,46 @@ import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
 
 class SingleJointedArmSimTest {
-  SingleJointedArmSim m_sim =
-      new SingleJointedArmSim(
-          DCMotor.getVex775Pro(2),
-          300,
-          3.0,
-          Units.inchesToMeters(30.0),
-          -Math.PI,
-          0.0,
-          true,
-          Math.PI / 2.0);
-
   @Test
   void testArmDisabled() {
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getVex775Pro(2),
+            300,
+            3.0,
+            Units.inchesToMeters(30.0),
+            -Math.PI,
+            0.0,
+            true,
+            Math.PI / 2.0);
+
     // Reset Arm angle to 0
-    m_sim.setState(VecBuilder.fill(0.0, 0.0));
+    sim.setState(VecBuilder.fill(0.0, 0.0));
 
     for (int i = 0; i < 12 / 0.02; i++) {
-      m_sim.setInput(0.0);
-      m_sim.update(0.020);
+      sim.setInput(0.0);
+      sim.update(0.020);
     }
 
     // the arm should swing down
-    assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
+    assertEquals(-Math.PI / 2.0, sim.getAngleRads(), 0.1);
+  }
+
+  @Test
+  void testInitialState() {
+    double startingAngleRads = Math.PI / 4.0;
+    SingleJointedArmSim sim =
+        new SingleJointedArmSim(
+            DCMotor.getKrakenX60(2),
+            125,
+            3.0,
+            Units.inchesToMeters(30.0),
+            0,
+            Math.PI / 2.0,
+            true,
+            startingAngleRads);
+
+    assertEquals(startingAngleRads, sim.getAngleRads());
+    assertEquals(0, sim.getVelocityRadPerSec());
   }
 }

wpimath/algorithms.md

@@ -92,7 +92,7 @@ Substitute these into the feedforward equation.
   uₖ = kₛ sgn(x) + kᵥxₖ₊₁
 ```
 
-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0
 
 ```
   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -107,6 +107,21 @@ where
   B_d = A⁻¹(eᴬᵀ - I)B
 ```
 
+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Elevator feedforward
 
 ### Derivation
@@ -199,7 +214,7 @@ Substitute these into the feedforward equation.
   uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
 ```
 
-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0
 
 ```
   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -214,6 +229,21 @@ where
   B_d = A⁻¹(eᴬᵀ - I)B
 ```
 
+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
 
 ### Derivation

wpimath/src/main/java/edu/wpi/first/math/MathUtil.java

@@ -4,6 +4,9 @@
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+
 /** Math utility functions. */
 public final class MathUtil {
   private MathUtil() {
@@ -209,4 +212,62 @@ public final class MathUtil {
     double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
     return Math.abs(error) < tolerance;
   }
+
+  /**
+   * Limits translation velocity.
+   *
+   * @param current Translation at current timestep.
+   * @param next Translation at next timestep.
+   * @param dt Timestep duration.
+   * @param maxVelocity Maximum translation velocity.
+   * @return Returns the next Translation2d limited to maxVelocity
+   */
+  public static Translation2d slewRateLimit(
+      Translation2d current, Translation2d next, double dt, double maxVelocity) {
+    if (maxVelocity < 0) {
+      Exception e = new IllegalArgumentException();
+      MathSharedStore.reportError(
+          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
+      return next;
+    }
+    Translation2d diff = next.minus(current);
+    double dist = diff.getNorm();
+    if (dist < 1e-9) {
+      return next;
+    }
+    if (dist > maxVelocity * dt) {
+      // Move maximum allowed amount in direction of the difference
+      return current.plus(diff.times(maxVelocity * dt / dist));
+    }
+    return next;
+  }
+
+  /**
+   * Limits translation velocity.
+   *
+   * @param current Translation at current timestep.
+   * @param next Translation at next timestep.
+   * @param dt Timestep duration.
+   * @param maxVelocity Maximum translation velocity.
+   * @return Returns the next Translation3d limited to maxVelocity
+   */
+  public static Translation3d slewRateLimit(
+      Translation3d current, Translation3d next, double dt, double maxVelocity) {
+    if (maxVelocity < 0) {
+      Exception e = new IllegalArgumentException();
+      MathSharedStore.reportError(
+          "maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
+      return next;
+    }
+    Translation3d diff = next.minus(current);
+    double dist = diff.getNorm();
+    if (dist < 1e-9) {
+      return next;
+    }
+    if (dist > maxVelocity * dt) {
+      // Move maximum allowed amount in direction of the difference
+      return current.plus(diff.times(maxVelocity * dt / dist));
+    }
+    return next;
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -202,13 +202,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
    */
   public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
     // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
       return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
     } else {
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
       return kg
           + ks * Math.signum(currentVelocity)
           + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);

wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java

@@ -186,13 +186,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
    */
   public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
     // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
       return ks * Math.signum(nextVelocity) + kv * nextVelocity;
     } else {
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
       return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
     }
   }

wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java

@@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
   }
 
   /**
-   * Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
-   * of the filter.
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
+   * covariance (s) of the filter.
    *
    * @param x An array of the means.
    * @param s Square-root covariance of the filter.
@@ -86,6 +86,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
     // 2 * states + 1 by states
     Matrix<S, ?> sigmas =
         new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+
+    // equation (17)
     sigmas.setColumn(0, x);
     for (int k = 0; k < m_states.getNum(); k++) {
       var xPlusU = x.plus(U.extractColumnVector(k));

wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java

@@ -35,9 +35,9 @@ import org.ejml.simple.SimpleMatrix;
  * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
  * chapter 9 "Stochastic control theory".
  *
- * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
- * information about the SR-UKF, see <a
- * href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
+ * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). The main reason for
+ * this is to guarantee that the covariance matrix remains positive definite. For more information
+ * about the SR-UKF, see https://www.researchgate.net/publication/3908304.
  *
  * @param <States> Number of states.
  * @param <Inputs> Number of inputs.
@@ -105,7 +105,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   }
 
   /**
-   * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
+   * Constructs an Unscented Kalman filter with custom mean, residual, and addition functions. Using
    * custom functions for arithmetic can be useful if you have angles in the state or measurements,
    * because they allow you to correctly account for the modular nature of angle arithmetic.
    *
@@ -193,12 +193,21 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
           "Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
     }
 
-    // New mean is usually just the sum of the sigmas * weight:
-    //       n
-    // dot = Σ W[k] Xᵢ[k]
-    //      k=1
+    // New mean is usually just the sum of the sigmas * weights:
+    //
+    //      2n
+    //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+    //      i=0
+    //
+    // equations (19) and (23) in the paper show this,
+    // but we allow a custom function, usually for angle wrapping
     Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
 
+    // Form an intermediate matrix S⁻ as:
+    //
+    //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+    //
+    // the part of equations (20) and (24) within the "qr{}"
     Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
     for (int i = 0; i < 2 * s.getNum(); i++) {
       Sbar.setColumn(
@@ -214,8 +223,24 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
       throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
     }
 
-    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
-    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
+    // Compute the square-root covariance of the sigma points
+    //
+    // We transpose S⁻ first because we formed it by horizontally
+    // concatenating each part; it should be vertical so we can take
+    // the QR decomposition as defined in the "QR Decomposition" passage
+    // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+    //
+    // The resulting matrix R is the square-root covariance S, but it
+    // is upper triangular, so we need to transpose it.
+    //
+    // equations (20) and (24)
+    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose());
+
+    // Update or downdate the square-root covariance with (𝒳₀-x̂)
+    // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+    //
+    // equations (21) and (25)
+    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true);
 
     return new Pair<>(x, newS);
   }
@@ -256,7 +281,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    */
   @Override
   public Matrix<States, States> getP() {
-    return m_S.transpose().times(m_S);
+    return m_S.times(m_S.transpose());
   }
 
   /**
@@ -280,7 +305,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    */
   @Override
   public void setP(Matrix<States, States> newP) {
-    m_S = newP.lltDecompose(false);
+    m_S = newP.lltDecompose(true);
   }
 
   /**
@@ -347,14 +372,28 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
     var squareRootDiscQ = discQ.lltDecompose(true);
 
+    // Generate sigma points around the state mean
+    //
+    // equation (17)
     var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
 
+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
     for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
       Matrix<States, N1> x = sigmas.extractColumnVector(i);
 
       m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
     }
 
+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
     var ret =
         squareRootUnscentedTransform(
             m_states,
@@ -459,7 +498,15 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     final var discR = Discretization.discretizeR(R, m_dtSeconds);
     final var squareRootDiscR = discR.lltDecompose(true);
 
-    // Transform sigma points into measurement space
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
     Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
     var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
     for (int i = 0; i < m_pts.getNumSigmas(); i++) {
@@ -467,7 +514,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
       sigmasH.setColumn(i, hRet);
     }
 
-    // Mean and covariance of prediction passed through unscented transform
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
     var transRet =
         squareRootUnscentedTransform(
             m_states,
@@ -481,30 +532,54 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     var yHat = transRet.getFirst();
     var Sy = transRet.getSecond();
 
-    // Compute cross covariance of the state and the measurements
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
     Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
     for (int i = 0; i < m_pts.getNumSigmas(); i++) {
-      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
       var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
       var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
 
       Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
     }
 
-    // K = (P_{xy} / S_yᵀ) / S_y
-    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
-    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
     Matrix<States, R> K =
         Sy.transpose()
             .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
             .transpose();
 
-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    // Compute the posterior state mean
+    //
+    // x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
 
+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
     Matrix<States, R> U = K.times(Sy);
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
     for (int i = 0; i < rows.getNum(); i++) {
-      m_S.rankUpdate(U.extractColumnVector(i), -1, false);
+      m_S.rankUpdate(U.extractColumnVector(i), -1, true);
     }
   }
 }

wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java

@@ -21,8 +21,8 @@ public class Debouncer {
     kBoth
   }
 
-  private final double m_debounceTimeSeconds;
-  private final DebounceType m_debounceType;
+  private double m_debounceTimeSeconds;
+  private DebounceType m_debounceType;
   private boolean m_baseline;
 
   private double m_prevTimeSeconds;
@@ -86,4 +86,42 @@ public class Debouncer {
       return m_baseline;
     }
   }
+
+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public void setDebounceTime(double time) {
+    m_debounceTimeSeconds = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline for the filtered value to
+   *     change.
+   */
+  public double getDebounceTime() {
+    return m_debounceTimeSeconds;
+  }
+
+  /**
+   * Sets the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public void setDebounceType(DebounceType type) {
+    m_debounceType = type;
+  }
+
+  /**
+   * Gets the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  public DebounceType getDebounceType() {
+    return m_debounceType;
+  }
 }

wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp

@@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate(
     sleipnir::Hessian hessianF{cost, xAD};
     Eigen::SparseMatrix<double> H = hessianF.Value();
 
-    double error = std::numeric_limits<double>::infinity();
-    while (error > 1e-8) {
+    double error_k = std::numeric_limits<double>::infinity();
+    double error_k1 = std::abs(g.coeff(0));
+
+    // Loop until error stops decreasing or max iterations is reached
+    for (size_t iteration = 0;
+         iteration < 50 && error_k1 < (1.0 - 1e-10) * error_k; ++iteration) {
+      error_k = error_k1;
+
       // Iterate via Newton's method.
       //
       //   xₖ₊₁ = xₖ − H⁻¹g
@@ -97,7 +103,7 @@ units::volt_t ArmFeedforward::Calculate(
       g = gradientF.Value();
       H = hessianF.Value();
 
-      error = std::abs(g.coeff(0));
+      error_k1 = std::abs(g.coeff(0));
     }
   }
 

wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp

@@ -34,11 +34,12 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
                      slp::pow(y - rotPoint.Y().value(), 2));
 
     // (x − x_c)²/a² + (y − y_c)²/b² = 1
-    problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) /
-                              (m_xSemiAxis.value() * m_xSemiAxis.value()) +
-                          slp::pow(y - m_center.Y().value(), 2) /
-                              (m_ySemiAxis.value() * m_ySemiAxis.value()) ==
-                      1);
+    // b²(x − x_c)² + a²(y − y_c)² = a²b²
+    double a2 = m_xSemiAxis.value() * m_xSemiAxis.value();
+    double b2 = m_ySemiAxis.value() * m_ySemiAxis.value();
+    problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) +
+                          a2 * slp::pow(y - m_center.Y().value(), 2) ==
+                      a2 * b2);
 
     problem.Solve();
 

wpimath/src/main/native/include/frc/MathUtil.h

@@ -10,9 +10,15 @@
 #include <gcem.hpp>
 #include <wpi/SymbolExports.h>
 
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
 #include "units/angle.h"
 #include "units/base.h"
+#include "units/length.h"
 #include "units/math.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 
@@ -207,4 +213,65 @@ constexpr std::signed_integral auto FloorMod(std::signed_integral auto x,
                                              std::signed_integral auto y) {
   return x - FloorDiv(x, y) * y;
 }
+
+/**
+ * Limits translation velocity.
+ *
+ * @param current Translation at current timestep.
+ * @param next Translation at next timestep.
+ * @param dt Timestep duration.
+ * @param maxVelocity Maximum translation velocity.
+ * @return Returns the next Translation2d limited to maxVelocity
+ */
+constexpr Translation2d SlewRateLimit(const Translation2d& current,
+                                      const Translation2d& next,
+                                      units::second_t dt,
+                                      units::meters_per_second_t maxVelocity) {
+  if (maxVelocity < 0_mps) {
+    wpi::math::MathSharedStore::ReportError(
+        "maxVelocity must be a non-negative number, got {}!", maxVelocity);
+    return next;
+  }
+  Translation2d diff = next - current;
+  units::meter_t dist = diff.Norm();
+  if (dist < 1e-9_m) {
+    return next;
+  }
+  if (dist > maxVelocity * dt) {
+    // Move maximum allowed amount in direction of the difference
+    return current + diff * (maxVelocity * dt / dist);
+  }
+  return next;
+}
+
+/**
+ * Limits translation velocity.
+ *
+ * @param current Translation at current timestep.
+ * @param next Translation at next timestep.
+ * @param dt Timestep duration.
+ * @param maxVelocity Maximum translation velocity.
+ * @return Returns the next Translation3d limited to maxVelocity
+ */
+constexpr Translation3d SlewRateLimit(const Translation3d& current,
+                                      const Translation3d& next,
+                                      units::second_t dt,
+                                      units::meters_per_second_t maxVelocity) {
+  if (maxVelocity < 0_mps) {
+    wpi::math::MathSharedStore::ReportError(
+        "maxVelocity must be a non-negative number, got {}!", maxVelocity);
+    return next;
+  }
+  Translation3d diff = next - current;
+  units::meter_t dist = diff.Norm();
+  if (dist < 1e-9_m) {
+    return next;
+  }
+  if (dist > maxVelocity * dt) {
+    // Move maximum allowed amount in direction of the difference
+    return current + diff * (maxVelocity * dt / dist);
+  }
+  return next;
+}
+
 }  // namespace frc

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -133,13 +133,13 @@ class ElevatorFeedforward {
       units::unit_t<Velocity> currentVelocity,
       units::unit_t<Velocity> nextVelocity) const {
     // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
       return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
     } else {
       double A = -kV.value() / kA.value();
       double B = 1.0 / kA.value();
       double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
       return kG + kS * wpi::sgn(currentVelocity) +
              units::volt_t{
                  1.0 / B_d *

wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h

@@ -109,13 +109,13 @@ class SimpleMotorFeedforward {
       units::unit_t<Velocity> currentVelocity,
       units::unit_t<Velocity> nextVelocity) const {
     // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
       return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
     } else {
       double A = -kV.value() / kA.value();
       double B = 1.0 / kA.value();
       double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
       return kS * wpi::sgn(currentVelocity) +
              units::volt_t{
                  1.0 / B_d *

wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h

@@ -51,7 +51,7 @@ class MerweScaledSigmaPoints {
 
   /**
    * Computes the sigma points for an unscented Kalman filter given the mean
-   * (x) and square-root covariance(S) of the filter.
+   * (x) and square-root covariance (S) of the filter.
    *
    * @param x An array of the means.
    * @param S Square-root covariance of the filter.
@@ -68,6 +68,8 @@ class MerweScaledSigmaPoints {
     Matrixd<States, States> U = eta * S;
 
     Matrixd<States, 2 * States + 1> sigmas;
+
+    // equation (17)
     sigmas.template block<States, 1>(0, 0) = x;
     for (int k = 0; k < States; ++k) {
       sigmas.template block<States, 1>(0, k + 1) =

wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h

@@ -43,7 +43,9 @@ namespace frc {
  * "Stochastic control theory".
  *
  * <p> This class implements a square-root-form unscented Kalman filter
- * (SR-UKF). For more information about the SR-UKF, see
+ * (SR-UKF). The main reason for this is to guarantee that the covariance
+ * matrix remains positive definite.
+ * For more information about the SR-UKF, see
  * https://www.researchgate.net/publication/3908304.
  *
  * @tparam States Number of states.
@@ -108,7 +110,7 @@ class UnscentedKalmanFilter {
   }
 
   /**
-   * Constructs an unscented Kalman filter with custom mean, residual, and
+   * Constructs an Unscented Kalman filter with custom mean, residual, and
    * addition functions. Using custom functions for arithmetic can be useful if
    * you have angles in the state or measurements, because they allow you to
    * correctly account for the modular nature of angle arithmetic.
@@ -189,7 +191,7 @@ class UnscentedKalmanFilter {
   /**
    * Returns the reconstructed error covariance matrix P.
    */
-  StateMatrix P() const { return m_S.transpose() * m_S; }
+  StateMatrix P() const { return m_S * m_S.transpose(); }
 
   /**
    * Set the current square-root error covariance matrix S by taking the square
@@ -197,7 +199,7 @@ class UnscentedKalmanFilter {
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
+  void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); }
 
   /**
    * Returns the state estimate x-hat.
@@ -252,14 +254,28 @@ class UnscentedKalmanFilter {
     DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
     Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
 
+    // Generate sigma points around the state mean
+    //
+    // equation (17)
     Matrixd<States, 2 * States + 1> sigmas =
         m_pts.SquareRootSigmaPoints(m_xHat, m_S);
 
+    // Project each sigma point forward in time according to the
+    // dynamics f(x, u)
+    //
+    //   sigmas  = 𝒳ₖ₋₁
+    //   sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
+    //
+    // equation (18)
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
       StateVector x = sigmas.template block<States, 1>(0, i);
       m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
     }
 
+    // Pass the predicted sigmas (𝒳) through the Unscented Transform
+    // to compute the prior state mean and covariance
+    //
+    // equations (18) (19) and (20)
     auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
         m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
         discQ.template triangularView<Eigen::Lower>());
@@ -367,7 +383,15 @@ class UnscentedKalmanFilter {
     Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
     Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
 
-    // Transform sigma points into measurement space
+    // Generate new sigma points from the prior mean and covariance
+    // and transform them into measurement space using h(x, u)
+    //
+    //   sigmas  = 𝒳
+    //   sigmasH = 𝒴
+    //
+    // This differs from equation (22) which uses
+    // the prior sigma points, regenerating them allows
+    // multiple measurement updates per time update
     Matrixd<Rows, 2 * States + 1> sigmasH;
     Matrixd<States, 2 * States + 1> sigmas =
         m_pts.SquareRootSigmaPoints(m_xHat, m_S);
@@ -376,16 +400,26 @@ class UnscentedKalmanFilter {
           h(sigmas.template block<States, 1>(0, i), u);
     }
 
-    // Mean and covariance of prediction passed through UT
+    // Pass the predicted measurement sigmas through the Unscented Transform
+    // to compute the mean predicted measurement and square-root innovation
+    // covariance.
+    //
+    // equations (23) (24) and (25)
     auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
         sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
         discR.template triangularView<Eigen::Lower>());
 
-    // Compute cross covariance of the state and the measurements
+    // Compute cross covariance of the predicted state and measurement sigma
+    // points given as:
+    //
+    //           2n
+    //   P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
+    //           i=0
+    //
+    // equation (26)
     Matrixd<States, Rows> Pxy;
     Pxy.setZero();
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
       Pxy +=
           m_pts.Wc(i) *
           (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -393,21 +427,39 @@ class UnscentedKalmanFilter {
               .transpose();
     }
 
-    // K = (P_{xy} / S_yᵀ) / S_y
-    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
-    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
+    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
+    // that.
+    //
+    //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
+    //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
+    //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
+    //
+    // equation (27)
     Matrixd<States, Rows> K =
         Sy.transpose()
             .fullPivHouseholderQr()
             .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
             .transpose();
 
-    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    // Compute the posterior state mean
+    //
+    //   x̂ = x̂⁻ + K(y − ŷ⁻)
+    //
+    // second part of equation (27)
     m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
 
+    // Compute the intermediate matrix U for downdating
+    // the square-root covariance
+    //
+    // equation (28)
     Matrixd<States, Rows> U = K * Sy;
+
+    // Downdate the posterior square-root state covariance
+    //
+    // equation (29)
     for (int i = 0; i < Rows; i++) {
-      Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+      Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
           m_S, U.template block<States, 1>(0, i), -1);
     }
   }

wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h

@@ -47,12 +47,21 @@ SquareRootUnscentedTransform(
                                   const Vectord<CovDim>&)>
         residualFunc,
     const Matrixd<CovDim, CovDim>& squareRootR) {
-  // New mean is usually just the sum of the sigmas * weight:
-  //       n
-  // dot = Σ W[k] Xᵢ[k]
-  //      k=1
+  // New mean is usually just the sum of the sigmas * weights:
+  //
+  //      2n
+  //   x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
+  //      i=0
+  //
+  // equations (19) and (23) in the paper show this,
+  // but we allow a custom function, usually for angle wrapping
   Vectord<CovDim> x = meanFunc(sigmas, Wm);
 
+  // Form an intermediate matrix S⁻ as:
+  //
+  //   [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
+  //
+  // the part of equations (20) and (24) within the "qr{}"
   Matrixd<CovDim, States * 2 + CovDim> Sbar;
   for (int i = 0; i < States * 2; i++) {
     Sbar.template block<CovDim, 1>(0, i) =
@@ -61,14 +70,29 @@ SquareRootUnscentedTransform(
   }
   Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
 
-  // Merwe defines the QR decomposition as Aᵀ = QR
+  // Compute the square-root covariance of the sigma points.
+  //
+  // We transpose S⁻ first because we formed it by horizontally
+  // concatenating each part; it should be vertical so we can take
+  // the QR decomposition as defined in the "QR Decomposition" passage
+  // of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
+  //
+  // The resulting matrix R is the square-root covariance S, but it
+  // is upper triangular, so we need to transpose it.
+  //
+  // equations (20) and (24)
   Matrixd<CovDim, CovDim> S = Sbar.transpose()
                                   .householderQr()
                                   .matrixQR()
                                   .template block<CovDim, CovDim>(0, 0)
-                                  .template triangularView<Eigen::Upper>();
+                                  .template triangularView<Eigen::Upper>()
+                                  .transpose();
 
-  Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+  // Update or downdate the square-root covariance with (𝒳₀-x̂)
+  // depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
+  //
+  // equations (21) and (25)
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
       S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
 
   return std::make_tuple(x, S);

wpimath/src/main/native/include/frc/filter/Debouncer.h

@@ -48,6 +48,38 @@ class WPILIB_DLLEXPORT Debouncer {
    */
   bool Calculate(bool input);
 
+  /**
+   * Sets the time to debounce.
+   *
+   * @param time The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr void SetDebounceTime(units::second_t time) {
+    m_debounceTime = time;
+  }
+
+  /**
+   * Gets the time to debounce.
+   *
+   * @return The number of seconds the value must change from baseline
+   *             for the filtered value to change.
+   */
+  constexpr units::second_t GetDebounceTime() const { return m_debounceTime; }
+
+  /**
+   * Set the debounce type.
+   *
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  constexpr void SetDebounceType(DebounceType type) { m_debounceType = type; }
+
+  /**
+   * Get the debounce type.
+   *
+   * @return Which type of state change the debouncing will be performed on.
+   */
+  constexpr DebounceType GetDebounceType() const { return m_debounceType; }
+
  private:
   units::second_t m_debounceTime;
   bool m_baseline;

wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java

@@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
@@ -167,4 +169,55 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
     assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
   }
+
+  @Test
+  void testSlewRateTranslation2dUnchanged() {
+    Translation2d translation1 = new Translation2d(0, 0);
+    Translation2d translation2 = new Translation2d(2, 2);
+
+    Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation2d expected1 = new Translation2d(2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation2dChanged() {
+    Translation2d translation3 = new Translation2d(1, 1);
+    Translation2d translation4 = new Translation2d(3, 3);
+
+    Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation2d expected2 =
+        new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0));
+
+    assertEquals(expected2, result2);
+  }
+
+  @Test
+  void testSlewRateTranslation3dUnchanged() {
+    Translation3d translation1 = new Translation3d(0, 0, 0);
+    Translation3d translation2 = new Translation3d(2, 2, 2);
+
+    Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation3d expected1 = new Translation3d(2, 2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation3dChanged() {
+    Translation3d translation3 = new Translation3d(1, 1, 1);
+    Translation3d translation4 = new Translation3d(3, 3, 3);
+
+    Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation3d expected2 =
+        new Translation3d(
+            1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0));
+
+    assertEquals(expected2, result2);
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java

@@ -18,15 +18,13 @@ import java.util.function.BiFunction;
 import org.junit.jupiter.api.Test;
 
 class ArmFeedforwardTest {
-  private static final double ks = 0.5;
-  private static final double kg = 1;
-  private static final double kv = 1.5;
-  private static final double ka = 2;
-  private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
-
   /**
    * Simulates a single-jointed arm and returns the final state.
    *
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
    * @param currentAngle The starting angle in radians.
    * @param currentVelocity The starting angular velocity in radians per second.
    * @param input The input voltage.
@@ -34,7 +32,14 @@ class ArmFeedforwardTest {
    * @return The final state as a 2-vector of angle and angular velocity.
    */
   private Matrix<N2, N1> simulate(
-      double currentAngle, double currentVelocity, double input, double dt) {
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double input,
+      double dt) {
     final Matrix<N2, N2> A =
         new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
     final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
@@ -61,47 +66,115 @@ class ArmFeedforwardTest {
    * Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
    * then simulates an arm using that voltage to verify correctness.
    *
+   * @param armFF The feedforward object.
+   * @param ks The static gain, in volts.
+   * @param kv The velocity gain, in volt seconds per radian.
+   * @param ka The acceleration gain, in volt seconds² per radian.
+   * @param kg The gravity gain, in volts.
    * @param currentAngle The starting angle in radians.
    * @param currentVelocity The starting angular velocity in radians per second.
    * @param input The input voltage.
    * @param dt The simulation time in seconds.
    */
   private void calculateAndSimulate(
-      double currentAngle, double currentVelocity, double nextVelocity, double dt) {
-    final double input =
-        m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
-    assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
+      ArmFeedforward armFF,
+      double ks,
+      double kv,
+      double ka,
+      double kg,
+      double currentAngle,
+      double currentVelocity,
+      double nextVelocity,
+      double dt) {
+    final double input = armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
+    assertEquals(
+        nextVelocity,
+        simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0),
+        1e-4);
   }
 
   @Test
   void testCalculate() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
     // calculate(angle, angular velocity)
-    assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
-    assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
+    assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002);
+    assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002);
 
     // calculate(currentAngle, currentVelocity, nextAngle, dt)
-    calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
-    calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020);
-    calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020);
-    calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020);
+    calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020);
+  }
+
+  @Test
+  void testCalculateIllConditionedModel() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 1e-2;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    final double currentAngle = 1.0;
+    final double currentVelocity = 0.02;
+    final double nextVelocity = 0.0;
+
+    var averageAccel = (nextVelocity - currentVelocity) / 0.02;
+
+    assertEquals(
+        armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity),
+        ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle));
+  }
+
+  @Test
+  void testCalculateIllConditionedGradient() {
+    final double ks = 0.39671;
+    final double kv = 2.7167;
+    final double ka = 0.50799;
+    final double kg = 0.2708;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02);
   }
 
   @Test
   void testAchievableVelocity() {
-    assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
-    assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
   }
 
   @Test
   void testAchievableAcceleration() {
-    assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
-    assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
-    assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
-    assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+    final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
+
+    assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
+    assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
+    assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
   }
 
   @Test
   void testNegativeGains() {
+    final double ks = 0.5;
+    final double kv = 1.5;
+    final double ka = 2;
+    final double kg = 1;
+
     assertAll(
         () ->
             assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),

wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java

@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.StateSpaceUtil;
@@ -18,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
@@ -26,11 +28,13 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import java.util.Collections;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class UnscentedKalmanFilterTest {
-  private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     var motors = DCMotor.getCIM(2);
 
     // var gLow = 15.32;    // Low gear ratio
@@ -63,17 +67,17 @@ class UnscentedKalmanFilterTest {
   }
 
   @SuppressWarnings("PMD.UnusedFormalParameter")
-  private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
   }
 
   @SuppressWarnings("PMD.UnusedFormalParameter")
-  private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     return x.copy();
   }
 
   @Test
-  void testInit() {
+  void testDriveInit() {
     var dtSeconds = 0.005;
     assertDoesNotThrow(
         () -> {
@@ -81,8 +85,8 @@ class UnscentedKalmanFilterTest {
               new UnscentedKalmanFilter<>(
                   Nat.N5(),
                   Nat.N3(),
-                  UnscentedKalmanFilterTest::getDynamics,
-                  UnscentedKalmanFilterTest::getLocalMeasurementModel,
+                  UnscentedKalmanFilterTest::driveDynamics,
+                  UnscentedKalmanFilterTest::driveLocalMeasurementModel,
                   VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
                   VecBuilder.fill(0.0001, 0.01, 0.01),
                   AngleStatistics.angleMean(2),
@@ -95,10 +99,10 @@ class UnscentedKalmanFilterTest {
           var u = VecBuilder.fill(12.0, 12.0);
           observer.predict(u, dtSeconds);
 
-          var localY = getLocalMeasurementModel(observer.getXhat(), u);
+          var localY = driveLocalMeasurementModel(observer.getXhat(), u);
           observer.correct(u, localY);
 
-          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
           var R =
               StateSpaceUtil.makeCovarianceMatrix(
                   Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
@@ -106,7 +110,7 @@ class UnscentedKalmanFilterTest {
               Nat.N5(),
               u,
               globalY,
-              UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+              UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
               R,
               AngleStatistics.angleMean(2),
               AngleStatistics.angleResidual(2),
@@ -116,16 +120,16 @@ class UnscentedKalmanFilterTest {
   }
 
   @Test
-  void testConvergence() {
-    double dtSeconds = 0.005;
-    double rbMeters = 0.8382 / 2.0; // Robot radius
+  void testDriveConvergence() {
+    final double dtSeconds = 0.005;
+    final double rbMeters = 0.8382 / 2.0; // Robot radius
 
     UnscentedKalmanFilter<N5, N2, N3> observer =
         new UnscentedKalmanFilter<>(
             Nat.N5(),
             Nat.N3(),
-            UnscentedKalmanFilterTest::getDynamics,
-            UnscentedKalmanFilterTest::getLocalMeasurementModel,
+            UnscentedKalmanFilterTest::driveDynamics,
+            UnscentedKalmanFilterTest::driveLocalMeasurementModel,
             VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
             VecBuilder.fill(0.0001, 0.5, 0.5),
             AngleStatistics.angleMean(2),
@@ -149,7 +153,7 @@ class UnscentedKalmanFilterTest {
         NumericalJacobian.numericalJacobianU(
             Nat.N5(),
             Nat.N2(),
-            UnscentedKalmanFilterTest::getDynamics,
+            UnscentedKalmanFilterTest::driveDynamics,
             new Matrix<>(Nat.N5(), Nat.N1()),
             new Matrix<>(Nat.N2(), Nat.N1()));
 
@@ -164,7 +168,7 @@ class UnscentedKalmanFilterTest {
     var trueXhat = observer.getXhat();
 
     double totalTime = trajectory.getTotalTimeSeconds();
-    for (int i = 0; i < (totalTime / dtSeconds); i++) {
+    for (int i = 0; i < (totalTime / dtSeconds); ++i) {
       var ref = trajectory.sample(dtSeconds * i);
       double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
       double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
@@ -177,25 +181,27 @@ class UnscentedKalmanFilterTest {
               vl,
               vr);
 
-      Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      Matrix<N3, N1> localY =
+          driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
       var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
 
       observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
 
       var rdot = nextR.minus(r).div(dtSeconds);
-      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+      u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
 
       observer.predict(u, dtSeconds);
 
       r = nextR;
       trueXhat =
-          NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
+          NumericalIntegration.rk4(
+              UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dtSeconds);
     }
 
-    var localY = getLocalMeasurementModel(trueXhat, u);
+    var localY = driveLocalMeasurementModel(trueXhat, u);
     observer.correct(u, localY);
 
-    var globalY = getGlobalMeasurementModel(trueXhat, u);
+    var globalY = driveGlobalMeasurementModel(trueXhat, u);
     var R =
         StateSpaceUtil.makeCovarianceMatrix(
             Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
@@ -203,7 +209,7 @@ class UnscentedKalmanFilterTest {
         Nat.N5(),
         u,
         globalY,
-        UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+        UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
         R,
         AngleStatistics.angleMean(2),
         AngleStatistics.angleResidual(2),
@@ -241,7 +247,7 @@ class UnscentedKalmanFilterTest {
     Matrix<N1, N1> ref = VecBuilder.fill(100);
     Matrix<N1, N1> u = VecBuilder.fill(0);
 
-    for (int i = 0; i < (2.0 / dt); i++) {
+    for (int i = 0; i < (2.0 / dt); ++i) {
       observer.predict(u, dt);
 
       u = discB.solve(ref.minus(discA.times(ref)));
@@ -269,4 +275,125 @@ class UnscentedKalmanFilterTest {
 
     assertTrue(observer.getP().isEqual(P, 1e-9));
   }
+
+  // Second system, single motor feedforward estimator
+  private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
+  }
+
+  private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
+    double p = x.get(0, 0);
+    double v = x.get(1, 0);
+    double kV = x.get(2, 0);
+    double kA = x.get(3, 0);
+    double V = u.get(0, 0);
+
+    double a = -kV / kA * v + 1.0 / kA * V;
+    return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
+  }
+
+  private static Matrix<N1, N1> motorControlInput(double t) {
+    return MatBuilder.fill(
+        Nat.N1(),
+        Nat.N1(),
+        MathUtil.clamp(
+            8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+                + 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+                + 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
+            -12.0,
+            12.0));
+  }
+
+  @Test
+  void testMotorConvergence() {
+    final double dtSeconds = 0.01;
+    final int steps = 500;
+    final double true_kV = 3;
+    final double true_kA = 0.2;
+
+    final double pos_stddev = 0.02;
+    final double vel_stddev = 0.1;
+    final double accel_stddev = 0.1;
+
+    var states =
+        new ArrayList<>(
+            Collections.nCopies(
+                steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
+    var inputs =
+        new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
+    var measurements =
+        new ArrayList<>(
+            Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
+    states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
+
+    var A =
+        MatBuilder.fill(
+            Nat.N4(),
+            Nat.N4(),
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            -true_kV / true_kA,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0);
+    var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
+
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    for (int i = 0; i < steps; ++i) {
+      inputs.set(i, motorControlInput(i * dtSeconds));
+      states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
+      measurements.set(
+          i,
+          motorMeasurementModel(states.get(i + 1), inputs.get(i))
+              .plus(
+                  StateSpaceUtil.makeWhiteNoiseVector(
+                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+    }
+
+    var P0 =
+        MatBuilder.fill(
+            Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
+            0.0, 0.0, 10);
+
+    var observer =
+        new UnscentedKalmanFilter<N4, N1, N3>(
+            Nat.N4(),
+            Nat.N3(),
+            UnscentedKalmanFilterTest::motorDynamics,
+            UnscentedKalmanFilterTest::motorMeasurementModel,
+            VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
+            VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
+            dtSeconds);
+
+    observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
+    observer.setP(P0);
+
+    for (int i = 0; i < steps; ++i) {
+      observer.predict(inputs.get(i), dtSeconds);
+      observer.correct(inputs.get(i), measurements.get(i));
+    }
+
+    assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
+    assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java

@@ -4,7 +4,9 @@
 
 package edu.wpi.first.math.filter;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertSame;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.util.WPIUtilJNI;
@@ -67,4 +69,20 @@ class DebouncerTest {
 
     assertFalse(debouncer.calculate(false));
   }
+
+  @Test
+  void debounceParamsTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    assertEquals(debouncer.getDebounceTime(), 0.02);
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kBoth);
+
+    debouncer.setDebounceTime(0.1);
+
+    assertEquals(debouncer.getDebounceTime(), 0.1);
+
+    debouncer.setDebounceType(Debouncer.DebounceType.kFalling);
+
+    assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling);
+  }
 }

wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -3,11 +3,17 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 
 #include <gtest/gtest.h>
 
 #include "frc/MathUtil.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
 #include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
 
 #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
 
@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
   EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
   EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
 }
+
+TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
+  const frc::Translation2d translation1{0_m, 0_m};
+  const frc::Translation2d translation2{2_m, 2_m};
+
+  const frc::Translation2d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
+
+  const frc::Translation2d expected1{2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
+  const frc::Translation2d translation3{1_m, 1_m};
+  const frc::Translation2d translation4{3_m, 3_m};
+
+  const frc::Translation2d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
+
+  const frc::Translation2d expected2{
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
+
+  EXPECT_EQ(result2, expected2);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
+  const frc::Translation3d translation1{0_m, 0_m, 0_m};
+  const frc::Translation3d translation2{2_m, 2_m, 2_m};
+
+  const frc::Translation3d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
+
+  const frc::Translation3d expected1{2_m, 2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
+  const frc::Translation3d translation3{1_m, 1_m, 1_m};
+  const frc::Translation3d translation4{3_m, 3_m, 3_m};
+
+  const frc::Translation3d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
+
+  const frc::Translation3d expected2{
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
+
+  EXPECT_EQ(result2, expected2);
+}

wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp

@@ -15,27 +15,32 @@
 #include "units/time.h"
 #include "units/voltage.h"
 
-static constexpr auto Ks = 0.5_V;
-static constexpr auto Kv = 1.5_V / 1_rad_per_s;
-static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
-static constexpr auto Kg = 1_V;
-
 namespace {
 
+using Ks_unit = decltype(1_V);
+using Kv_unit = decltype(1_V / 1_rad_per_s);
+using Ka_unit = decltype(1_V / 1_rad_per_s_sq);
+using Kg_unit = decltype(1_V);
+
 /**
  * Simulates a single-jointed arm and returns the final state.
  *
+ * @param Ks The static gain, in volts.
+ * @param Kv The velocity gain, in volt seconds per radian.
+ * @param Ka The acceleration gain, in volt seconds² per radian.
+ * @param Kg The gravity gain, in volts.
  * @param currentAngle The starting angle.
  * @param currentVelocity The starting angular velocity.
  * @param input The input voltage.
  * @param dt The simulation time.
  * @return The final state as a 2-vector of angle and angular velocity.
  */
-frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
+frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
+                            units::radian_t currentAngle,
                             units::radians_per_second_t currentVelocity,
                             units::volt_t input, units::second_t dt) {
-  constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
-  constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
+  frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
+  frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
 
   return frc::RK4(
       [&](const frc::Matrixd<2, 1>& x,
@@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
  * Simulates a single-jointed arm and returns the final state.
  *
  * @param armFF The feedforward object.
+ * @param Ks The static gain, in volts.
+ * @param Kv The velocity gain, in volt seconds per radian.
+ * @param Ka The acceleration gain, in volt seconds² per radian.
+ * @param Kg The gravity gain, in volts.
  * @param currentAngle The starting angle.
  * @param currentVelocity The starting angular velocity.
  * @param input The input voltage.
  * @param dt The simulation time.
  */
-void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
+void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
+                          Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
                           units::radian_t currentAngle,
                           units::radians_per_second_t currentVelocity,
                           units::radians_per_second_t nextVelocity,
                           units::second_t dt) {
   auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
-  EXPECT_NEAR(nextVelocity.value(),
-              Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
+  EXPECT_NEAR(
+      nextVelocity.value(),
+      Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1),
+      1e-4);
 }
 
 }  // namespace
 
 TEST(ArmFeedforwardTest, Calculate) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
   frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
 
   // Calculate(angle, angular velocity)
@@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) {
       0.002);
 
   // Calculate(currentAngle, currentVelocity, nextAngle, dt)
-  CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       1.05_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       0.95_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       1.05_rad_per_s, 20_ms);
-  CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
-                       0.95_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 1.05_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 0.95_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 1.05_rad_per_s, 20_ms);
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
+                       1_rad_per_s, 0.95_rad_per_s, 20_ms);
+}
+
+TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
+  constexpr auto Ks = 0.39671_V;
+  constexpr auto Kv = 2.7167_V / 1_rad_per_s;
+  constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 0.2708_V;
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
+  constexpr auto currentAngle = 1_rad;
+  constexpr auto currentVelocity = 0.02_rad_per_s;
+  constexpr auto nextVelocity = 0_rad_per_s;
+
+  constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms;
+
+  EXPECT_DOUBLE_EQ(
+      armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
+      (Ks + Kv * currentVelocity + Ka * averageAccel +
+       Kg * units::math::cos(currentAngle))
+          .value());
+}
+
+TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
+  constexpr auto Ks = 0.39671_V;
+  constexpr auto Kv = 2.7167_V / 1_rad_per_s;
+  constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 0.2708_V;
+  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
+  CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
+                       0_rad_per_s, 20_ms);
 }
 
 TEST(ArmFeedforwardTest, AchievableVelocity) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
   frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
   EXPECT_NEAR(armFF
                   .MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
                                          1_rad_per_s_sq)
@@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
 }
 
 TEST(ArmFeedforwardTest, AchievableAcceleration) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
   frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
+
   EXPECT_NEAR(armFF
                   .MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
                                              1_rad_per_s)
@@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
 }
 
 TEST(ArmFeedforwardTest, NegativeGains) {
+  constexpr auto Ks = 0.5_V;
+  constexpr auto Kv = 1.5_V / 1_rad_per_s;
+  constexpr auto Ka = 2_V / 1_rad_per_s_sq;
+  constexpr auto Kg = 1_V;
   frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
+
   EXPECT_EQ(armFF.GetKv().value(), 0);
   EXPECT_EQ(armFF.GetKa().value(), 0);
 }

wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp

@@ -2,7 +2,9 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
+#include <algorithm>
 #include <cmath>
+#include <numbers>
 #include <vector>
 
 #include <Eigen/QR>
@@ -12,15 +14,19 @@
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/Discretization.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
 #include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "units/moment_of_inertia.h"
 
 namespace {
 
-frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
+// First test system, differential drive
+frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
+                              const frc::Vectord<2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;    // Low gear ratio
@@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
           k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-frc::Vectord<3> LocalMeasurementModel(
+frc::Vectord<3> DriveLocalMeasurementModel(
     const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<3>{x(2), x(3), x(4)};
 }
 
-frc::Vectord<5> GlobalMeasurementModel(
+frc::Vectord<5> DriveGlobalMeasurementModel(
     const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
   return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
 }
-}  // namespace
 
-TEST(UnscentedKalmanFilterTest, Init) {
+TEST(UnscentedKalmanFilterTest, DriveInit) {
   constexpr auto dt = 5_ms;
 
-  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                               LocalMeasurementModel,
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
+                                               DriveLocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.01, 0.01},
                                                frc::AngleMean<5, 5>(2),
@@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) {
   frc::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
-  auto localY = LocalMeasurementModel(observer.Xhat(), u);
+  auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
   observer.Correct(u, localY);
 
-  auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
+  auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                       frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
                       frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
 }
 
-TEST(UnscentedKalmanFilterTest, Convergence) {
+TEST(UnscentedKalmanFilterTest, DriveConvergence) {
   constexpr auto dt = 5_ms;
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
-  frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
-                                               LocalMeasurementModel,
+  frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
+                                               DriveLocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.5, 0.5},
                                                frc::AngleMean<5, 5>(2),
@@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
   frc::Vectord<5> r = frc::Vectord<5>::Zero();
   frc::Vectord<2> u = frc::Vectord<2>::Zero();
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
-                                            frc::Vectord<2>::Zero());
+  auto B = frc::NumericalJacobianU<5, 5, 2>(
+      DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
 
   observer.SetXhat(frc::Vectord<5>{
       trajectory.InitialPose().Translation().X().value(),
@@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
         ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
-    auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
+    auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
     frc::Vectord<5> rdot = (nextR - r) / dt.value();
-    u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
+    u = B.householderQr().solve(rdot -
+                                DriveDynamics(r, frc::Vectord<2>::Zero()));
 
     observer.Predict(u, dt);
 
     r = nextR;
-    trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
+    trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
   }
 
-  auto localY = LocalMeasurementModel(trueXhat, u);
+  auto localY = DriveLocalMeasurementModel(trueXhat, u);
   observer.Correct(u, localY);
 
-  auto globalY = GlobalMeasurementModel(trueXhat, u);
+  auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+  observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                       frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
                       frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
 
@@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
   EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
 }
 
+TEST(UnscentedKalmanFilterTest, LinearUKF) {
+  constexpr units::second_t dt = 20_ms;
+  auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
+      0.02_V / 1_mps, 0.006_V / 1_mps_sq);
+  frc::UnscentedKalmanFilter<1, 1, 1> observer{
+      [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return plant.A() * x + plant.B() * u;
+      },
+      [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return plant.CalculateY(x, u);
+      },
+      {0.05},
+      {1.0},
+      dt};
+
+  frc::Matrixd<1, 1> discA;
+  frc::Matrixd<1, 1> discB;
+  frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
+
+  frc::Vectord<1> ref{100.0};
+  frc::Vectord<1> u{0.0};
+
+  for (int i = 0; i < 2.0 / dt.value(); ++i) {
+    observer.Predict(u, dt);
+
+    u = discB.householderQr().solve(ref - discA * ref);
+  }
+
+  EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
+}
+
 TEST(UnscentedKalmanFilterTest, RoundTripP) {
   constexpr auto dt = 5_ms;
 
@@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) {
 
   ASSERT_TRUE(observer.P().isApprox(P));
 }
+
+// Second system, single motor feedforward estimator
+frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
+                              const frc::Vectord<1>& u) {
+  double v = x(1);
+  double kV = x(2);
+  double kA = x(3);
+
+  double V = u(0);
+
+  double a = -kV / kA * v + 1.0 / kA * V;
+  return frc::Vectord<4>{v, a, 0.0, 0.0};
+}
+
+frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
+                                      const frc::Vectord<1>& u) {
+  double p = x(0);
+  double v = x(1);
+  double kV = x(2);
+  double kA = x(3);
+
+  double V = u(0);
+
+  double a = -kV / kA * v + 1.0 / kA * V;
+  return frc::Vectord<3>{p, v, a};
+}
+
+frc::Vectord<1> MotorControlInput(double t) {
+  return frc::Vectord<1>{
+      std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
+                     6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
+                     4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
+                 -12.0, 12.0)};
+}
+
+TEST(UnscentedKalmanFilterTest, MotorConvergence) {
+  constexpr units::second_t dt = 10_ms;
+  constexpr int steps = 500;
+  constexpr double true_kV = 3;
+  constexpr double true_kA = 0.2;
+
+  constexpr double pos_stddev = 0.02;
+  constexpr double vel_stddev = 0.1;
+  constexpr double accel_stddev = 0.1;
+
+  std::vector<frc::Vectord<4>> states(steps + 1);
+  std::vector<frc::Vectord<1>> inputs(steps);
+  std::vector<frc::Vectord<3>> measurements(steps);
+  states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
+
+  constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
+                                 {0.0, -true_kV / true_kA, 0.0, 0.0},
+                                 {0.0, 0.0, 0.0, 0.0},
+                                 {0.0, 0.0, 0.0, 0.0}};
+  constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
+
+  frc::Matrixd<4, 4> discA;
+  frc::Matrixd<4, 1> discB;
+  frc::DiscretizeAB(A, B, dt, &discA, &discB);
+
+  for (int i = 0; i < steps; ++i) {
+    inputs[i] = MotorControlInput(i * dt.value());
+    states[i + 1] = discA * states[i] + discB * inputs[i];
+    measurements[i] =
+        MotorMeasurementModel(states[i + 1], inputs[i]) +
+        frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
+  }
+
+  frc::Vectord<4> P0{0.001, 0.001, 10, 10};
+
+  frc::UnscentedKalmanFilter<4, 1, 3> observer{
+      MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
+      wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
+
+  observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
+  observer.SetP(P0.asDiagonal());
+
+  for (int i = 0; i < steps; ++i) {
+    observer.Predict(inputs[i], dt);
+    observer.Correct(inputs[i], measurements[i]);
+  }
+
+  EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
+  EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
+}
+
+}  // namespace

wpimath/src/test/native/cpp/filter/DebouncerTest.cpp

@@ -56,3 +56,20 @@ TEST_F(DebouncerTest, DebounceBoth) {
 
   EXPECT_FALSE(debouncer.Calculate(false));
 }
+
+TEST_F(DebouncerTest, DebounceParams) {
+  frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
+
+  EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
+  EXPECT_TRUE(debouncer.GetDebounceType() ==
+              frc::Debouncer::DebounceType::kBoth);
+
+  debouncer.SetDebounceTime(100_ms);
+
+  EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms);
+
+  debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling);
+
+  EXPECT_TRUE(debouncer.GetDebounceType() ==
+              frc::Debouncer::DebounceType::kFalling);
+}

wpiutil/src/main/native/unix/StackTrace.cpp

@@ -4,7 +4,9 @@
 
 #include "wpi/StackTrace.h"
 
+#ifndef __EMSCRIPTEN__
 #include <execinfo.h>
+#endif
 
 #include <string>
 
@@ -16,7 +18,7 @@
 namespace wpi {
 
 std::string GetStackTraceDefault(int offset) {
-#ifndef __ANDROID__
+#if !defined(__ANDROID__) && !defined(__EMSCRIPTEN__)
   void* stackTrace[128];
   int stackSize = backtrace(stackTrace, 128);
   char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
@@ -40,7 +42,7 @@ std::string GetStackTraceDefault(int offset) {
 
   return std::string{trace.str()};
 #else
-  // backtrace_symbols not supported on android
+  // backtrace_symbols not supported
   return "";
 #endif
 }