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

[wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850)

Browse Source 
Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular.

This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed.

To fix the bug, a couple things needed to be changed:
all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor,
In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular,
P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively.

Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jack Cammarata
2025-03-14 13:05:15 -04:00
committed by GitHub
parent 71b6e8ec58
commit 1efaaefd78
7 changed files with 494 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

@@ -51,7 +51,7 @@ class MerweScaledSigmaPoints {
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and square-root covariance(S) of the filter.
* (x) and square-root covariance (S) of the filter.
*
* @param x An array of the means.
* @param S Square-root covariance of the filter.
@@ -68,6 +68,8 @@ class MerweScaledSigmaPoints {
Matrixd<States, States> U = eta * S;
Matrixd<States, 2 * States + 1> sigmas;
// equation (17)
sigmas.template block<States, 1>(0, 0) = x;
for (int k = 0; k < States; ++k) {
sigmas.template block<States, 1>(0, k + 1) =

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

@@ -43,7 +43,9 @@ namespace frc {
* "Stochastic control theory".
*
* <p> This class implements a square-root-form unscented Kalman filter
* (SR-UKF). For more information about the SR-UKF, see
* (SR-UKF). The main reason for this is to guarantee that the covariance
* matrix remains positive definite.
* For more information about the SR-UKF, see
* https://www.researchgate.net/publication/3908304.
*
* @tparam States Number of states.
@@ -108,7 +110,7 @@ class UnscentedKalmanFilter {
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
* Constructs an Unscented Kalman filter with custom mean, residual, and
* addition functions. Using custom functions for arithmetic can be useful if
* you have angles in the state or measurements, because they allow you to
* correctly account for the modular nature of angle arithmetic.
@@ -189,7 +191,7 @@ class UnscentedKalmanFilter {
/**
* Returns the reconstructed error covariance matrix P.
*/
StateMatrix P() const { return m_S.transpose() * m_S; }
StateMatrix P() const { return m_S * m_S.transpose(); }
/**
* Set the current square-root error covariance matrix S by taking the square
@@ -197,7 +199,7 @@ class UnscentedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
void SetP(const StateMatrix& P) { m_S = P.llt().matrixL(); }
/**
* Returns the state estimate x-hat.
@@ -252,14 +254,28 @@ class UnscentedKalmanFilter {
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
// Generate sigma points around the state mean
//
// equation (17)
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
// Project each sigma point forward in time according to the
// dynamics f(x, u)
//
// sigmas = 𝒳ₖ₋₁
// sigmasF = 𝒳ₖ,ₖ₋₁ or just 𝒳 for readability
//
// equation (18)
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
// Pass the predicted sigmas (𝒳) through the Unscented Transform
// to compute the prior state mean and covariance
//
// equations (18) (19) and (20)
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
@@ -367,7 +383,15 @@ class UnscentedKalmanFilter {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
// Generate new sigma points from the prior mean and covariance
// and transform them into measurement space using h(x, u)
//
// sigmas = 𝒳
// sigmasH = 𝒴
//
// This differs from equation (22) which uses
// the prior sigma points, regenerating them allows
// multiple measurement updates per time update
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
@@ -376,16 +400,26 @@ class UnscentedKalmanFilter {
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
// Pass the predicted measurement sigmas through the Unscented Transform
// to compute the mean predicted measurement and square-root innovation
// covariance.
//
// equations (23) (24) and (25)
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
// Compute cross covariance of the predicted state and measurement sigma
// points given as:
//
// 2n
// P_{xy} = Σ Wᵢ⁽ᶜ⁾[𝒳ᵢ - x̂][𝒴ᵢ - ŷ⁻]ᵀ
// i=0
//
// equation (26)
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
@@ -393,21 +427,39 @@ class UnscentedKalmanFilter {
.transpose();
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
// Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
// is equivalent to MATLAB's \ operator, so we need to rearrange to use
// that.
//
// K = (P_{xy} / S_{y}ᵀ) / S_{y}
// K = (S_{y} \ P_{xy})ᵀ / S_{y}
// K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
//
// equation (27)
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
// Compute the posterior state mean
//
// x̂ = x̂⁻ + K(y − ŷ⁻)
//
// second part of equation (27)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
// Compute the intermediate matrix U for downdating
// the square-root covariance
//
// equation (28)
Matrixd<States, Rows> U = K * Sy;
// Downdate the posterior square-root state covariance
//
// equation (29)
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}

38
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -47,12 +47,21 @@ SquareRootUnscentedTransform(
const Vectord<CovDim>&)>
residualFunc,
const Matrixd<CovDim, CovDim>& squareRootR) {
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
// New mean is usually just the sum of the sigmas * weights:
//
// 2n
// x̂ = Σ Wᵢ⁽ᵐ⁾𝒳ᵢ
// i=0
//
// equations (19) and (23) in the paper show this,
// but we allow a custom function, usually for angle wrapping
Vectord<CovDim> x = meanFunc(sigmas, Wm);
// Form an intermediate matrix S⁻ as:
//
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
//
// the part of equations (20) and (24) within the "qr{}"
Matrixd<CovDim, States * 2 + CovDim> Sbar;
for (int i = 0; i < States * 2; i++) {
Sbar.template block<CovDim, 1>(0, i) =
@@ -61,14 +70,29 @@ SquareRootUnscentedTransform(
}
Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
// Merwe defines the QR decomposition as Aᵀ = QR
// Compute the square-root covariance of the sigma points.
//
// We transpose S⁻ first because we formed it by horizontally
// concatenating each part; it should be vertical so we can take
// the QR decomposition as defined in the "QR Decomposition" passage
// of section 3. "EFFICIENT SQUARE-ROOT IMPLEMENTATION"
//
// The resulting matrix R is the square-root covariance S, but it
// is upper triangular, so we need to transpose it.
//
// equations (20) and (24)
Matrixd<CovDim, CovDim> S = Sbar.transpose()
.householderQr()
.matrixQR()
.template block<CovDim, CovDim>(0, 0)
.template triangularView<Eigen::Upper>();
.template triangularView<Eigen::Upper>()
.transpose();
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
// Update or downdate the square-root covariance with (𝒳₀-x̂)
// depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
//
// equations (21) and (25)
Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(
S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
return std::make_tuple(x, S);