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

[wpimath] Refactor StateSpaceUtil into separate files (#8421)

Browse Source 
* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()
This commit is contained in:
Tyler Veness
2025-11-29 10:28:38 -08:00
committed by GitHub
parent c8e6ce1ca4
commit a79f86ade3
51 changed files with 755 additions and 741 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -11,10 +11,10 @@
#include <gtest/gtest.h>
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/moment_of_inertia.hpp"
namespace {
@@ -79,7 +79,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
}
@@ -123,8 +123,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
@@ -139,7 +138,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());

14
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -14,13 +14,13 @@
#include "wpi/math/estimator/AngleStatistics.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/Discretization.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/moment_of_inertia.hpp"
namespace {
@@ -90,7 +90,7 @@ TEST(MerweUKFTest, DriveInit) {
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(
u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
@@ -146,8 +146,7 @@ TEST(MerweUKFTest, DriveConvergence) {
auto localY =
DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(
@@ -163,7 +162,7 @@ TEST(MerweUKFTest, DriveConvergence) {
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
wpi::math::AngleMean<5, 2 * 5 + 1>(2),
wpi::math::AngleResidual<5>(2),
@@ -296,9 +295,8 @@ TEST(MerweUKFTest, MotorConvergence) {
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]) +
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
measurements[i] = MotorMeasurementModel(states[i + 1], inputs[i]) +
wpi::math::Normal(pos_stddev, vel_stddev, accel_stddev);
}
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};

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

@@ -14,13 +14,13 @@
#include "wpi/math/estimator/AngleStatistics.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/Discretization.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/moment_of_inertia.hpp"
namespace {
@@ -90,7 +90,7 @@ TEST(S3UKFTest, DriveInit) {
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
auto R = wpi::math::CovarianceMatrix(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),
@@ -146,8 +146,7 @@ TEST(S3UKFTest, DriveConvergence) {
auto localY =
DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
observer.Correct(
u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(
@@ -163,7 +162,7 @@ TEST(S3UKFTest, DriveConvergence) {
observer.Correct(u, localY);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
auto R = wpi::math::CovarianceMatrix(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),
@@ -296,9 +295,8 @@ TEST(S3UKFTest, MotorConvergence) {
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]) +
wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
measurements[i] = MotorMeasurementModel(states[i + 1], inputs[i]) +
wpi::math::Normal(pos_stddev, vel_stddev, accel_stddev);
}
wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};