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 c69d94b7972..59323b21d65 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 @@ -290,6 +290,19 @@ public void correct(Matrix u, Matrix y) { correct(m_outputs, 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. + */ + public void correct(Matrix u, Matrix y, Matrix R) { + correct(m_outputs, u, y, m_h, R, m_residualFuncY, m_addFuncX); + } + /** * Correct the state estimate x-hat using the measurements in y. * @@ -302,15 +315,15 @@ public void correct(Matrix u, Matrix y) { * @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); + Matrix R) { + correct(rows, u, y, h, R, Matrix::minus, Matrix::plus); } /** @@ -325,7 +338,7 @@ public void correct( * @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. * @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 void correct( Matrix 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 132faf6d2aa..a5de856feb0 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 void correct(Matrix u, Matrix y) { 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. + * + *

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 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 void correct(Matrix u, Matrix y) { * @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 void correct( Nat rows, @@ -419,7 +432,7 @@ public void correct( * @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 diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h index 0b3450b9291..32ed558f71b 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -171,6 +171,33 @@ class ExtendedKalmanFilter { Correct(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 fe583e79eff..9526f0cb489 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