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

[wpimath] Replace UKF implementation with square root form (#4168)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Connor Worley
2022-06-08 22:19:01 -07:00
committed by GitHub
parent 45b7fc445b
commit a99c11c14c
22 changed files with 494 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -90,11 +90,15 @@ template <int CovDim, int States>
Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm,
int angleStatesIdx) {
double sumSin = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::sin(it); })
double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
return std::sin(it);
}) *
Wm)
.sum();
double sumCos = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::cos(it); })
double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
return std::cos(it);
}) *
Wm)
.sum();
Vectord<CovDim> ret = sigmas * Wm;

6
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -21,14 +21,14 @@ class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
Vectord<States> xHat;
Matrixd<States, States> errorCovariances;
Matrixd<States, States> squareRootErrorCovariances;
Vectord<Inputs> inputs;
Vectord<Outputs> localMeasurements;
ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
const Vectord<Outputs>& localY)
: xHat(observer.Xhat()),
errorCovariances(observer.P()),
squareRootErrorCovariances(observer.S()),
inputs(u),
localMeasurements(localY) {}
};
@@ -135,7 +135,7 @@ class KalmanFilterLatencyCompensator {
auto& [key, snapshot] = m_pastObserverSnapshots[i];
if (i == indexOfClosestEntry) {
observer->SetP(snapshot.errorCovariances);
observer->SetS(snapshot.squareRootErrorCovariances);
observer->SetXhat(snapshot.xHat);
}

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

@@ -6,7 +6,6 @@
#include <cmath>
#include "Eigen/Cholesky"
#include "frc/EigenCore.h"
namespace frc {
@@ -52,20 +51,21 @@ class MerweScaledSigmaPoints {
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and covariance(P) of the filter.
* (x) and square-root covariance(S) of the filter.
*
* @param x An array of the means.
* @param P Covariance of the filter.
* @param S Square-root covariance of the filter.
*
* @return Two dimensional array of sigma points. Each column contains all of
* the sigmas for one dimension in the problem space. Ordered by
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*
*/
Matrixd<States, 2 * States + 1> SigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& P) {
Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& S) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
double eta = std::sqrt(lambda + States);
Matrixd<States, States> U = eta * S;
Matrixd<States, 2 * States + 1> sigmas;
sigmas.template block<States, 1>(0, 0) = x;

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

@@ -35,6 +35,10 @@ namespace frc {
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
* "Stochastic control theory".
*
* <p> This class implements a square-root-form unscented Kalman filter
* (SR-UKF). For more information about the SR-UKF, see
* https://www.researchgate.net/publication/3908304.
*
* @tparam States The number of states.
* @tparam Inputs The number of inputs.
* @tparam Outputs The number of outputs.
@@ -111,24 +115,37 @@ class UnscentedKalmanFilter {
units::second_t dt);
/**
* Returns the error covariance matrix P.
* Returns the square-root error covariance matrix S.
*/
const StateMatrix& P() const { return m_P; }
const StateMatrix& S() const { return m_S; }
/**
* Returns an element of the error covariance matrix P.
* Returns an element of the square-root error covariance matrix S.
*
* @param i Row of P.
* @param j Column of P.
* @param i Row of S.
* @param j Column of S.
*/
double P(int i, int j) const { return m_P(i, j); }
double S(int i, int j) const { return m_S(i, j); }
/**
* Set the current error covariance matrix P.
* Set the current square-root error covariance matrix S.
*
* @param S The square-root error covariance matrix S.
*/
void SetS(const StateMatrix& S) { m_S = S; }
/**
* Returns the reconstructed error covariance matrix P.
*/
StateMatrix P() const { return m_S.transpose() * m_S; }
/**
* Set the current square-root error covariance matrix S by taking the square
* root of P.
*
* @param P The error covariance matrix P.
*/
void SetP(const StateMatrix& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
/**
* Returns the state estimate x-hat.
@@ -162,7 +179,7 @@ class UnscentedKalmanFilter {
*/
void Reset() {
m_xHat.setZero();
m_P.setZero();
m_S.setZero();
m_sigmasF.setZero();
}
@@ -254,7 +271,7 @@ class UnscentedKalmanFilter {
m_residualFuncY;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_S;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, 2 * States + 1> m_sigmasF;

49
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -76,21 +76,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
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);
}
auto ret = UnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
m_xHat = std::get<0>(ret);
m_P = std::get<1>(ret);
m_P += discQ;
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
template <int States, int Inputs, int Outputs>
@@ -123,20 +124,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Py] = UnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
Py += discR;
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
Matrixd<States, Rows> Pxy;
@@ -149,19 +152,23 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
.transpose();
}
// K = P_{xy} P_y⁻¹
// Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
// Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
m_P -= K * Py * K.transpose();
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
} // namespace frc

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

@@ -6,15 +6,17 @@
#include <tuple>
#include "Eigen/QR"
#include "frc/EigenCore.h"
namespace frc {
/**
* Computes unscented transform of a set of sigma points and weights. CovDim
* returns the mean and covariance in a tuple.
* returns the mean and square-root covariance of the sigma points in a tuple.
*
* This works in conjunction with the UnscentedKalmanFilter class.
* This works in conjunction with the UnscentedKalmanFilter class. For use with
* square-root form UKFs.
*
* @tparam CovDim Dimension of covariance of sigma points after passing
* through the transform.
@@ -26,12 +28,14 @@ namespace frc {
* vectors using a given set of weights.
* @param residualFunc A function that computes the residual of two state
* vectors (i.e. it subtracts them.)
* @param squareRootR Square-root of the noise covaraince of the sigma points.
*
* @return Tuple of x, mean of sigma points; P, covariance of sigma points after
* passing through the transform.
* @return Tuple of x, mean of sigma points; S, square-root covariance of
* sigmas.
*/
template <int CovDim, int States>
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
SquareRootUnscentedTransform(
const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
@@ -39,25 +43,33 @@ std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
meanFunc,
std::function<Vectord<CovDim>(const Vectord<CovDim>&,
const Vectord<CovDim>&)>
residualFunc) {
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
Vectord<CovDim> x = meanFunc(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Matrixd<CovDim, 2 * States + 1> y;
for (int i = 0; i < 2 * States + 1; ++i) {
// y[:, i] = sigmas[:, i] - x
y.template block<CovDim, 1>(0, i) =
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
Matrixd<CovDim, States * 2 + CovDim> Sbar;
for (int i = 0; i < States * 2; i++) {
Sbar.template block<CovDim, 1>(0, i) =
std::sqrt(Wc[1]) *
residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
}
Matrixd<CovDim, CovDim> P =
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
return std::make_tuple(x, P);
// Merwe defines the QR decomposition as Aᵀ = QR
Matrixd<CovDim, CovDim> S = Sbar.transpose()
.householderQr()
.matrixQR()
.template block<CovDim, CovDim>(0, 0)
.template triangularView<Eigen::Upper>();
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
return std::make_tuple(x, S);
}
} // namespace frc