Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Implement Scaled Spherical Simplex Filter (S3F) (#8091)

Browse Source 
Adds S3SigmaPoints based on MerweScaledSigmaPoints. In addition, restructures UnscentedKalmanFilter to support different sigma point generators and provides MerweUKF and S3UKF for convenience when working with either kind of filter.

S3UKFTest is copied from MerweUKFTest (which is a rename of UnscentedKalmanFilterTest). Curiously, however, in Java the original tolerance used in MerweUKFTest.testDriveConvergence() for the final rotation was too low for S3UKFTest, so the tolerance is increased from 0.000005 (5e-6) radians to 0.00015 (1.5e-4) radians. However, the C++ version still uses the original tolerance. (This difference is probably because Java uses a final rotation of 5.846 degrees while C++ uses a final rotation of 5.846 radians)

Closes #8072.

Breaking changes:

- (C++) UnscentedKalmanFilter has a new template parameter for the sigma point generator type.
- (Java) UnscentedKalmanFilter has an additional parameter to every constructor providing an instance of a sigma point generator.
- (C++) int MerweScaledSigmaPoints.NumSigmas() has been replaced with constexpr int MerweScaledSigmaPoints::NumSigmas.
- (C++) The second parameter of SquareRootUnscentedTransform has been changed from States to NumSigmas.
This commit is contained in:
Joseph Eng
2025-07-15 21:17:25 -07:00
committed by GitHub
parent f03df5388e
commit 1530fccbd0
22 changed files with 1694 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -114,7 +114,7 @@ class ExtendedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));

39
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java → wpimath/src/test/java/edu/wpi/first/math/estimator/MerweUKFTest.java
View File

