mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpimath] Refactor KalmanFilter to be steady-state only (#2657)
I didn't notice a performance difference between the original implementation and this one for a flywheel simulation, so this simplifies a lot of internals. This class can no longer implement KalmanTypeFilter because that class allows setting the error covariance for use in the KalmanFilterLatencyCompensator class. This won't impact the holonomic pose estimators that use KalmanFilterLatencyCompensator because they all use an EKF.
This commit is contained in:
Tyler Veness
@@ -56,55 +56,65 @@ class KalmanFilterImpl {
|
||||
units::second_t dt) {
|
||||
m_plant = &plant;
|
||||
|
||||
m_contQ = MakeCovMatrix(stateStdDevs);
|
||||
m_contR = MakeCovMatrix(measurementStdDevs);
|
||||
auto contQ = MakeCovMatrix(stateStdDevs);
|
||||
auto contR = MakeCovMatrix(measurementStdDevs);
|
||||
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(plant.A(), m_contQ, dt, &discA, &discQ);
|
||||
DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
|
||||
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
auto discR = DiscretizeR<Outputs>(contR, dt);
|
||||
|
||||
const auto& C = plant.C();
|
||||
|
||||
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
|
||||
bool isObservable = IsStabilizable<States, Outputs>(discA.transpose(),
|
||||
plant.C().transpose());
|
||||
if (isObservable) {
|
||||
if (Outputs <= States) {
|
||||
m_P = drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), plant.C().transpose(), discQ, m_discR);
|
||||
} else {
|
||||
m_P.setZero();
|
||||
}
|
||||
} else {
|
||||
bool isObservable =
|
||||
IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
|
||||
if (!isObservable) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"The system passed to the Kalman Filter is not observable!");
|
||||
"The system passed to the Kalman filter is not observable!");
|
||||
throw std::invalid_argument(
|
||||
"The system passed to the Kalman Filter is not observable!");
|
||||
"The system passed to the Kalman filter is not observable!");
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, States, States> P =
|
||||
drake::math::DiscreteAlgebraicRiccatiEquation(
|
||||
discA.transpose(), C.transpose(), discQ, discR);
|
||||
|
||||
Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
|
||||
|
||||
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
|
||||
// efficiently.
|
||||
//
|
||||
// K = PC^T S^-1
|
||||
// KS = PC^T
|
||||
// (KS)^T = (PC^T)^T
|
||||
// S^T K^T = CP^T
|
||||
//
|
||||
// The solution of Ax = b can be found via x = A.solve(b).
|
||||
//
|
||||
// K^T = S^T.solve(CP^T)
|
||||
// K = (S^T.solve(CP^T))^T
|
||||
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
|
||||
|
||||
Reset();
|
||||
}
|
||||
|
||||
KalmanFilterImpl(KalmanFilterImpl&&) = default;
|
||||
KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default;
|
||||
|
||||
/**
|
||||
* Returns the error covariance matrix P.
|
||||
* Returns the steady-state Kalman gain matrix K.
|
||||
*/
|
||||
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
|
||||
const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
|
||||
|
||||
/**
|
||||
* Returns an element of the error covariance matrix P.
|
||||
* Returns an element of the steady-state Kalman gain matrix K.
|
||||
*
|
||||
* @param i Row of P.
|
||||
* @param j Column of P.
|
||||
* @param i Row of K.
|
||||
* @param j Column of K.
|
||||
*/
|
||||
double P(int i, int j) const { return m_P(i, j); }
|
||||
|
||||
/**
|
||||
* Set the current error covariance matrix P.
|
||||
*
|
||||
* @param P The error covariance matrix P.
|
||||
*/
|
||||
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
|
||||
double K(int i, int j) const { return m_K(i, j); }
|
||||
|
||||
/**
|
||||
* Returns the state estimate x-hat.
|
||||
@@ -146,13 +156,6 @@ class KalmanFilterImpl {
|
||||
*/
|
||||
void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
|
||||
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
|
||||
|
||||
Eigen::Matrix<double, States, States> discA;
|
||||
Eigen::Matrix<double, States, States> discQ;
|
||||
DiscretizeAQTaylor<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
|
||||
|
||||
m_P = discA * m_P * discA.transpose() + discQ;
|
||||
m_discR = DiscretizeR<Outputs>(m_contR, dt);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -163,75 +166,19 @@ class KalmanFilterImpl {
|
||||
*/
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Outputs, 1>& y) {
|
||||
Correct<Outputs>(u, y, m_plant->C(), m_plant->D(), m_discR);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 C matrix 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 C Output matrix.
|
||||
* @param D Feedthrough matrix.
|
||||
* @param R Measurement noise covariance matrix.
|
||||
*/
|
||||
template <int Rows>
|
||||
void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
|
||||
const Eigen::Matrix<double, Rows, 1>& y,
|
||||
const Eigen::Matrix<double, Rows, States>& C,
|
||||
const Eigen::Matrix<double, Rows, Inputs>& D,
|
||||
const Eigen::Matrix<double, Rows, Rows>& R) {
|
||||
const auto& x = m_xHat;
|
||||
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + R;
|
||||
|
||||
// We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
|
||||
// efficiently.
|
||||
//
|
||||
// K = PC^T S^-1
|
||||
// KS = PC^T
|
||||
// (KS)^T = (PC^T)^T
|
||||
// S^T K^T = CP^T
|
||||
//
|
||||
// The solution of Ax = b can be found via x = A.solve(b).
|
||||
//
|
||||
// K^T = S^T.solve(CP^T)
|
||||
// K = (S^T.solve(CP^T))^T
|
||||
Eigen::Matrix<double, States, Rows> K =
|
||||
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
|
||||
|
||||
m_xHat = x + K * (y - (C * x + D * u));
|
||||
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
|
||||
m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
|
||||
}
|
||||
|
||||
private:
|
||||
LinearSystem<States, Inputs, Outputs>* m_plant;
|
||||
|
||||
/**
|
||||
* Error covariance matrix.
|
||||
* The steady-state Kalman gain matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, States> m_P;
|
||||
Eigen::Matrix<double, States, Outputs> m_K;
|
||||
|
||||
/**
|
||||
* Continuous process noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, States, States> m_contQ;
|
||||
|
||||
/**
|
||||
* Continuous measurement noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, Outputs> m_contR;
|
||||
|
||||
/**
|
||||
* Discrete measurement noise covariance matrix.
|
||||
*/
|
||||
Eigen::Matrix<double, Outputs, Outputs> m_discR;
|
||||
|
||||
/**
|
||||
* State estimate x-hat.
|
||||
* The state estimate.
|
||||
*/
|
||||
Eigen::Matrix<double, States, 1> m_xHat;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user