diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java index 96a9875d3f..a0ea245770 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java @@ -4,134 +4,145 @@ package org.wpilib.math.estimator; -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import java.util.List; -import java.util.Random; import org.junit.jupiter.api.Test; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.numbers.N1; -import org.wpilib.math.numbers.N2; -import org.wpilib.math.numbers.N3; -import org.wpilib.math.numbers.N6; -import org.wpilib.math.system.DCMotor; +import org.wpilib.math.random.Normal; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.Nat; class KalmanFilterTest { - private static LinearSystem elevatorPlant; - - private static final double kDt = 0.005; - - static { - createElevator(); - } - - private static void createElevator() { - var motors = DCMotor.getVex775Pro(2); - - var m = 5.0; - var r = 0.0181864; - var G = 1.0; - elevatorPlant = Models.elevatorFromPhysicalConstants(motors, m, r, G); - } - - // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ, - // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ - LinearSystem m_swerveObserverSystem = - new LinearSystem<>( - MatBuilder.fill( - Nat.N6(), Nat.N6(), // A - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0), - MatBuilder.fill( - Nat.N6(), Nat.N3(), // B - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), - MatBuilder.fill( - Nat.N3(), Nat.N6(), // C - 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0), - new Matrix<>(Nat.N3(), Nat.N3())); // D - @Test - @SuppressWarnings("unchecked") - void testElevatorKalmanFilter() { - var Q = VecBuilder.fill(0.05, 1.0); - var R = VecBuilder.fill(0.0001); + void testSwerveStationary() { + final var dt = 0.02; - assertDoesNotThrow( - () -> - new KalmanFilter<>( - Nat.N2(), Nat.N1(), (LinearSystem) elevatorPlant.slice(0), Q, R, kDt)); - } - - @Test - void testSwerveKFStationary() { - var random = new Random(); + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + var plant = + new LinearSystem<>( + MatBuilder.fill( + Nat.N6(), + Nat.N6(), + // spotless:off + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0), + // spotless:on + MatBuilder.fill( + Nat.N6(), + Nat.N3(), + // spotless:off + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1), + // spotless:on + MatBuilder.fill( + Nat.N3(), + Nat.N6(), + // spotless:off + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0), + // spotless:on + new Matrix<>(Nat.N3(), Nat.N3())); var filter = new KalmanFilter<>( Nat.N6(), Nat.N3(), - m_swerveObserverSystem, - VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state - // weights - VecBuilder.fill(2, 2, 2), // measurement weights - 0.020); + plant, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(1.0, 1.0, 1.0), + dt); - Matrix measurement; - for (int i = 0; i < 100; i++) { - // the robot is at [0, 0, 0] so we just park here - measurement = - VecBuilder.fill(random.nextGaussian(), random.nextGaussian(), random.nextGaussian()); - filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement); + var u = VecBuilder.fill(0.0, 0.0, 0.0); - // we continue to not accelerate - filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020); + for (int i = 0; i < 100; ++i) { + var y = Normal.normal(VecBuilder.fill(1.0, 1.0, 1.0)); + + filter.correct(u, y); + filter.predict(u, dt); } assertEquals(0.0, filter.getXhat(0), 0.3); - assertEquals(0.0, filter.getXhat(0), 0.3); + assertEquals(0.0, filter.getXhat(1), 0.3); } @Test - void testSwerveKFMovingWithoutAccelerating() { - var random = new Random(); + void testSwerveBadInitialPose() { + final var dt = 0.02; + + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + var plant = + new LinearSystem<>( + MatBuilder.fill( + Nat.N6(), + Nat.N6(), + // spotless:off + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0), + // spotless:on + MatBuilder.fill( + Nat.N6(), + Nat.N3(), + // spotless:off + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1), + // spotless:on + MatBuilder.fill( + Nat.N3(), + Nat.N6(), + // spotless:off + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0), + // spotless:on + new Matrix<>(Nat.N3(), Nat.N3())); var filter = new KalmanFilter<>( Nat.N6(), Nat.N3(), - m_swerveObserverSystem, - VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state - // weights - VecBuilder.fill(4, 4, 4), // measurement weights - 0.020); + plant, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.1, 0.1, 0.25), + dt); - // we set the velocity of the robot so that it's moving forward slowly + // Set nonzero position filter.setXhat(0, 0.5); filter.setXhat(1, 0.5); - for (int i = 0; i < 300; i++) { - // the robot is at [0, 0, 0] so we just park here - var measurement = - VecBuilder.fill( - random.nextGaussian() / 10d, - random.nextGaussian() / 10d, - random.nextGaussian() / 4d // std dev of [1, 1, 1] - ); + var u = VecBuilder.fill(0.0, 0.0, 0.0); - filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement); + // Let filter converge to zero position + for (int i = 0; i < 300; ++i) { + var y = Normal.normal(VecBuilder.fill(0.1, 0.1, 0.25)); - // we continue to not accelerate - filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020); + filter.correct(u, y); + filter.predict(u, dt); } assertEquals(0.0, filter.getXhat(0), 0.2); @@ -139,46 +150,85 @@ class KalmanFilterTest { } @Test - void testSwerveKFMovingOverTrajectory() { - var random = new Random(); + void testSwerveMovingOverTrajectory() { + final var dt = 0.02; + + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + var plant = + new LinearSystem<>( + MatBuilder.fill( + Nat.N6(), + Nat.N6(), + // spotless:off + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0), + // spotless:on + MatBuilder.fill( + Nat.N6(), + Nat.N3(), + // spotless:off + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1), + // spotless:on + MatBuilder.fill( + Nat.N3(), + Nat.N6(), + // spotless:off + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0), + // spotless:on + new Matrix<>(Nat.N3(), Nat.N3())); var filter = new KalmanFilter<>( Nat.N6(), Nat.N3(), - m_swerveObserverSystem, - VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state - // weights - VecBuilder.fill(4, 4, 4), // measurement weights - 0.020); + plant, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.2, 0.2, 1.0 / 3.0), + dt); var trajectory = TrajectoryGenerator.generateTrajectory( - List.of(new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(5, 5, Rotation2d.kZero)), - new TrajectoryConfig(2, 2)); - var time = 0.0; + List.of(new Pose2d(0.0, 0.0, Rotation2d.kZero), new Pose2d(5.0, 5.0, Rotation2d.kZero)), + new TrajectoryConfig(2.0, 2.0)); + var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0); - while (time <= trajectory.getTotalTime()) { - var sample = trajectory.sample(time); - var measurement = + for (var t = 0.0; t < trajectory.getTotalTime(); t += dt) { + var sample = trajectory.sample(t); + + var y = VecBuilder.fill( - sample.pose.getTranslation().getX() + random.nextGaussian() / 5d, - sample.pose.getTranslation().getY() + random.nextGaussian() / 5d, - sample.pose.getRotation().getRadians() + random.nextGaussian() / 3d); + sample.pose.getTranslation().getX(), + sample.pose.getTranslation().getY(), + sample.pose.getRotation().getRadians()); + var noise = Normal.normal(VecBuilder.fill(0.2, 0.2, 1.0 / 3.0)); + y.set(0, 0, y.get(0, 0) + noise.get(0, 0)); + y.set(1, 0, y.get(1, 0) + noise.get(1, 0)); + y.set(2, 0, y.get(2, 0) + noise.get(2, 0)); var velocity = VecBuilder.fill( sample.velocity * sample.pose.getRotation().getCos(), sample.velocity * sample.pose.getRotation().getSin(), sample.curvature * sample.velocity); - var u = (velocity.minus(lastVelocity)).div(0.020); + var u = velocity.minus(lastVelocity).div(dt); + + filter.correct(u, y); + filter.predict(u, dt); + lastVelocity = velocity; - - filter.correct(u, measurement); - filter.predict(u, 0.020); - - time += 0.020; } assertEquals( diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp index bdf46e0a14..c1d0aaed7f 100644 --- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp @@ -7,14 +7,136 @@ #include #include -#include "wpi/math/system/DCMotor.hpp" -#include "wpi/math/system/Models.hpp" -#include "wpi/units/moment_of_inertia.hpp" +#include "wpi/math/random/Normal.hpp" +#include "wpi/math/system/LinearSystem.hpp" +#include "wpi/math/trajectory/TrajectoryConfig.hpp" +#include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/units/time.hpp" -TEST(KalmanFilterTest, Flywheel) { - auto motor = wpi::math::DCMotor::NEO(); - auto flywheel = - wpi::math::Models::FlywheelFromPhysicalConstants(motor, 1_kg_sq_m, 1.0); - wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms}; +TEST(KalmanFilterTest, SwerveStationary) { + constexpr wpi::units::second_t dt = 20_ms; + + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + wpi::math::LinearSystem<6, 3, 3> plant{ + Eigen::Matrix{{0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}}, + Eigen::Matrix{ + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, + Eigen::Matrix{ + {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}}, + Eigen::Matrix::Zero()}; + + wpi::math::KalmanFilter<6, 3, 3> filter{ + plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}, dt}; + + constexpr Eigen::Vector u{{0.0}, {0.0}, {0.0}}; + + for (int i = 0; i < 100; ++i) { + auto y = wpi::math::Normal(1.0, 1.0, 1.0); + + filter.Correct(u, y); + filter.Predict(u, dt); + } + + EXPECT_NEAR(0.0, filter.Xhat(0), 0.3); + EXPECT_NEAR(0.0, filter.Xhat(1), 0.3); +} + +TEST(KalmanFilterTest, SwerveBadInitialPose) { + constexpr wpi::units::second_t dt = 20_ms; + + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + wpi::math::LinearSystem<6, 3, 3> plant{ + Eigen::Matrix{{0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}}, + Eigen::Matrix{ + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, + Eigen::Matrix{ + {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}}, + Eigen::Matrix::Zero()}; + + wpi::math::KalmanFilter<6, 3, 3> filter{ + plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {0.1, 0.1, 0.25}, dt}; + + // Set nonzero position + filter.SetXhat(0, 0.5); + filter.SetXhat(1, 0.5); + + constexpr Eigen::Vector u{{0.0}, {0.0}, {0.0}}; + + // Let filter converge to zero position + for (int i = 0; i < 300; ++i) { + auto y = wpi::math::Normal(0.1, 0.1, 0.25); + + filter.Correct(u, y); + filter.Predict(u, dt); + } + + EXPECT_NEAR(0.0, filter.Xhat(0), 0.2); + EXPECT_NEAR(0.0, filter.Xhat(1), 0.2); +} + +TEST(KalmanFilterTest, SwerveMovingOverTrajectory) { + constexpr wpi::units::second_t dt = 20_ms; + + // Swerve drive with x = [x, y, θ, v_x, v_y, ω]ᵀ, u = [a_x, a_y, α]ᵀ, + // y = [x, y, θ]ᵀ + wpi::math::LinearSystem<6, 3, 3> plant{ + Eigen::Matrix{{0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}}, + Eigen::Matrix{ + {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, + Eigen::Matrix{ + {1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}}, + Eigen::Matrix::Zero()}; + + wpi::math::KalmanFilter<6, 3, 3> filter{ + plant, {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, {0.2, 0.2, 1.0 / 3.0}, dt}; + + auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory( + {wpi::math::Pose2d{0_m, 0_m, 0_rad}, wpi::math::Pose2d{5_m, 5_m, 0_rad}}, + wpi::math::TrajectoryConfig{2_mps, 2_mps_sq}); + + Eigen::Vector3d lastVelocity{{0.0}, {0.0}, {0.0}}; + + for (wpi::units::second_t t = 0_s; t < trajectory.TotalTime(); t += dt) { + auto sample = trajectory.Sample(t); + + Eigen::Vector3d y{sample.pose.Translation().X().value(), + sample.pose.Translation().Y().value(), + sample.pose.Rotation().Radians().value()}; + y += wpi::math::Normal(0.2, 0.2, 1.0 / 3.0); + + Eigen::Vector3d velocity{ + (sample.velocity * sample.pose.Rotation().Cos()).value(), + (sample.velocity * sample.pose.Rotation().Sin()).value(), + (sample.curvature * sample.velocity).value()}; + Eigen::Vector3d u = (velocity - lastVelocity) / dt.value(); + + filter.Correct(u, y); + filter.Predict(u, dt); + + lastVelocity = velocity; + } + + EXPECT_NEAR( + trajectory.Sample(trajectory.TotalTime()).pose.Translation().X().value(), + filter.Xhat(0), 0.2); + EXPECT_NEAR( + trajectory.Sample(trajectory.TotalTime()).pose.Translation().Y().value(), + filter.Xhat(1), 0.2); }