From 1efaaefd78a634599b522fc2420c7283722554fb Mon Sep 17 00:00:00 2001 From: Jack Cammarata <50638415+PascalSkylake@users.noreply.github.com> Date: Fri, 14 Mar 2025 13:05:15 -0400 Subject: [PATCH] [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 --- .../estimator/MerweScaledSigmaPoints.java | 6 +- .../math/estimator/UnscentedKalmanFilter.java | 117 +++++++++--- .../frc/estimator/MerweScaledSigmaPoints.h | 4 +- .../frc/estimator/UnscentedKalmanFilter.h | 78 ++++++-- .../frc/estimator/UnscentedTransform.h | 38 +++- .../estimator/UnscentedKalmanFilterTest.java | 173 +++++++++++++++--- .../estimator/UnscentedKalmanFilterTest.cpp | 166 ++++++++++++++--- 7 files changed, 494 insertions(+), 88 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java index 7c378dff47..d3131a83e9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java @@ -69,8 +69,8 @@ public class MerweScaledSigmaPoints { } /** - * 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 { // 2 * states + 1 by states Matrix 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)); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index c64ec85cf9..1bc474b4c1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -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 * chapter 9 "Stochastic control theory". * - *

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. + *

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 Number of states. * @param Number of inputs. @@ -105,7 +105,7 @@ public class UnscentedKalmanFilter 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 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 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 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 getP() { - return m_S.transpose().times(m_S); + return m_S.times(m_S.transpose()); } /** @@ -280,7 +305,7 @@ public class UnscentedKalmanFilter newP) { - m_S = newP.lltDecompose(false); + m_S = newP.lltDecompose(true); } /** @@ -347,14 +372,28 @@ public class UnscentedKalmanFilter 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 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 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 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 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); } } } diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h index ffc65eee85..2605d5e03e 100644 --- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h +++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h @@ -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 U = eta * S; Matrixd sigmas; + + // equation (17) sigmas.template block(0, 0) = x; for (int k = 0; k < States; ++k) { sigmas.template block(0, k + 1) = diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 87ee5ffd29..21838870a8 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -43,7 +43,9 @@ namespace frc { * "Stochastic control theory". * *

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(contA, m_contQ, m_dt, &discA, &discQ); Eigen::internal::llt_inplace::blocked(discQ); + // Generate sigma points around the state mean + // + // equation (17) Matrixd 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(0, i); m_sigmasF.template block(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( m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX, discQ.template triangularView()); @@ -367,7 +383,15 @@ class UnscentedKalmanFilter { Matrixd discR = DiscretizeR(R, m_dt); Eigen::internal::llt_inplace::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 sigmasH; Matrixd sigmas = m_pts.SquareRootSigmaPoints(m_xHat, m_S); @@ -376,16 +400,26 @@ class UnscentedKalmanFilter { h(sigmas.template block(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( sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY, discR.template triangularView()); - // 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 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(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 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 U = K * Sy; + + // Downdate the posterior square-root state covariance + // + // equation (29) for (int i = 0; i < Rows; i++) { - Eigen::internal::llt_inplace::rankUpdate( + Eigen::internal::llt_inplace::rankUpdate( m_S, U.template block(0, i), -1); } } diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h index 4a99e90157..0002638d79 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -47,12 +47,21 @@ SquareRootUnscentedTransform( const Vectord&)> residualFunc, const Matrixd& 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 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 Sbar; for (int i = 0; i < States * 2; i++) { Sbar.template block(0, i) = @@ -61,14 +70,29 @@ SquareRootUnscentedTransform( } Sbar.template block(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 S = Sbar.transpose() .householderQr() .matrixQR() .template block(0, 0) - .template triangularView(); + .template triangularView() + .transpose(); - Eigen::internal::llt_inplace::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::rankUpdate( S, residualFunc(sigmas.template block(0, 0), x), Wc[0]); return std::make_tuple(x, S); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index c5a6cd11a5..c5252a412d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -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 getDynamics(Matrix x, Matrix u) { + private static Matrix driveDynamics(Matrix x, Matrix 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 getLocalMeasurementModel(Matrix x, Matrix u) { + private static Matrix driveLocalMeasurementModel(Matrix x, Matrix u) { return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0)); } @SuppressWarnings("PMD.UnusedFormalParameter") - private static Matrix getGlobalMeasurementModel(Matrix x, Matrix u) { + private static Matrix driveGlobalMeasurementModel(Matrix x, Matrix 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 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 localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1())); + Matrix 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 ref = VecBuilder.fill(100); Matrix 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 motorDynamics(Matrix x, Matrix 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 motorMeasurementModel(Matrix x, Matrix 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 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( + 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); + } } diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp index c2c6f824d8..21d2df98f4 100644 --- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -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 #include +#include #include #include @@ -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( + 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> states(steps + 1); + std::vector> inputs(steps); + std::vector> 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