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] Move DiscretizeR() in EKF and UKF from Predict() to Correct() (#2753)

Browse Source 
By storing the previous dt, it can be moved into Correct() where it is
actually used. This lets us take the continuous R as an argument in the
user-provided R overload.
This commit is contained in:
Tyler Veness
2020-10-07 21:54:38 -07:00
committed by GitHub
parent bf26656547
commit 32f429a819
4 changed files with 37 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,6 +49,7 @@ class ExtendedKalmanFilter {
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
@@ -63,14 +64,15 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_discR = DiscretizeR<Outputs>(m_contR, dt);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
bool isObservable =
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
if (isObservable && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, m_discR);
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
}
@@ -139,6 +141,8 @@ class ExtendedKalmanFilter {
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
m_dt = dt;
// Find continuous A
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -150,7 +154,6 @@ class ExtendedKalmanFilter {
m_xHat = RungeKutta(m_f, m_xHat, u, dt);
m_P = discA * m_P * discA.transpose() + discQ;
m_discR = DiscretizeR<Outputs>(m_contR, dt);
}
/**
@@ -161,7 +164,7 @@ class ExtendedKalmanFilter {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
Correct<Outputs>(u, y, m_h, m_discR);
Correct<Outputs>(u, y, m_h, m_contR);
}
/**
@@ -187,8 +190,9 @@ class ExtendedKalmanFilter {
const Eigen::Matrix<double, Rows, Rows>& R) {
const Eigen::Matrix<double, Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + R;
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
// efficiently.
@@ -222,7 +226,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, Outputs, Outputs> m_discR;
units::second_t m_dt;
Eigen::Matrix<double, States, States> m_initP;
};

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

@@ -50,8 +50,7 @@ class UnscentedKalmanFilter {
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_discR = DiscretizeR<Outputs>(m_contR, dt);
m_dt = dt;
Reset();
}
@@ -119,6 +118,8 @@ class UnscentedKalmanFilter {
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -141,7 +142,6 @@ class UnscentedKalmanFilter {
m_P = std::get<1>(ret);
m_P += discQ;
m_discR = DiscretizeR<Outputs>(m_contR, dt);
}
/**
@@ -152,7 +152,7 @@ class UnscentedKalmanFilter {
*/
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
const Eigen::Matrix<double, Outputs, 1>& y) {
Correct<Outputs>(u, y, m_h, m_discR);
Correct<Outputs>(u, y, m_h, m_contR);
}
/**
@@ -176,6 +176,8 @@ class UnscentedKalmanFilter {
const Eigen::Matrix<double, Inputs, 1>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
// Transform sigma points into measurement space
Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
@@ -188,7 +190,7 @@ class UnscentedKalmanFilter {
// Mean and covariance of prediction passed through UT
auto [yHat, Py] =
UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
Py += R;
Py += discR;
// Compute cross covariance of the state and the measurements
Eigen::Matrix<double, States, Rows> Pxy;
@@ -224,8 +226,8 @@ class UnscentedKalmanFilter {
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, Outputs, Outputs> m_discR;
Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;
};