Skip to content

Commit

Permalink
[wpimath] Add EKF/UKF u-y-R correct overload (#5832)
Browse files Browse the repository at this point in the history 
Also clean up comments on other overloads and fix a typo.
  • Loading branch information
@calcmogul
calcmogul committed Oct 27, 2023
1 parent 60bcded commit 98c14f1
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 11 deletions.
25 changes: 19 additions & 6 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,19 @@ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> 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.
*
* <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_residualFuncY, m_addFuncX);
}

/**
* Correct the state estimate x-hat using the measurements in y.
*
Expand All @@ -302,15 +315,15 @@ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> 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 <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);
Matrix<Rows, Rows> R) {
correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
}

/**
Expand All @@ -325,7 +338,7 @@ public <Rows extends Num> 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.
Expand All @@ -335,11 +348,11 @@ public <Rows extends Num> void correct(
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);

Expand Down
17 changes: 15 additions & 2 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -378,6 +378,19 @@ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> 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.
*
* <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.
*
Expand All @@ -390,7 +403,7 @@ public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> 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 <R extends Num> void correct(
Nat<R> rows,
Expand Down Expand Up @@ -419,7 +432,7 @@ public <R extends Num> 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
Expand Down
29 changes: 28 additions & 1 deletion wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,6 +171,33 @@ class ExtendedKalmanFilter {
Correct<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.
*/
void Correct(const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R) {
Correct<Outputs>(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 <int Rows>
void Correct(
const InputVector& u, const Vectord<Rows>& y,
Expand All @@ -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.
Expand Down
19 changes: 17 additions & 2 deletions wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Outputs, Outputs>& R) {
Correct<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.
*
Expand All @@ -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 <int Rows>
void Correct(
Expand All @@ -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
Expand Down

0 comments on commit 98c14f1

Please sign in to comment.