Merge branch 'main' into 2027

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() {
@@ -44,50 +47,51 @@ public final class MathUtil {
    * @return The value after the deadband is applied.
    */
   public static double applyDeadband(double value, double deadband, double maxMagnitude) {
-    if (Math.abs(value) > deadband) {
-      if (maxMagnitude / deadband > 1.0e12) {
-        // If max magnitude is sufficiently large, the implementation encounters
-        // roundoff error.  Implementing the limiting behavior directly avoids
-        // the problem.
-        return value > 0.0 ? value - deadband : value + deadband;
-      }
-      if (value > 0.0) {
-        // Map deadband to 0 and map max to max.
-        //
-        // y - y₁ = m(x - x₁)
-        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-        //
-        // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
-        // x₁ = deadband
-        // y₁ = 0
-        // x₂ = max
-        // y₂ = max
-        //
-        // y = (max - 0)/(max - deadband) (x - deadband) + 0
-        // y = max/(max - deadband) (x - deadband)
-        // y = max (x - deadband)/(max - deadband)
-        return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
-      } else {
-        // Map -deadband to 0 and map -max to -max.
-        //
-        // y - y₁ = m(x - x₁)
-        // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-        // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-        //
-        // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
-        // x₁ = -deadband
-        // y₁ = 0
-        // x₂ = -max
-        // y₂ = -max
-        //
-        // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
-        // y = max/(max - deadband) (x + deadband)
-        // y = max (x + deadband)/(max - deadband)
-        return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
-      }
+    if (Math.abs(value) < deadband) {
+      return 0;
+    }
+    if (value > 0.0) {
+      // Map deadband to 0 and map max to max with a linear relationship.
+      //
+      //   y - y₁ = m(x - x₁)
+      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+      //
+      //   x₁ = deadband
+      //   y₁ = 0
+      //   x₂ = max
+      //   y₂ = max
+      //   y = (max - 0)/(max - deadband) (x - deadband) + 0
+      //   y = max/(max - deadband) (x - deadband)
+      //
+      // To handle high values of max, rewrite so that max only appears on the denominator.
+      //
+      //   y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
+      //   y = (1 + deadband/(max - deadband)) (x - deadband)
+      return (1 + deadband / (maxMagnitude - deadband)) * (value - deadband);
     } else {
-      return 0.0;
+      // Map -deadband to 0 and map -max to -max with a linear relationship.
+      //
+      //   y - y₁ = m(x - x₁)
+      //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+      //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+      //
+      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+      //
+      //   x₁ = -deadband
+      //   y₁ = 0
+      //   x₂ = -max
+      //   y₂ = -max
+      //   y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+      //   y = max/(max - deadband) (x + deadband)
+      //
+      // To handle high values of max, rewrite so that max only appears on the denominator.
+      //
+      //   y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
+      //   y = (1 + deadband/(max - deadband)) (x + deadband)
+      return (1 + deadband / (maxMagnitude - deadband)) * (value + deadband);
     }
   }
 
@@ -209,4 +213,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

@@ -188,13 +188,13 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
    */
   public double calculate(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

@@ -171,13 +171,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
    */
   public double calculate(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, dt).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, dt));
     }
 
+    // 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_dt);
     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_debounceTime;
-  private final DebounceType m_debounceType;
+  private double m_debounceTime;
+  private DebounceType m_debounceType;
   private boolean m_baseline;
 
   private double m_prevTime;
@@ -40,11 +40,7 @@ public class Debouncer {
 
     resetTimer();
 
-    m_baseline =
-        switch (m_debounceType) {
-          case kBoth, kRising -> false;
-          case kFalling -> true;
-        };
+    m_baseline = m_debounceType == DebounceType.kFalling;
   }
 
   /**
@@ -86,4 +82,44 @@ 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_debounceTime = 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_debounceTime;
+  }
+
+  /**
+   * 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;
+
+    m_baseline = m_debounceType == DebounceType.kFalling;
+  }
+
+  /**
+   * 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/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -42,7 +42,7 @@ public class TrapezoidProfile {
   private int m_direction;
 
   private final Constraints m_constraints;
-  private State m_current;
+  private State m_current = new State();
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -186,7 +186,8 @@ public class TrapezoidProfile {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or zero if no goal was
+   *     set.
    */
   public double timeLeftUntil(double target) {
     double position = m_current.position * m_direction;
@@ -252,7 +253,7 @@ public class TrapezoidProfile {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no goal was set.
    */
   public double totalTime() {
     return m_endDecel;

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/filter/Debouncer.cpp

@@ -10,15 +10,7 @@ using namespace frc;
 
 Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
     : m_debounceTime(debounceTime), m_debounceType(type) {
-  switch (type) {
-    case DebounceType::kBoth:  // fall-through
-    case DebounceType::kRising:
-      m_baseline = false;
-      break;
-    case DebounceType::kFalling:
-      m_baseline = true;
-      break;
-  }
+  m_baseline = m_debounceType == DebounceType::kFalling;
   ResetTimer();
 }
 

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 {
 
@@ -37,51 +43,55 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
     magnitude = units::math::abs(value);
   }
 
-  if (magnitude > deadband) {
-    if (maxMagnitude / deadband > 1.0E12) {
-      // If max magnitude is sufficiently large, the implementation encounters
-      // roundoff error.  Implementing the limiting behavior directly avoids
-      // the problem.
-      return value > T{0.0} ? value - deadband : value + deadband;
-    }
-    if (value > T{0.0}) {
-      // Map deadband to 0 and map max to max.
-      //
-      // y - y₁ = m(x - x₁)
-      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-      //
-      // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
-      // x₁ = deadband
-      // y₁ = 0
-      // x₂ = max
-      // y₂ = max
-      //
-      // y = (max - 0)/(max - deadband) (x - deadband) + 0
-      // y = max/(max - deadband) (x - deadband)
-      // y = max (x - deadband)/(max - deadband)
-      return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
-    } else {
-      // Map -deadband to 0 and map -max to -max.
-      //
-      // y - y₁ = m(x - x₁)
-      // y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
-      // y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
-      //
-      // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
-      // x₁ = -deadband
-      // y₁ = 0
-      // x₂ = -max
-      // y₂ = -max
-      //
-      // y = (-max - 0)/(-max + deadband) (x + deadband) + 0
-      // y = max/(max - deadband) (x + deadband)
-      // y = max (x + deadband)/(max - deadband)
-      return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
-    }
-  } else {
+  if (magnitude < deadband) {
     return T{0.0};
   }
+
+  if (value > T{0.0}) {
+    // Map deadband to 0 and map max to max with a linear relationship.
+    //
+    //   y - y₁ = m(x - x₁)
+    //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+    //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+    //
+    // (x₁, y₁) = (deadband, 0) and (x₂, y₂) = (max, max).
+    //
+    //   x₁ = deadband
+    //   y₁ = 0
+    //   x₂ = max
+    //   y₂ = max
+    //   y = (max - 0)/(max - deadband) (x - deadband) + 0
+    //   y = max/(max - deadband) (x - deadband)
+    //
+    // To handle high values of max, rewrite so that max only appears on the
+    // denominator.
+    //
+    //   y = ((max - deadband) + deadband)/(max - deadband) (x - deadband)
+    //   y = (1 + deadband/(max - deadband)) (x - deadband)
+    return (1.0 + deadband / (maxMagnitude - deadband)) * (value - deadband);
+  } else {
+    // Map -deadband to 0 and map -max to -max with a linear relationship.
+    //
+    //   y - y₁ = m(x - x₁)
+    //   y - y₁ = (y₂ - y₁)/(x₂ - x₁) (x - x₁)
+    //   y = (y₂ - y₁)/(x₂ - x₁) (x - x₁) + y₁
+    //
+    // (x₁, y₁) = (-deadband, 0) and (x₂, y₂) = (-max, -max).
+    //
+    //   x₁ = -deadband
+    //   y₁ = 0
+    //   x₂ = -max
+    //   y₂ = -max
+    //   y = (-max - 0)/(-max + deadband) (x + deadband) + 0
+    //   y = max/(max - deadband) (x + deadband)
+    //
+    // To handle high values of max, rewrite so that max only appears on the
+    // denominator.
+    //
+    //   y = ((max - deadband) + deadband)/(max - deadband) (x + deadband)
+    //   y = (1 + deadband/(max - deadband)) (x + deadband)
+    return (1.0 + deadband / (maxMagnitude - deadband)) * (value + deadband);
+  }
 }
 
 /**
@@ -207,4 +217,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

@@ -93,13 +93,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,42 @@ 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;
+
+    m_baseline = m_debounceType == DebounceType::kFalling;
+  }
+
+  /**
+   * 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/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -198,7 +198,8 @@ class TrapezoidProfile {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or
+   * zero if no goal was set.
    */
   constexpr units::second_t TimeLeftUntil(Distance_t target) const {
     Distance_t position = m_current.position * m_direction;
@@ -269,7 +270,8 @@ class TrapezoidProfile {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no
+   * goal was set.
    */
   constexpr units::second_t TotalTime() const { return m_endDecel; }
 
@@ -309,14 +311,14 @@ class TrapezoidProfile {
   }
 
   // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
+  int m_direction = 1;
 
   Constraints m_constraints;
   State m_current;
 
-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDecel;
+  units::second_t m_endAccel = 0_s;
+  units::second_t m_endFullSpeed = 0_s;
+  units::second_t m_endDecel = 0_s;
 };
 
 }  // namespace frc

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,46 +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.calculate(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.calculate(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.calculate(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
@@ -60,17 +64,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 dt = 0.005;
     assertDoesNotThrow(
         () -> {
@@ -78,8 +82,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),
@@ -92,10 +96,10 @@ class UnscentedKalmanFilterTest {
           var u = VecBuilder.fill(12.0, 12.0);
           observer.predict(u, dt);
 
-          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));
@@ -103,7 +107,7 @@ class UnscentedKalmanFilterTest {
               Nat.N5(),
               u,
               globalY,
-              UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+              UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
               R,
               AngleStatistics.angleMean(2),
               AngleStatistics.angleResidual(2),
@@ -113,16 +117,16 @@ class UnscentedKalmanFilterTest {
   }
 
   @Test
-  void testConvergence() {
-    double dt = 0.005;
-    double rb = 0.8382 / 2.0; // Robot radius
+  void testDriveConvergence() {
+    final double dt = 0.005;
+    final double rb = 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),
@@ -146,7 +150,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()));
 
@@ -161,7 +165,7 @@ class UnscentedKalmanFilterTest {
     var trueXhat = observer.getXhat();
 
     double totalTime = trajectory.getTotalTime();
-    for (int i = 0; i < (totalTime / dt); i++) {
+    for (int i = 0; i < (totalTime / dt); ++i) {
       var ref = trajectory.sample(dt * i);
       double vl = ref.velocity * (1 - (ref.curvature * rb));
       double vr = ref.velocity * (1 + (ref.curvature * rb));
@@ -174,24 +178,26 @@ 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(dt);
-      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, dt);
 
       r = nextR;
-      trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt);
+      trueXhat =
+          NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt);
     }
 
-    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));
@@ -199,7 +205,7 @@ class UnscentedKalmanFilterTest {
         Nat.N5(),
         u,
         globalY,
-        UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+        UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
         R,
         AngleStatistics.angleMean(2),
         AngleStatistics.angleResidual(2),
@@ -236,7 +242,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)));
@@ -264,4 +270,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 dt = 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, dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    for (int i = 0; i < steps; ++i) {
+      inputs.set(i, motorControlInput(i * dt));
+      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),
+            dt);
+
+    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), dt);
+      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,21 @@ 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);
+    assertTrue(debouncer.calculate(false));
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java

@@ -246,4 +246,11 @@ class TrapezoidProfileTest {
       }
     }
   }
+
+  @Test
+  void initalizationOfCurrentState() {
+    var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1));
+    assertNear(profile.timeLeftUntil(0), 0, 1e-10);
+    assertNear(profile.totalTime(), 0, 1e-10);
+  }
 }

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,22 @@ 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);
+
+  EXPECT_TRUE(debouncer.Calculate(false));
+}

wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
     }
   }
 }
+
+TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
+  EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
+}