diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index c69d94b797..59323b21d6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -293,24 +293,14 @@ public class ExtendedKalmanFilterThis 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). + *

This is useful for when the measurement noise covariances vary. * - * @param 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 void correct( - Nat rows, - Matrix u, - Matrix y, - BiFunction, Matrix, Matrix> h, - Matrix contR) { - correct(rows, u, y, h, contR, Matrix::minus, Matrix::plus); + public void correct(Matrix u, Matrix y, Matrix R) { + correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX); } /** @@ -325,7 +315,30 @@ public class ExtendedKalmanFilter void correct( + Nat rows, + Matrix u, + Matrix y, + BiFunction, Matrix, Matrix> h, + Matrix R) { + correct(rows, u, y, h, R, Matrix::minus, Matrix::plus); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + *

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 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 u, Matrix y, BiFunction, Matrix, Matrix> h, - Matrix contR, + Matrix R, BiFunction, Matrix, Matrix> residualFuncY, BiFunction, Matrix, Matrix> 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); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index 132faf6d2a..a5de856feb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -378,6 +378,19 @@ public class UnscentedKalmanFilterThis 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 u, Matrix y, Matrix 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 void correct( Nat rows, @@ -419,7 +432,7 @@ public class UnscentedKalmanFilter(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX); } + /** + * Correct the state estimate x-hat using the measurements in y. + * + * 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. + */ + void Correct(const InputVector& u, const OutputVector& y, + const Matrixd& R) { + Correct(u, y, m_h, R, m_residualFuncY, m_addFuncX); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * 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 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. + */ template void Correct( const InputVector& u, const Vectord& y, @@ -188,7 +215,7 @@ class ExtendedKalmanFilter { * @param y Measurement vector. * @param h A vector-valued function of x and u that returns * the measurement vector. - * @param R Discrete measurement noise covariance matrix. + * @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. diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index fe583e79ef..9526f0cb48 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -210,6 +210,21 @@ class UnscentedKalmanFilter { m_residualFuncX, m_addFuncX); } + /** + * Correct the state estimate x-hat using the measurements in y. + * + * 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. + */ + void Correct(const InputVector& u, const OutputVector& y, + const Matrixd& R) { + Correct(u, y, m_h, R, m_meanFuncY, m_residualFuncY, + m_residualFuncX, m_addFuncX); + } + /** * Correct the state estimate x-hat using the measurements in y. * @@ -221,7 +236,7 @@ class UnscentedKalmanFilter { * @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. */ template void Correct( @@ -240,7 +255,7 @@ class UnscentedKalmanFilter { * @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