diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h index de0de37e96..8b94313ba7 100644 --- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -82,15 +82,14 @@ AngleAdd(int angleStateIdx) { * * @tparam CovDim Dimension of covariance of sigma points after passing through * the transform. - * @tparam States Number of states. + * @tparam NumSigmas Number of sigma points. * @param sigmas Sigma points. * @param Wm Weights for the mean. * @param angleStatesIdx The row containing the angles. */ -template -Vectord AngleMean(const Matrixd& sigmas, - const Vectord<2 * States + 1>& Wm, - int angleStatesIdx) { +template +Vectord AngleMean(const Matrixd& sigmas, + const Vectord& Wm, int angleStatesIdx) { double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) { return std::sin(it); }) * @@ -113,15 +112,15 @@ Vectord AngleMean(const Matrixd& sigmas, * * @tparam CovDim Dimension of covariance of sigma points after passing through * the transform. - * @tparam States Number of states. + * @tparam NumSigmas Number of sigma points. * @param angleStateIdx The row containing the angles. */ -template -std::function(const Matrixd&, - const Vectord<2 * States + 1>&)> +template +std::function(const Matrixd&, + const Vectord&)> AngleMean(int angleStateIdx) { return [=](auto sigmas, auto Wm) { - return AngleMean(sigmas, Wm, angleStateIdx); + return AngleMean(sigmas, Wm, angleStateIdx); }; } diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h index 1b5b5cfa1f..0cd1af63a5 100644 --- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h +++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h @@ -58,13 +58,13 @@ class MerweScaledSigmaPoints { * Xi_0, Xi_{1..n}, Xi_{n+1..2n}. * */ - Matrixd SquareRootSigmaPoints( + Matrixd SquareRootSigmaPoints( const Vectord& x, const Matrixd& S) { double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; double eta = std::sqrt(lambda + States); Matrixd U = eta * S; - Matrixd sigmas; + Matrixd sigmas; // equation (17) sigmas.template block(0, 0) = x; @@ -81,7 +81,7 @@ class MerweScaledSigmaPoints { /** * Returns the weight for each sigma point for the mean. */ - const Vectord<2 * States + 1>& Wm() const { return m_Wm; } + const Vectord& Wm() const { return m_Wm; } /** * Returns an element of the weight for each sigma point for the mean. @@ -93,7 +93,7 @@ class MerweScaledSigmaPoints { /** * Returns the weight for each sigma point for the covariance. */ - const Vectord<2 * States + 1>& Wc() const { return m_Wc; } + const Vectord& Wc() const { return m_Wc; } /** * Returns an element of the weight for each sigma point for the covariance. @@ -103,8 +103,8 @@ class MerweScaledSigmaPoints { double Wc(int i) const { return m_Wc(i, 0); } private: - Vectord<2 * States + 1> m_Wm; - Vectord<2 * States + 1> m_Wc; + Vectord m_Wm; + Vectord m_Wc; double m_alpha; int m_kappa; @@ -117,8 +117,8 @@ class MerweScaledSigmaPoints { double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; double c = 0.5 / (States + lambda); - m_Wm = Vectord<2 * States + 1>::Constant(c); - m_Wc = Vectord<2 * States + 1>::Constant(c); + m_Wm = Vectord::Constant(c); + m_Wc = Vectord::Constant(c); m_Wm(0) = lambda / (States + lambda); m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta); diff --git a/wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h index a19cd072ac..7b9efebf5c 100644 --- a/wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h +++ b/wpimath/src/main/native/include/frc/estimator/S3SigmaPoints.h @@ -58,7 +58,7 @@ class S3SigmaPoints { Matrixd SquareRootSigmaPoints( const Vectord& x, const Matrixd& S) const { // table (1), equation (12) - wpi::array q(wpi::empty_array); + wpi::array q{wpi::empty_array}; for (size_t t = 1; t <= States; ++t) { q[t - 1] = m_alpha * std::sqrt(static_cast(t * (States + 1)) / static_cast(t + 1)); diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp index 1c4bdaa58b..747223a6b8 100644 --- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -19,7 +19,7 @@ TEST(AngleStatisticsTest, Mean) { weights.fill(1.0 / sigmas.cols()); EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1) - .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3)); + .isApprox(frc::AngleMean<3, 3>(sigmas, weights, 1), 1e-3)); } TEST(AngleStatisticsTest, Mean_DynamicSize) { diff --git a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp index 649b514ba6..0c3aa23e7c 100644 --- a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp @@ -74,8 +74,8 @@ TEST(MerweUKFTest, DriveInit) { DriveLocalMeasurementModel, {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.01, 0.01}, - frc::AngleMean<5, 5>(2), - frc::AngleMean<3, 5>(0), + frc::AngleMean<5, 2 * 5 + 1>(2), + frc::AngleMean<3, 2 * 5 + 1>(0), frc::AngleResidual<5>(2), frc::AngleResidual<3>(0), frc::AngleAdd<5>(2), @@ -89,7 +89,7 @@ TEST(MerweUKFTest, DriveInit) { 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, DriveGlobalMeasurementModel, R, - frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); } @@ -101,8 +101,8 @@ TEST(MerweUKFTest, DriveConvergence) { DriveLocalMeasurementModel, {0.5, 0.5, 10.0, 1.0, 1.0}, {0.0001, 0.5, 0.5}, - frc::AngleMean<5, 5>(2), - frc::AngleMean<3, 5>(0), + frc::AngleMean<5, 2 * 5 + 1>(2), + frc::AngleMean<3, 2 * 5 + 1>(0), frc::AngleResidual<5>(2), frc::AngleResidual<3>(0), frc::AngleAdd<5>(2), @@ -158,7 +158,7 @@ TEST(MerweUKFTest, DriveConvergence) { auto globalY = DriveGlobalMeasurementModel(trueXhat, u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R, - frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleMean<5, 2 * 5 + 1>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2) ); diff --git a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp b/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp index 649b514ba6..fb38c0f3d5 100644 --- a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp @@ -13,7 +13,7 @@ #include "frc/EigenCore.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/AngleStatistics.h" -#include "frc/estimator/MerweUKF.h" +#include "frc/estimator/S3UKF.h" #include "frc/system/Discretization.h" #include "frc/system/NumericalIntegration.h" #include "frc/system/NumericalJacobian.h" @@ -67,19 +67,19 @@ frc::Vectord<5> DriveGlobalMeasurementModel( return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)}; } -TEST(MerweUKFTest, DriveInit) { +TEST(S3UKFTest, DriveInit) { constexpr auto dt = 5_ms; - frc::MerweUKF<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), - frc::AngleMean<3, 5>(0), - frc::AngleResidual<5>(2), - frc::AngleResidual<3>(0), - frc::AngleAdd<5>(2), - dt}; + frc::S3UKF<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>(2), + frc::AngleMean<3, 5 + 2>(0), + frc::AngleResidual<5>(2), + frc::AngleResidual<3>(0), + frc::AngleAdd<5>(2), + dt}; frc::Vectord<2> u{12.0, 12.0}; observer.Predict(u, dt); @@ -89,24 +89,24 @@ TEST(MerweUKFTest, DriveInit) { 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, DriveGlobalMeasurementModel, R, - frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); } -TEST(MerweUKFTest, DriveConvergence) { +TEST(S3UKFTest, DriveConvergence) { constexpr auto dt = 5_ms; constexpr auto rb = 0.8382_m / 2.0; // Robot radius - frc::MerweUKF<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), - frc::AngleMean<3, 5>(0), - frc::AngleResidual<5>(2), - frc::AngleResidual<3>(0), - frc::AngleAdd<5>(2), - dt}; + frc::S3UKF<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>(2), + frc::AngleMean<3, 5 + 2>(0), + frc::AngleResidual<5>(2), + frc::AngleResidual<3>(0), + frc::AngleAdd<5>(2), + dt}; auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, @@ -158,7 +158,7 @@ TEST(MerweUKFTest, DriveConvergence) { auto globalY = DriveGlobalMeasurementModel(trueXhat, u); auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R, - frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2), + frc::AngleMean<5, 5 + 2>(2), frc::AngleResidual<5>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2) ); @@ -169,16 +169,16 @@ TEST(MerweUKFTest, DriveConvergence) { EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1), 0.15); EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2), - 0.000005); + 0.00015); EXPECT_NEAR(0.0, observer.Xhat(3), 0.1); EXPECT_NEAR(0.0, observer.Xhat(4), 0.1); } -TEST(MerweUKFTest, LinearUKF) { +TEST(S3UKFTest, 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::MerweUKF<1, 1, 1> observer{ + frc::S3UKF<1, 1, 1> observer{ [&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { return plant.A() * x + plant.B() * u; }, @@ -205,10 +205,10 @@ TEST(MerweUKFTest, LinearUKF) { EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5); } -TEST(MerweUKFTest, RoundTripP) { +TEST(S3UKFTest, RoundTripP) { constexpr auto dt = 5_ms; - frc::MerweUKF<2, 2, 2> observer{ + frc::S3UKF<2, 2, 2> observer{ [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; }, [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; }, {0.0, 0.0}, @@ -255,7 +255,7 @@ frc::Vectord<1> MotorControlInput(double t) { -12.0, 12.0)}; } -TEST(MerweUKFTest, MotorConvergence) { +TEST(S3UKFTest, MotorConvergence) { constexpr units::second_t dt = 10_ms; constexpr int steps = 500; constexpr double true_kV = 3; @@ -290,7 +290,7 @@ TEST(MerweUKFTest, MotorConvergence) { frc::Vectord<4> P0{0.001, 0.001, 10, 10}; - frc::MerweUKF<4, 1, 3> observer{ + frc::S3UKF<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};