@@ -33,7 +33,7 @@ import java.util.Collections;
import java.util.List;
import org.junit.jupiter.api.Test;
class UnscentedKalmanFilterTest {
class MerweUKFTest {
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
@@ -78,12 +78,12 @@ class UnscentedKalmanFilterTest {
var dt = 0.005;
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
MerweUKF<N5, N2, N3> observer =
new MerweUKF<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
MerweUKFTest::driveDynamics,
MerweUKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
AngleStatistics.angleMean(2),
@@ -107,7 +107,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
MerweUKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -121,12 +121,12 @@ class UnscentedKalmanFilterTest {
final double dt = 0.005;
final double rb = 0.8382 / 2.0; // Robot radius
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
MerweUKF<N5, N2, N3> observer =
new MerweUKF<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
MerweUKFTest::driveDynamics,
MerweUKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.5, 0.5),
AngleStatistics.angleMean(2),
@@ -139,7 +139,7 @@ class UnscentedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
@@ -150,7 +150,7 @@ class UnscentedKalmanFilterTest {
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
UnscentedKalmanFilterTest::driveDynamics,
MerweUKFTest::driveDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
@@ -190,8 +190,7 @@ class UnscentedKalmanFilterTest {
observer.predict(u, dt);
r = nextR;
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt);
trueXhat = NumericalIntegration.rk4(MerweUKFTest::driveDynamics, trueXhat, u, dt);
}
var localY = driveLocalMeasurementModel(trueXhat, u);
@@ -205,7 +204,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
MerweUKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -226,7 +225,7 @@ class UnscentedKalmanFilterTest {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer =
new UnscentedKalmanFilter<>(
new MerweUKF<>(
Nat.N1(),
Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
@@ -256,7 +255,7 @@ class UnscentedKalmanFilterTest {
var dt = 0.005;
var observer =
new UnscentedKalmanFilter<>(
new MerweUKF<>(
Nat.N2(),
Nat.N2(),
(x, u) -> x,
@@ -371,11 +370,11 @@ class UnscentedKalmanFilterTest {
0.0, 0.0, 10);
var observer =
new UnscentedKalmanFilter<N4, N1, N3>(
new MerweUKF<N4, N1, N3>(
Nat.N4(),
Nat.N3(),
UnscentedKalmanFilterTest::motorDynamics,
UnscentedKalmanFilterTest::motorMeasurementModel,
MerweUKFTest::motorDynamics,
MerweUKFTest::motorMeasurementModel,
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
dt);

86
wpimath/src/test/java/edu/wpi/first/math/estimator/S3SigmaPointsTest.java Normal file
View File

@@ -0,0 +1,86 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertAll;
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.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N2;
import org.junit.jupiter.api.Test;
class S3SigmaPointsTest {
@Test
void testSimplex() {
double alpha = 1e-3;
double beta = 2;
Nat<N2> N = Nat.N2();
var sigmaPoints = new S3SigmaPoints<>(N, alpha, beta);
var points = sigmaPoints.squareRootSigmaPoints(new Vector<>(N), Matrix.eye(N));
var v1 = new Vector<>(points.extractColumnVector(1));
var v2 = new Vector<>(points.extractColumnVector(2));
var v3 = new Vector<>(points.extractColumnVector(3));
assertAll(
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v1.norm(), 1e-15),
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v2.norm(), 1e-15),
() -> assertEquals(alpha * Math.sqrt(N.getNum()), v3.norm(), 1e-15),
() -> assertEquals(v1.minus(v2).norm(), v1.minus(v3).norm(), 1e-15),
() -> assertEquals(v1.minus(v2).norm(), v2.minus(v3).norm(), 1e-15));
}
@Test
void testZeroMeanPoints() {
var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
var points =
sigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
assertTrue(
points.isEqual(
MatBuilder.fill(
Nat.N2(),
Nat.N4(),
0.0,
-0.00122474,
0.00122474,
0.0,
0.0,
-0.00070711,
-0.00070711,
0.00141421),
1E-6));
}
@Test
void testNonzeroMeanPoints() {
var sigmaPoints = new S3SigmaPoints<>(Nat.N2());
var points =
sigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
assertTrue(
points.isEqual(
MatBuilder.fill(
Nat.N2(),
Nat.N4(),
1.0,
0.99877526,
1.00122474,
1.0,
2.0,
1.99776393,
1.99776393,
2.00447214),
1E-6));
}
}

393
wpimath/src/test/java/edu/wpi/first/math/estimator/S3UKFTest.java Normal file
View File

@@ -0,0 +1,393 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
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;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
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;
import edu.wpi.first.math.system.NumericalJacobian;
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 S3UKFTest {
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
// var gLow = 15.32; // Low gear ratio
var gHigh = 7.08; // High gear ratio
var rb = 0.8382 / 2.0; // Robot radius
var r = 0.0746125; // Wheel radius
var m = 63.503; // Robot mass
var J = 5.6; // Robot moment of inertia
var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
var C2 = gHigh * motors.Kt / (motors.R * r);
var k1 = 1.0 / m + Math.pow(rb, 2) / J;
var k2 = 1.0 / m - Math.pow(rb, 2) / J;
var vl = x.get(3, 0);
var vr = x.get(4, 0);
var Vl = u.get(0, 0);
var Vr = u.get(1, 0);
var v = 0.5 * (vl + vr);
return VecBuilder.fill(
v * Math.cos(x.get(2, 0)),
v * Math.sin(x.get(2, 0)),
(vr - vl) / (2.0 * rb),
k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
void testDriveInit() {
var dt = 0.005;
assertDoesNotThrow(
() -> {
S3UKF<N5, N2, N3> observer =
new S3UKF<>(
Nat.N5(),
Nat.N3(),
S3UKFTest::driveDynamics,
S3UKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dt);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dt);
var localY = driveLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
observer.correct(
Nat.N5(),
u,
globalY,
S3UKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
});
}
@Test
void testDriveConvergence() {
final double dt = 0.005;
final double rb = 0.8382 / 2.0; // Robot radius
S3UKF<N5, N2, N3> observer =
new S3UKF<>(
Nat.N5(),
Nat.N3(),
S3UKFTest::driveDynamics,
S3UKFTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.5, 0.5),
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dt);
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromRadians(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
var B =
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
S3UKFTest::driveDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
observer.setXhat(
VecBuilder.fill(
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0,
0.0));
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / dt); ++i) {
var ref = trajectory.sample(dt * i);
double vl = ref.velocity * (1 - (ref.curvature * rb));
double vr = ref.velocity * (1 + (ref.curvature * rb));
var nextR =
VecBuilder.fill(
ref.pose.getTranslation().getX(),
ref.pose.getTranslation().getY(),
ref.pose.getRotation().getRadians(),
vl,
vr);
Matrix<N3, N1> localY =
driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dt);
r = nextR;
trueXhat = NumericalIntegration.rk4(S3UKFTest::driveDynamics, trueXhat, u, dt);
}
var localY = driveLocalMeasurementModel(trueXhat, u);
observer.correct(u, localY);
var globalY = driveGlobalMeasurementModel(trueXhat, u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N5(),
u,
globalY,
S3UKFTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
final var finalPosition = trajectory.sample(trajectory.getTotalTime());
assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055);
assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15);
assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.00015);
assertEquals(0.0, observer.getXhat(3), 0.1);
assertEquals(0.0, observer.getXhat(4), 0.1);
}
@Test
void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer =
new S3UKF<>(
Nat.N1(),
Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
plant::calculateY,
VecBuilder.fill(0.05),
VecBuilder.fill(1.0),
dt);
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
Matrix<N1, N1> ref = VecBuilder.fill(100);
Matrix<N1, N1> u = VecBuilder.fill(0);
for (int i = 0; i < (2.0 / dt); ++i) {
observer.predict(u, dt);
u = discB.solve(ref.minus(discA.times(ref)));
}
assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
}
@Test
void testRoundTripP() {
var dt = 0.005;
var observer =
new S3UKF<>(
Nat.N2(),
Nat.N2(),
(x, u) -> x,
(x, u) -> x,
VecBuilder.fill(0.0, 0.0),
VecBuilder.fill(0.0, 0.0),
dt);
var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
observer.setP(P);
assertTrue(observer.getP().isEqual(P, 1e-9));
}
// Second system, single motor feedforward estimator
private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
}
private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double p = x.get(0, 0);
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
}
private static Matrix<N1, N1> motorControlInput(double t) {
return MatBuilder.fill(
Nat.N1(),
Nat.N1(),
MathUtil.clamp(
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
-12.0,
12.0));
}
@Test
void testMotorConvergence() {
final double dt = 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, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
for (int i = 0; i < steps; ++i) {
inputs.set(i, motorControlInput(i * dt));
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 S3UKF<N4, N1, N3>(
Nat.N4(),
Nat.N3(),
S3UKFTest::motorDynamics,
S3UKFTest::motorMeasurementModel,
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
dt);
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), dt);
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);
}
}

