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] Fix UnscentedKalmanFilter and improve math docs (#7850)

Browse Source 
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 <calcmogul@gmail.com>
This commit is contained in:
Jack Cammarata
2025-03-14 13:05:15 -04:00
committed by GitHub
parent 71b6e8ec58
commit 1efaaefd78
7 changed files with 494 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -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<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
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
@@ -63,17 +67,17 @@ class UnscentedKalmanFilterTest {
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
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> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> 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<N5, N2, N3> 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<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
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(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<N1, N1> ref = VecBuilder.fill(100);
Matrix<N1, N1> 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<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 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<N4, N1, N3>(
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);
}
}

166
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -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 <algorithm>
#include <cmath>
#include <numbers>
#include <vector>
#include <Eigen/QR>
@@ -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<units::meters>(
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<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::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