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

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -26,7 +26,7 @@ namespace {
// First test system, differential drive
wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<2>& u) {
auto motors = wpi::math::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -58,12 +58,14 @@ wpi::math::Vectord<5> DriveDynamics(const wpi::math::Vectord<5>& x,
}
wpi::math::Vectord<3> DriveLocalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<3>{x(2), x(3), x(4)};
}
wpi::math::Vectord<5> DriveGlobalMeasurementModel(
const wpi::math::Vectord<5>& x, [[maybe_unused]] const wpi::math::Vectord<2>& u) {
const wpi::math::Vectord<5>& x,
[[maybe_unused]] const wpi::math::Vectord<2>& u) {
return wpi::math::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
@@ -71,15 +73,15 @@ TEST(S3UKFTest, DriveInit) {
constexpr auto dt = 5_ms;
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
wpi::math::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
@@ -88,9 +90,10 @@ TEST(S3UKFTest, DriveInit) {
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
observer.Correct<5>(
u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2));
}
TEST(S3UKFTest, DriveConvergence) {
@@ -98,19 +101,19 @@ TEST(S3UKFTest, DriveConvergence) {
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
wpi::math::S3UKF<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleMean<3, 5 + 2>(0),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<3>(0),
wpi::math::AngleAdd<5>(2),
dt};
auto waypoints =
std::vector<wpi::math::Pose2d>{wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto waypoints = std::vector<wpi::math::Pose2d>{
wpi::math::Pose2d{2.75_m, 22.521_m, 0_rad},
wpi::math::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
@@ -118,7 +121,8 @@ TEST(S3UKFTest, DriveConvergence) {
wpi::math::Vectord<2> u = wpi::math::Vectord<2>::Zero();
auto B = wpi::math::NumericalJacobianU<5, 5, 2>(
DriveDynamics, wpi::math::Vectord<5>::Zero(), wpi::math::Vectord<2>::Zero());
DriveDynamics, wpi::math::Vectord<5>::Zero(),
wpi::math::Vectord<2>::Zero());
observer.SetXhat(wpi::math::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -139,12 +143,14 @@ TEST(S3UKFTest, DriveConvergence) {
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
auto localY =
DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
u = B.householderQr().solve(
rdot - DriveDynamics(r, wpi::math::Vectord<2>::Zero()));
observer.Predict(u, dt);
@@ -158,7 +164,8 @@ TEST(S3UKFTest, DriveConvergence) {
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
wpi::math::AngleMean<5, 5 + 2>(2),
wpi::math::AngleResidual<5>(2),
wpi::math::AngleResidual<5>(2), wpi::math::AngleAdd<5>(2)
);
@@ -176,8 +183,9 @@ TEST(S3UKFTest, DriveConvergence) {
TEST(S3UKFTest, LinearUKF) {
constexpr wpi::units::second_t dt = 20_ms;
auto plant = wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
auto plant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
wpi::math::S3UKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
@@ -209,8 +217,12 @@ TEST(S3UKFTest, RoundTripP) {
constexpr auto dt = 5_ms;
wpi::math::S3UKF<2, 2, 2> observer{
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) { return x; },
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
[](const wpi::math::Vectord<2>& x, const wpi::math::Vectord<2>& u) {
return x;
},
{0.0, 0.0},
{0.0, 0.0},
dt};
@@ -223,7 +235,7 @@ TEST(S3UKFTest, RoundTripP) {
// Second system, single motor feedforward estimator
wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
@@ -235,7 +247,7 @@ wpi::math::Vectord<4> MotorDynamics(const wpi::math::Vectord<4>& x,
}
wpi::math::Vectord<3> MotorMeasurementModel(const wpi::math::Vectord<4>& x,
const wpi::math::Vectord<1>& u) {
const wpi::math::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
@@ -271,9 +283,9 @@ TEST(S3UKFTest, MotorConvergence) {
states[0] = wpi::math::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr wpi::math::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}};
{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 wpi::math::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
wpi::math::Matrixd<4, 4> discA;
@@ -291,7 +303,8 @@ TEST(S3UKFTest, MotorConvergence) {
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};
wpi::math::S3UKF<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
MotorDynamics, MotorMeasurementModel,
wpi::util::array{0.1, 1.0, 1e-10, 1e-10},
wpi::util::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(wpi::math::Vectord<4>{0.0, 0.0, 2.0, 2.0});