Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Also clean up comments on other overloads and fix a typo.
This commit is contained in:
Tyler Veness
2023-10-26 19:17:15 -07:00
committed by GitHub
parent 60bcdeded9
commit 98c14f1692
4 changed files with 90 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -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,
@@ -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.

19
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -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.
*
@@ -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(
@@ -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