mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Fix UnscentedKalmanFilter and improve math docs (#7850)
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
@@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
|
||||
* of the filter.
|
||||
* Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root
|
||||
* covariance (s) of the filter.
|
||||
*
|
||||
* @param x An array of the means.
|
||||
* @param s Square-root covariance of the filter.
|
||||
@@ -86,6 +86,8 @@ public class MerweScaledSigmaPoints<S extends Num> {
|
||||
// 2 * states + 1 by states
|
||||
Matrix<S, ?> sigmas =
|
||||
new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
|
||||
|
||||
// equation (17)
|
||||
sigmas.setColumn(0, x);
|
||||
for (int k = 0; k < m_states.getNum(); k++) {
|
||||
var xPlusU = x.plus(U.extractColumnVector(k));
|
||||
|
||||
@@ -35,9 +35,9 @@ import org.ejml.simple.SimpleMatrix;
|
||||
* href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>
|
||||
* 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 <a
|
||||
* href="https://www.researchgate.net/publication/3908304">https://www.researchgate.net/publication/3908304</a>.
|
||||
* <p>This class implements a square-root-form unscented Kalman filter (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.
|
||||
*
|
||||
* @param <States> Number of states.
|
||||
* @param <Inputs> Number of inputs.
|
||||
@@ -105,7 +105,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
|
||||
* 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.
|
||||
*
|
||||
@@ -193,12 +193,21 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
"Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
|
||||
}
|
||||
|
||||
// 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
|
||||
Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
|
||||
|
||||
// Form an intermediate matrix S⁻ as:
|
||||
//
|
||||
// [√{W₁⁽ᶜ⁾}(𝒳_{1:2L} - x̂) √{Rᵛ}]
|
||||
//
|
||||
// the part of equations (20) and (24) within the "qr{}"
|
||||
Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
|
||||
for (int i = 0; i < 2 * s.getNum(); i++) {
|
||||
Sbar.setColumn(
|
||||
@@ -214,8 +223,24 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage);
|
||||
}
|
||||
|
||||
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
|
||||
newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
|
||||
// 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)
|
||||
Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)).transpose());
|
||||
|
||||
// Update or downdate the square-root covariance with (𝒳₀-x̂)
|
||||
// depending on whether its weight (W₀⁽ᶜ⁾) is positive or negative.
|
||||
//
|
||||
// equations (21) and (25)
|
||||
newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), true);
|
||||
|
||||
return new Pair<>(x, newS);
|
||||
}
|
||||
@@ -256,7 +281,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
*/
|
||||
@Override
|
||||
public Matrix<States, States> getP() {
|
||||
return m_S.transpose().times(m_S);
|
||||
return m_S.times(m_S.transpose());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -280,7 +305,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
*/
|
||||
@Override
|
||||
public void setP(Matrix<States, States> newP) {
|
||||
m_S = newP.lltDecompose(false);
|
||||
m_S = newP.lltDecompose(true);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -347,14 +372,28 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
|
||||
var squareRootDiscQ = discQ.lltDecompose(true);
|
||||
|
||||
// Generate sigma points around the state mean
|
||||
//
|
||||
// equation (17)
|
||||
var 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.getNumSigmas(); ++i) {
|
||||
Matrix<States, N1> x = sigmas.extractColumnVector(i);
|
||||
|
||||
m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
|
||||
}
|
||||
|
||||
// Pass the predicted sigmas (𝒳) through the Unscented Transform
|
||||
// to compute the prior state mean and covariance
|
||||
//
|
||||
// equations (18) (19) and (20)
|
||||
var ret =
|
||||
squareRootUnscentedTransform(
|
||||
m_states,
|
||||
@@ -459,7 +498,15 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
final var discR = Discretization.discretizeR(R, m_dtSeconds);
|
||||
final var squareRootDiscR = discR.lltDecompose(true);
|
||||
|
||||
// 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
|
||||
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
|
||||
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
|
||||
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
|
||||
@@ -467,7 +514,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
sigmasH.setColumn(i, hRet);
|
||||
}
|
||||
|
||||
// Mean and covariance of prediction passed through unscented transform
|
||||
// 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)
|
||||
var transRet =
|
||||
squareRootUnscentedTransform(
|
||||
m_states,
|
||||
@@ -481,30 +532,54 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
|
||||
var yHat = transRet.getFirst();
|
||||
var Sy = transRet.getSecond();
|
||||
|
||||
// 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)
|
||||
Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
|
||||
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
|
||||
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
|
||||
var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
|
||||
var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
|
||||
|
||||
Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
|
||||
}
|
||||
|
||||
// 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)
|
||||
Matrix<States, R> K =
|
||||
Sy.transpose()
|
||||
.solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
|
||||
.transpose();
|
||||
|
||||
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
|
||||
// Compute the posterior state mean
|
||||
//
|
||||
// x̂ = x̂⁻ + K(y − ŷ⁻)
|
||||
//
|
||||
// second part of equation (27)
|
||||
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
|
||||
|
||||
// Compute the intermediate matrix U for downdating
|
||||
// the square-root covariance
|
||||
//
|
||||
// equation (28)
|
||||
Matrix<States, R> U = K.times(Sy);
|
||||
|
||||
// Downdate the posterior square-root state covariance
|
||||
//
|
||||
// equation (29)
|
||||
for (int i = 0; i < rows.getNum(); i++) {
|
||||
m_S.rankUpdate(U.extractColumnVector(i), -1, false);
|
||||
m_S.rankUpdate(U.extractColumnVector(i), -1, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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) =
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.StateSpaceUtil;
|
||||
@@ -18,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.numbers.N4;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.system.Discretization;
|
||||
import edu.wpi.first.math.system.NumericalIntegration;
|
||||
@@ -26,11 +28,13 @@ import edu.wpi.first.math.system.plant.DCMotor;
|
||||
import edu.wpi.first.math.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class UnscentedKalmanFilterTest {
|
||||
private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
var motors = DCMotor.getCIM(2);
|
||||
|
||||
// var gLow = 15.32; // Low gear ratio
|
||||
@@ -63,17 +67,17 @@ class UnscentedKalmanFilterTest {
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.UnusedFormalParameter")
|
||||
private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.UnusedFormalParameter")
|
||||
private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
return x.copy();
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInit() {
|
||||
void testDriveInit() {
|
||||
var dtSeconds = 0.005;
|
||||
assertDoesNotThrow(
|
||||
() -> {
|
||||
@@ -81,8 +85,8 @@ class UnscentedKalmanFilterTest {
|
||||
new UnscentedKalmanFilter<>(
|
||||
Nat.N5(),
|
||||
Nat.N3(),
|
||||
UnscentedKalmanFilterTest::getDynamics,
|
||||
UnscentedKalmanFilterTest::getLocalMeasurementModel,
|
||||
UnscentedKalmanFilterTest::driveDynamics,
|
||||
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
|
||||
VecBuilder.fill(0.0001, 0.01, 0.01),
|
||||
AngleStatistics.angleMean(2),
|
||||
@@ -95,10 +99,10 @@ class UnscentedKalmanFilterTest {
|
||||
var u = VecBuilder.fill(12.0, 12.0);
|
||||
observer.predict(u, dtSeconds);
|
||||
|
||||
var localY = getLocalMeasurementModel(observer.getXhat(), u);
|
||||
var localY = driveLocalMeasurementModel(observer.getXhat(), u);
|
||||
observer.correct(u, localY);
|
||||
|
||||
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
|
||||
var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
|
||||
var R =
|
||||
StateSpaceUtil.makeCovarianceMatrix(
|
||||
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
|
||||
@@ -106,7 +110,7 @@ class UnscentedKalmanFilterTest {
|
||||
Nat.N5(),
|
||||
u,
|
||||
globalY,
|
||||
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
|
||||
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
|
||||
R,
|
||||
AngleStatistics.angleMean(2),
|
||||
AngleStatistics.angleResidual(2),
|
||||
@@ -116,16 +120,16 @@ class UnscentedKalmanFilterTest {
|
||||
}
|
||||
|
||||
@Test
|
||||
void testConvergence() {
|
||||
double dtSeconds = 0.005;
|
||||
double rbMeters = 0.8382 / 2.0; // Robot radius
|
||||
void testDriveConvergence() {
|
||||
final double dtSeconds = 0.005;
|
||||
final double rbMeters = 0.8382 / 2.0; // Robot radius
|
||||
|
||||
UnscentedKalmanFilter<N5, N2, N3> observer =
|
||||
new UnscentedKalmanFilter<>(
|
||||
Nat.N5(),
|
||||
Nat.N3(),
|
||||
UnscentedKalmanFilterTest::getDynamics,
|
||||
UnscentedKalmanFilterTest::getLocalMeasurementModel,
|
||||
UnscentedKalmanFilterTest::driveDynamics,
|
||||
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
|
||||
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
|
||||
VecBuilder.fill(0.0001, 0.5, 0.5),
|
||||
AngleStatistics.angleMean(2),
|
||||
@@ -149,7 +153,7 @@ class UnscentedKalmanFilterTest {
|
||||
NumericalJacobian.numericalJacobianU(
|
||||
Nat.N5(),
|
||||
Nat.N2(),
|
||||
UnscentedKalmanFilterTest::getDynamics,
|
||||
UnscentedKalmanFilterTest::driveDynamics,
|
||||
new Matrix<>(Nat.N5(), Nat.N1()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
|
||||
@@ -164,7 +168,7 @@ class UnscentedKalmanFilterTest {
|
||||
var trueXhat = observer.getXhat();
|
||||
|
||||
double totalTime = trajectory.getTotalTimeSeconds();
|
||||
for (int i = 0; i < (totalTime / dtSeconds); i++) {
|
||||
for (int i = 0; i < (totalTime / dtSeconds); ++i) {
|
||||
var ref = trajectory.sample(dtSeconds * i);
|
||||
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
|
||||
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
|
||||
@@ -177,25 +181,27 @@ class UnscentedKalmanFilterTest {
|
||||
vl,
|
||||
vr);
|
||||
|
||||
Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
Matrix<N3, N1> localY =
|
||||
driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
|
||||
|
||||
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
|
||||
|
||||
var rdot = nextR.minus(r).div(dtSeconds);
|
||||
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
|
||||
u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
|
||||
|
||||
observer.predict(u, dtSeconds);
|
||||
|
||||
r = nextR;
|
||||
trueXhat =
|
||||
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
|
||||
NumericalIntegration.rk4(
|
||||
UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dtSeconds);
|
||||
}
|
||||
|
||||
var localY = getLocalMeasurementModel(trueXhat, u);
|
||||
var localY = driveLocalMeasurementModel(trueXhat, u);
|
||||
observer.correct(u, localY);
|
||||
|
||||
var globalY = getGlobalMeasurementModel(trueXhat, u);
|
||||
var globalY = driveGlobalMeasurementModel(trueXhat, u);
|
||||
var R =
|
||||
StateSpaceUtil.makeCovarianceMatrix(
|
||||
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
|
||||
@@ -203,7 +209,7 @@ class UnscentedKalmanFilterTest {
|
||||
Nat.N5(),
|
||||
u,
|
||||
globalY,
|
||||
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
|
||||
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
|
||||
R,
|
||||
AngleStatistics.angleMean(2),
|
||||
AngleStatistics.angleResidual(2),
|
||||
@@ -241,7 +247,7 @@ class UnscentedKalmanFilterTest {
|
||||
Matrix<N1, N1> ref = VecBuilder.fill(100);
|
||||
Matrix<N1, N1> u = VecBuilder.fill(0);
|
||||
|
||||
for (int i = 0; i < (2.0 / dt); i++) {
|
||||
for (int i = 0; i < (2.0 / dt); ++i) {
|
||||
observer.predict(u, dt);
|
||||
|
||||
u = discB.solve(ref.minus(discA.times(ref)));
|
||||
@@ -269,4 +275,125 @@ class UnscentedKalmanFilterTest {
|
||||
|
||||
assertTrue(observer.getP().isEqual(P, 1e-9));
|
||||
}
|
||||
|
||||
// Second system, single motor feedforward estimator
|
||||
private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
|
||||
double v = x.get(1, 0);
|
||||
double kV = x.get(2, 0);
|
||||
double kA = x.get(3, 0);
|
||||
|
||||
double V = u.get(0, 0);
|
||||
|
||||
double a = -kV / kA * v + 1.0 / kA * V;
|
||||
return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
|
||||
}
|
||||
|
||||
private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
|
||||
double p = x.get(0, 0);
|
||||
double v = x.get(1, 0);
|
||||
double kV = x.get(2, 0);
|
||||
double kA = x.get(3, 0);
|
||||
double V = u.get(0, 0);
|
||||
|
||||
double a = -kV / kA * v + 1.0 / kA * V;
|
||||
return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
|
||||
}
|
||||
|
||||
private static Matrix<N1, N1> motorControlInput(double t) {
|
||||
return MatBuilder.fill(
|
||||
Nat.N1(),
|
||||
Nat.N1(),
|
||||
MathUtil.clamp(
|
||||
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
|
||||
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
|
||||
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
|
||||
-12.0,
|
||||
12.0));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMotorConvergence() {
|
||||
final double dtSeconds = 0.01;
|
||||
final int steps = 500;
|
||||
final double true_kV = 3;
|
||||
final double true_kA = 0.2;
|
||||
|
||||
final double pos_stddev = 0.02;
|
||||
final double vel_stddev = 0.1;
|
||||
final double accel_stddev = 0.1;
|
||||
|
||||
var states =
|
||||
new ArrayList<>(
|
||||
Collections.nCopies(
|
||||
steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
|
||||
var inputs =
|
||||
new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
|
||||
var measurements =
|
||||
new ArrayList<>(
|
||||
Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
|
||||
states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
|
||||
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N4(),
|
||||
Nat.N4(),
|
||||
0.0,
|
||||
1.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-true_kV / true_kA,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0);
|
||||
var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
|
||||
|
||||
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
inputs.set(i, motorControlInput(i * dtSeconds));
|
||||
states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
|
||||
measurements.set(
|
||||
i,
|
||||
motorMeasurementModel(states.get(i + 1), inputs.get(i))
|
||||
.plus(
|
||||
StateSpaceUtil.makeWhiteNoiseVector(
|
||||
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
|
||||
}
|
||||
|
||||
var P0 =
|
||||
MatBuilder.fill(
|
||||
Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
|
||||
0.0, 0.0, 10);
|
||||
|
||||
var observer =
|
||||
new UnscentedKalmanFilter<N4, N1, N3>(
|
||||
Nat.N4(),
|
||||
Nat.N3(),
|
||||
UnscentedKalmanFilterTest::motorDynamics,
|
||||
UnscentedKalmanFilterTest::motorMeasurementModel,
|
||||
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
|
||||
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
|
||||
dtSeconds);
|
||||
|
||||
observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
|
||||
observer.setP(P0);
|
||||
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
observer.predict(inputs.get(i), dtSeconds);
|
||||
observer.correct(inputs.get(i), measurements.get(i));
|
||||
}
|
||||
|
||||
assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
|
||||
assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2,7 +2,9 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/QR>
|
||||
@@ -12,15 +14,19 @@
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/estimator/AngleStatistics.h"
|
||||
#include "frc/estimator/UnscentedKalmanFilter.h"
|
||||
#include "frc/system/Discretization.h"
|
||||
#include "frc/system/NumericalIntegration.h"
|
||||
#include "frc/system/NumericalJacobian.h"
|
||||
#include "frc/system/plant/DCMotor.h"
|
||||
#include "frc/system/plant/LinearSystemId.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "units/moment_of_inertia.h"
|
||||
|
||||
namespace {
|
||||
|
||||
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
|
||||
// First test system, differential drive
|
||||
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
|
||||
const frc::Vectord<2>& u) {
|
||||
auto motors = frc::DCMotor::CIM(2);
|
||||
|
||||
// constexpr double Glow = 15.32; // Low gear ratio
|
||||
@@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
|
||||
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
|
||||
}
|
||||
|
||||
frc::Vectord<3> LocalMeasurementModel(
|
||||
frc::Vectord<3> DriveLocalMeasurementModel(
|
||||
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
|
||||
return frc::Vectord<3>{x(2), x(3), x(4)};
|
||||
}
|
||||
|
||||
frc::Vectord<5> GlobalMeasurementModel(
|
||||
frc::Vectord<5> DriveGlobalMeasurementModel(
|
||||
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
|
||||
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, Init) {
|
||||
TEST(UnscentedKalmanFilterTest, DriveInit) {
|
||||
constexpr auto dt = 5_ms;
|
||||
|
||||
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
|
||||
LocalMeasurementModel,
|
||||
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
|
||||
DriveLocalMeasurementModel,
|
||||
{0.5, 0.5, 10.0, 1.0, 1.0},
|
||||
{0.0001, 0.01, 0.01},
|
||||
frc::AngleMean<5, 5>(2),
|
||||
@@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) {
|
||||
frc::Vectord<2> u{12.0, 12.0};
|
||||
observer.Predict(u, dt);
|
||||
|
||||
auto localY = LocalMeasurementModel(observer.Xhat(), u);
|
||||
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
|
||||
observer.Correct(u, localY);
|
||||
|
||||
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
|
||||
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
|
||||
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
|
||||
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
|
||||
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
|
||||
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
|
||||
}
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
|
||||
constexpr auto dt = 5_ms;
|
||||
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
|
||||
|
||||
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
|
||||
LocalMeasurementModel,
|
||||
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
|
||||
DriveLocalMeasurementModel,
|
||||
{0.5, 0.5, 10.0, 1.0, 1.0},
|
||||
{0.0001, 0.5, 0.5},
|
||||
frc::AngleMean<5, 5>(2),
|
||||
@@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
frc::Vectord<5> r = frc::Vectord<5>::Zero();
|
||||
frc::Vectord<2> u = frc::Vectord<2>::Zero();
|
||||
|
||||
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
|
||||
frc::Vectord<2>::Zero());
|
||||
auto B = frc::NumericalJacobianU<5, 5, 2>(
|
||||
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
|
||||
|
||||
observer.SetXhat(frc::Vectord<5>{
|
||||
trajectory.InitialPose().Translation().X().value(),
|
||||
@@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
|
||||
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
|
||||
|
||||
auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
|
||||
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
|
||||
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
|
||||
|
||||
frc::Vectord<5> rdot = (nextR - r) / dt.value();
|
||||
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
|
||||
u = B.householderQr().solve(rdot -
|
||||
DriveDynamics(r, frc::Vectord<2>::Zero()));
|
||||
|
||||
observer.Predict(u, dt);
|
||||
|
||||
r = nextR;
|
||||
trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
|
||||
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
|
||||
}
|
||||
|
||||
auto localY = LocalMeasurementModel(trueXhat, u);
|
||||
auto localY = DriveLocalMeasurementModel(trueXhat, u);
|
||||
observer.Correct(u, localY);
|
||||
|
||||
auto globalY = GlobalMeasurementModel(trueXhat, u);
|
||||
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
|
||||
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
|
||||
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
|
||||
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
|
||||
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
|
||||
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
|
||||
|
||||
@@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
|
||||
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
|
||||
}
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, LinearUKF) {
|
||||
constexpr units::second_t dt = 20_ms;
|
||||
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
|
||||
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
|
||||
frc::UnscentedKalmanFilter<1, 1, 1> observer{
|
||||
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
|
||||
return plant.A() * x + plant.B() * u;
|
||||
},
|
||||
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
|
||||
return plant.CalculateY(x, u);
|
||||
},
|
||||
{0.05},
|
||||
{1.0},
|
||||
dt};
|
||||
|
||||
frc::Matrixd<1, 1> discA;
|
||||
frc::Matrixd<1, 1> discB;
|
||||
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
|
||||
|
||||
frc::Vectord<1> ref{100.0};
|
||||
frc::Vectord<1> u{0.0};
|
||||
|
||||
for (int i = 0; i < 2.0 / dt.value(); ++i) {
|
||||
observer.Predict(u, dt);
|
||||
|
||||
u = discB.householderQr().solve(ref - discA * ref);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
|
||||
}
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, RoundTripP) {
|
||||
constexpr auto dt = 5_ms;
|
||||
|
||||
@@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) {
|
||||
|
||||
ASSERT_TRUE(observer.P().isApprox(P));
|
||||
}
|
||||
|
||||
// Second system, single motor feedforward estimator
|
||||
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
|
||||
const frc::Vectord<1>& u) {
|
||||
double v = x(1);
|
||||
double kV = x(2);
|
||||
double kA = x(3);
|
||||
|
||||
double V = u(0);
|
||||
|
||||
double a = -kV / kA * v + 1.0 / kA * V;
|
||||
return frc::Vectord<4>{v, a, 0.0, 0.0};
|
||||
}
|
||||
|
||||
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
|
||||
const frc::Vectord<1>& u) {
|
||||
double p = x(0);
|
||||
double v = x(1);
|
||||
double kV = x(2);
|
||||
double kA = x(3);
|
||||
|
||||
double V = u(0);
|
||||
|
||||
double a = -kV / kA * v + 1.0 / kA * V;
|
||||
return frc::Vectord<3>{p, v, a};
|
||||
}
|
||||
|
||||
frc::Vectord<1> MotorControlInput(double t) {
|
||||
return frc::Vectord<1>{
|
||||
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
|
||||
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
|
||||
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
|
||||
-12.0, 12.0)};
|
||||
}
|
||||
|
||||
TEST(UnscentedKalmanFilterTest, MotorConvergence) {
|
||||
constexpr units::second_t dt = 10_ms;
|
||||
constexpr int steps = 500;
|
||||
constexpr double true_kV = 3;
|
||||
constexpr double true_kA = 0.2;
|
||||
|
||||
constexpr double pos_stddev = 0.02;
|
||||
constexpr double vel_stddev = 0.1;
|
||||
constexpr double accel_stddev = 0.1;
|
||||
|
||||
std::vector<frc::Vectord<4>> states(steps + 1);
|
||||
std::vector<frc::Vectord<1>> inputs(steps);
|
||||
std::vector<frc::Vectord<3>> measurements(steps);
|
||||
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
|
||||
|
||||
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
|
||||
{0.0, -true_kV / true_kA, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0}};
|
||||
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
|
||||
|
||||
frc::Matrixd<4, 4> discA;
|
||||
frc::Matrixd<4, 1> discB;
|
||||
frc::DiscretizeAB(A, B, dt, &discA, &discB);
|
||||
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
inputs[i] = MotorControlInput(i * dt.value());
|
||||
states[i + 1] = discA * states[i] + discB * inputs[i];
|
||||
measurements[i] =
|
||||
MotorMeasurementModel(states[i + 1], inputs[i]) +
|
||||
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
|
||||
}
|
||||
|
||||
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
|
||||
|
||||
frc::UnscentedKalmanFilter<4, 1, 3> observer{
|
||||
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
|
||||
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
|
||||
|
||||
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
|
||||
observer.SetP(P0.asDiagonal());
|
||||
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
observer.Predict(inputs[i], dt);
|
||||
observer.Correct(inputs[i], measurements[i]);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
|
||||
EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
Reference in New Issue
Block a user