58
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp → wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -13,7 +13,7 @@
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/MerweUKF.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(UnscentedKalmanFilterTest, DriveInit) {
TEST(MerweUKFTest, DriveInit) {
constexpr auto dt = 5_ms;
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),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
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::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -93,20 +93,20 @@ TEST(UnscentedKalmanFilterTest, DriveInit) {
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
TEST(MerweUKFTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
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),
frc::AngleMean<3, 5>(0),
frc::AngleResidual<5>(2),
frc::AngleResidual<3>(0),
frc::AngleAdd<5>(2),
dt};
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};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
@@ -174,11 +174,11 @@ TEST(UnscentedKalmanFilterTest, DriveConvergence) {
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(UnscentedKalmanFilterTest, LinearUKF) {
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::UnscentedKalmanFilter<1, 1, 1> observer{
frc::MerweUKF<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(UnscentedKalmanFilterTest, LinearUKF) {
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
TEST(MerweUKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<2, 2, 2> observer{
frc::MerweUKF<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(UnscentedKalmanFilterTest, MotorConvergence) {
TEST(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
@@ -290,7 +290,7 @@ TEST(UnscentedKalmanFilterTest, MotorConvergence) {
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::UnscentedKalmanFilter<4, 1, 3> observer{
frc::MerweUKF<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};

50
wpimath/src/test/native/cpp/estimator/S3SigmaPointsTest.cpp Normal file
View File

@@ -0,0 +1,50 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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 <gtest/gtest.h>
#include "frc/estimator/S3SigmaPoints.h"
TEST(S3SigmaPointsTest, Simplex) {
constexpr double alpha = 1e-3;
constexpr double beta = 2;
constexpr size_t N = 2;
frc::S3SigmaPoints<N> sigmaPoints{alpha, beta};
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<N>::Zero(), frc::Matrixd<N, N>::Identity());
auto v1 = points.template block<2, 1>(0, 1);
auto v2 = points.template block<2, 1>(0, 2);
auto v3 = points.template block<2, 1>(0, 3);
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v1.norm());
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v2.norm());
EXPECT_DOUBLE_EQ(alpha * std::sqrt(N), v3.norm());
EXPECT_DOUBLE_EQ((v1 - v2).norm(), (v1 - v3).norm());
EXPECT_DOUBLE_EQ((v1 - v2).norm(), (v2 - v3).norm());
}
TEST(S3SigmaPointsTest, ZeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{0.0, -0.00122474, 0.00122474, 0.0},
{0.0, -0.00070711, -0.00070711, 0.00141421}})
.norm() < 1e-7);
}
TEST(S3SigmaPointsTest, NonzeroMean) {
frc::S3SigmaPoints<2> sigmaPoints;
auto points = sigmaPoints.SquareRootSigmaPoints(
frc::Vectord<2>{1.0, 2.0},
frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
EXPECT_TRUE(
(points - frc::Matrixd<2, 4>{{1.0, 0.99877526, 1.00122474, 1.0},
{2.0, 1.99776393, 1.99776393, 2.00447214}})
.norm() < 1e-7);
}

309
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp Normal file
View File

@@ -0,0 +1,309 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <algorithm>
#include <cmath>
#include <numbers>
#include <vector>
#include <Eigen/QR>
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/MerweUKF.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 {
// 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
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
constexpr auto m = 63.503_kg; // Robot mass
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
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> 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)};
}
TEST(MerweUKFTest, 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::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
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::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(MerweUKFTest, 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};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = 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(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
auto trueXhat = observer.Xhat();
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).value());
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
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 -
DriveDynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
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::AngleResidual<5>(2), frc::AngleAdd<5>(2)
);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
0.055);
EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
0.15);
EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
0.000005);
EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(MerweUKFTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::MerweUKF<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(MerweUKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
frc::MerweUKF<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},
{0.0, 0.0},
dt};
frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
observer.SetP(P);
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(MerweUKFTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
constexpr double pos_stddev = 0.02;
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::MerweUKF<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