[wpimath] Move DiscretizeR() in EKF and UKF from Predict() to Correct() (#2753)

By storing the previous dt, it can be moved into Correct() where it is
actually used. This lets us take the continuous R as an argument in the
user-provided R overload.

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java

@@ -39,13 +39,13 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
   @SuppressWarnings("MemberName")
   private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
   private final Matrix<States, States> m_contQ;
-  private Matrix<Outputs, Outputs> m_discR;
   private final Matrix<States, States> m_initP;
   private final Matrix<Outputs, Outputs> m_contR;
   @SuppressWarnings("MemberName")
   private Matrix<States, N1> m_xHat;
   @SuppressWarnings("MemberName")
   private Matrix<States, States> m_P;
+  private double m_dtSeconds;
 
   /**
    * Constructs an extended Kalman filter.
@@ -78,10 +78,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     m_f = f;
     m_h = h;
 
-    reset();
-
     m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
     this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dtSeconds = dtSeconds;
+
+    reset();
 
     final var contA = NumericalJacobian
           .numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
@@ -92,13 +93,13 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     final var discA = discPair.getFirst();
     final var discQ = discPair.getSecond();
 
-    m_discR = Discretization.discretizeR(m_contR, dtSeconds);
+    final var discR = Discretization.discretizeR(m_contR, dtSeconds);
 
     // IsStabilizable(A^T, C^T) will tell us if the system is observable.
     boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
     if (isObservable && outputs.getNum() <= states.getNum()) {
       m_initP = Drake.discreteAlgebraicRiccatiEquation(
-            discA.transpose(), C.transpose(), discQ, m_discR) ;
+            discA.transpose(), C.transpose(), discQ, discR) ;
     } else {
       m_initP = new Matrix<>(states, states);
     }
@@ -223,7 +224,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
 
     m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
     m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
-    m_discR = Discretization.discretizeR(m_contR, dtSeconds);
+    m_dtSeconds = dtSeconds;
   }
 
   /**
@@ -235,7 +236,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
   @SuppressWarnings("ParameterName")
   @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    correct(m_outputs, u, y, m_h, m_discR);
+    correct(m_outputs, u, y, m_h, m_contR);
   }
 
   /**
@@ -261,8 +262,9 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
         Matrix<Rows, Rows> R
   ) {
     final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
 
-    final var S = C.times(m_P).times(C.transpose()).plus(R);
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
 
     // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
     // efficiently.

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

@@ -45,8 +45,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
   private Matrix<States, States> m_P;
   private final Matrix<States, States> m_contQ;
   private final Matrix<Outputs, Outputs> m_contR;
-  private Matrix<Outputs, Outputs> m_discR;
   private Matrix<States, ?> m_sigmasF;
+  private double m_dtSeconds;
 
   private final MerweScaledSigmaPoints<States> m_pts;
 
@@ -61,7 +61,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
    *                           the measurement vector.
    * @param stateStdDevs       Standard deviations of model states.
    * @param measurementStdDevs Standard deviations of measurements.
-   * @param nominalDtSeconds   Nominal discretization timestep.
+   * @param dtSeconds          Nominal discretization timestep.
    */
   @SuppressWarnings("ParameterName")
   public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
@@ -71,7 +71,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
                                    Matrix<Outputs, N1>> h,
                                Matrix<States, N1> stateStdDevs,
                                Matrix<Outputs, N1> measurementStdDevs,
-                               double nominalDtSeconds) {
+                               double dtSeconds) {
     this.m_states = states;
     this.m_outputs = outputs;
 
@@ -81,7 +81,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
     m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
     m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
 
-    m_discR = Discretization.discretizeR(m_contR, nominalDtSeconds);
+    m_dtSeconds = dtSeconds;
 
     m_pts = new MerweScaledSigmaPoints<>(states);
 
@@ -238,7 +238,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
 
     m_xHat = ret.getFirst();
     m_P = ret.getSecond().plus(discQ);
-    m_discR = Discretization.discretizeR(m_contR, dtSeconds);
+    m_dtSeconds = dtSeconds;
   }
 
   /**
@@ -250,7 +250,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
   @SuppressWarnings("ParameterName")
   @Override
   public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    correct(m_outputs, u, y, m_h, m_discR);
+    correct(m_outputs, u, y, m_h, m_contR);
   }
 
   /**
@@ -272,6 +272,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
         Matrix<R, N1> y,
         BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
         Matrix<R, R> R) {
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
     // Transform sigma points into measurement space
     Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(
           rows.getNum(), 2 * m_states.getNum() + 1));
@@ -287,7 +289,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
     // Mean and covariance of prediction passed through unscented transform
     var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc());
     var yHat = transRet.getFirst();
-    var Py = transRet.getSecond().plus(R);
+    var Py = transRet.getSecond().plus(discR);
 
     // Compute cross covariance of the state and the measurements
     Matrix<States, R> Pxy = new Matrix<>(m_states, rows);