[wpimath] Add EKF/UKF u-y-R correct overload (#5832)

Also clean up comments on other overloads and fix a typo.

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

@@ -293,24 +293,14 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
-   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
-   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
-   * of this function).
+   * <p>This is useful for when the measurement noise covariances vary.
    *
-   * @param <Rows> Number of rows in the result of f(x, u).
-   * @param rows Number of rows in the result of f(x, u).
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
    */
-  public <Rows extends Num> void correct(
-      Nat<Rows> rows,
-      Matrix<Inputs, N1> u,
-      Matrix<Rows, N1> y,
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR) {
-    correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus);
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX);
   }
 
   /**
@@ -325,7 +315,30 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param contR Continuous measurement noise covariance matrix.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R) {
+    correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
    *     subtracts them.)
    * @param addFuncX A function that adds two state vectors.
@@ -335,11 +348,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
       Matrix<Inputs, N1> u,
       Matrix<Rows, N1> y,
       BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-      Matrix<Rows, Rows> contR,
+      Matrix<Rows, Rows> R,
       BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
-    final var discR = Discretization.discretizeR(contR, m_dtSeconds);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
 
     final var S = C.times(m_P).times(C.transpose()).plus(discR);
 

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

@@ -378,6 +378,19 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
         m_outputs, u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
   }
 
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurement noise covariances vary.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param R Continuous measurement noise covariance matrix.
+   */
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y, Matrix<Outputs, Outputs> R) {
+    correct(m_outputs, u, y, m_h, R, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
   /**
    * Correct the state estimate x-hat using the measurements in y.
    *
@@ -390,7 +403,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    */
   public <R extends Num> void correct(
       Nat<R> rows,
@@ -419,7 +432,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    * @param h A vector-valued function of x and u that returns the measurement vector.
-   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param R Continuous measurement noise covariance matrix.
    * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
    *     a given set of weights.
    * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it