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