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

[wpimath] Clean up Eigen usage

Browse Source 
* Replace Matrix<> with Vector<> where vectors are explicitly intended.
  I found these via `rg "Eigen::Matrix<double, \w+, 1>"`.
* Pass all Eigen matrices by const reference. I found these via `rg
  "\(Eigen"` on main (the initializer list constructors make more false
  positives).
* Replace MakeMatrix() and operator<< usage with initializer list
  constructors. I found these via `rg MakeMatrix` and `rg "<<"`
  respectively.
* Deprecate MakeMatrix()
This commit is contained in:
Tyler Veness
2021-08-19 00:23:48 -07:00
committed by Peter Johnson
parent 72716f51ce
commit 9359431bad
63 changed files with 821 additions and 955 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/src/test/native/cpp/EigenTest.cpp
View File

@@ -7,48 +7,40 @@
#include "gtest/gtest.h"
TEST(EigenTest, MultiplicationTest) {
Eigen::Matrix<double, 2, 2> m1;
m1 << 2, 1, 0, 1;
Eigen::Matrix<double, 2, 2> m2;
m2 << 3, 0, 0, 2.5;
Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
Eigen::Matrix<double, 2, 2> expectedResult;
expectedResult << 6.0, 2.5, 0.0, 2.5;
Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
Eigen::Matrix<double, 2, 3> m3;
m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
Eigen::Matrix<double, 3, 4> m4;
m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
Eigen::Matrix<double, 3, 4> m4{
{3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
const auto result2 = m3 * m4;
Eigen::Matrix<double, 2, 4> expectedResult2;
expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
{22.13, 9.82, 13.28, 23.53}};
EXPECT_TRUE(expectedResult2.isApprox(result2));
}
TEST(EigenTest, TransposeTest) {
Eigen::Matrix<double, 3, 1> vec;
vec << 1, 2, 3;
Eigen::Vector<double, 3> vec{1, 2, 3};
const auto transpose = vec.transpose();
Eigen::Matrix<double, 1, 3> expectedTranspose;
expectedTranspose << 1, 2, 3;
Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
EXPECT_TRUE(expectedTranspose.isApprox(transpose));
}
TEST(EigenTest, InverseTest) {
Eigen::Matrix<double, 3, 3> mat;
mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
Eigen::Matrix<double, 3, 3> mat{
{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
const auto inverse = mat.inverse();
const auto identity = Eigen::MatrixXd::Identity(3, 3);

3
wpimath/src/test/native/cpp/FormatterTest.cpp
View File

@@ -10,8 +10,7 @@
#include "units/velocity.h"
TEST(FormatterTest, Eigen) {
Eigen::Matrix<double, 3, 2> A;
A << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
EXPECT_EQ(
" 1.000000 2.000000\n"
" 3.000000 4.000000\n"

7
wpimath/src/test/native/cpp/StateSpaceTest.cpp
View File

@@ -45,8 +45,8 @@ class StateSpace : public testing::Test {
void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
double noise) {
Eigen::Matrix<double, 1, 1> y = plant.CalculateY(loop.Xhat(), loop.U()) +
Eigen::Matrix<double, 1, 1>(noise);
Eigen::Vector<double, 1> y =
plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
loop.Correct(y);
loop.Predict(kDt);
}
@@ -55,8 +55,7 @@ TEST_F(StateSpace, CorrectPredictLoop) {
std::default_random_engine generator;
std::normal_distribution<double> dist{0.0, kPositionStddev};
Eigen::Matrix<double, 2, 1> references;
references << 2.0, 0.0;
Eigen::Vector<double, 2> references{2.0, 0.0};
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {

28
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -12,12 +12,12 @@
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector
Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
EXPECT_NEAR(mat1(0), 1.0, 1e-3);
EXPECT_NEAR(mat1(1), 2.0, 1e-3);
// Row vector
Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
EXPECT_NEAR(mat2(0), 1.0, 1e-3);
EXPECT_NEAR(mat2(1), 2.0, 1e-3);
@@ -102,44 +102,42 @@ TEST(StateSpaceUtilTest, CovArray) {
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
Eigen::Matrix<double, 2, 2> A;
Eigen::Matrix<double, 2, 1> B;
B << 0, 1;
Eigen::Matrix<double, 2, 1> B{0, 1};
// We separate the result of IsStabilizable from the assertion because
// templates break gtest.
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
A << 1.2, 0, 0, 0.5;
bool ret = frc::IsStabilizable<2, 1>(A, B);
Eigen::Matrix<double, 2, 2> A1{{1.2, 0}, {0, 0.5}};
bool ret = frc::IsStabilizable<2, 1>(A1, B);
EXPECT_FALSE(ret);
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
A << 1, 0, 0, 0.5;
ret = frc::IsStabilizable<2, 1>(A, B);
Eigen::Matrix<double, 2, 2> A2{{1, 0}, {0, 0.5}};
ret = frc::IsStabilizable<2, 1>(A2, B);
EXPECT_FALSE(ret);
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
A << 0.2, 0, 0, 0.5;
ret = frc::IsStabilizable<2, 1>(A, B);
Eigen::Matrix<double, 2, 2> A3{{0.2, 0}, {0, 0.5}};
ret = frc::IsStabilizable<2, 1>(A3, B);
EXPECT_TRUE(ret);
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
A << 0.2, 0, 0, 1.2;
ret = frc::IsStabilizable<2, 1>(A, B);
Eigen::Matrix<double, 2, 2> A4{{0.2, 0}, {0, 1.2}};
ret = frc::IsStabilizable<2, 1>(A4, B);
EXPECT_TRUE(ret);
}

45
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -7,61 +7,46 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
#include "frc/controller/ControlAffinePlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
const Eigen::Matrix<double, 1, 1>& u) {
Eigen::Matrix<double, 2, 1> result;
result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
(frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
return result;
Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u) {
return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
}
Eigen::Matrix<double, 2, 1> StateDynamics(
const Eigen::Matrix<double, 2, 1>& x) {
Eigen::Matrix<double, 2, 1> result;
result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
return result;
Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
const Eigen::Matrix<double, 1, 1>&)>
std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
const Eigen::Vector<double, 1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, units::second_t(0.02)};
modelDynamics, units::second_t{0.02}};
Eigen::Matrix<double, 2, 1> r;
r << 2, 2;
Eigen::Matrix<double, 2, 1> nextR;
nextR << 3, 3;
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
modelDynamics = [](auto& x) { return StateDynamics(x); };
Eigen::Matrix<double, 2, 1> B;
B << 0, 1;
Eigen::Matrix<double, 2, 1> B{0, 1};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, B, units::second_t(0.02)};
Eigen::Matrix<double, 2, 1> r;
r << 2, 2;
Eigen::Matrix<double, 2, 1> nextR;
nextR << 3, 3;
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}

13
wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
View File

@@ -13,19 +13,14 @@
namespace frc {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Eigen::Matrix<double, 2, 2> A;
A << 1, 0, 0, 1;
Eigen::Matrix<double, 2, 1> B;
B << 0, 1;
Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
Eigen::Matrix<double, 2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
units::second_t(0.02)};
Eigen::Matrix<double, 2, 1> r;
r << 2, 2;
Eigen::Matrix<double, 2, 1> nextR;
nextR << 3, 3;
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}

13
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -21,21 +21,16 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ka = 0.6;
auto dt = 0.02_s;
Eigen::Matrix<double, 1, 1> A;
A << -Kv / Ka;
Eigen::Matrix<double, 1, 1> B;
B << 1.0 / Ka;
Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
Eigen::Matrix<double, 1, 1> r;
r << 2;
Eigen::Matrix<double, 1, 1> nextR;
nextR << 3;
Eigen::Vector<double, 1> r{2.0};
Eigen::Vector<double, 1> nextR{3.0};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).to<double>(), 0.002);

21
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -10,11 +10,12 @@
#include "frc/estimator/AngleStatistics.h"
TEST(AngleStatisticsTest, TestMean) {
Eigen::Matrix<double, 3, 3> sigmas;
sigmas << 1, 1.2, 0, 359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180,
0, 1, 2, 0;
Eigen::Matrix<double, 3, 3> sigmas{
{1, 1.2, 0},
{359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
{1, 2, 0}};
// Weights need to produce the mean of the sigmas
Eigen::Vector3d weights{};
Eigen::Vector3d weights;
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
@@ -22,16 +23,16 @@ TEST(AngleStatisticsTest, TestMean) {
}
TEST(AngleStatisticsTest, TestResidual) {
Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2);
Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1);
Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
Eigen::Vector3d(0, 2 * wpi::numbers::pi / 180, 1)));
Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
}
TEST(AngleStatisticsTest, TestAdd) {
Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2);
Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1);
Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3)));
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}

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

@@ -18,8 +18,8 @@
namespace {
Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -40,36 +40,26 @@ Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
Eigen::Matrix<double, 5, 1> result;
auto v = 0.5 * (vl + vr);
result(0) = v.to<double>() * std::cos(x(2));
result(1) = v.to<double>() * std::sin(x(2));
result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
result(3) =
return Eigen::Vector<double, 5>{
v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).to<double>(),
k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
result(4) =
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
return result;
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
}
Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 3, 1> y;
y << x(2), x(3), x(4);
return y;
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
}
Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 5, 1> y;
y << x(0), x(1), x(2), x(3), x(4);
return y;
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -81,8 +71,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Matrix<double, 2, 1> u;
u << 12.0, 12.0;
Eigen::Vector<double, 2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -109,19 +98,17 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
Eigen::Matrix<double, 5, 1> nextR;
Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
Eigen::Vector<double, 5>::Zero(),
Eigen::Vector<double, 2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(
Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
observer.SetXhat(frc::MakeMatrix<5, 1>(
observer.SetXhat(Eigen::Vector<double, 5>{
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
@@ -131,19 +118,18 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).to<double>());
nextR(0) = ref.pose.Translation().X().to<double>();
nextR(1) = ref.pose.Translation().Y().to<double>();
nextR(2) = ref.pose.Rotation().Radians().to<double>();
nextR(3) = vl.to<double>();
nextR(4) = vr.to<double>();
Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
ref.pose.Translation().Y().to<double>(),
ref.pose.Rotation().Radians().to<double>(),
vl.to<double>(), vr.to<double>()};
auto localY =
LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::Zero());
LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
u = B.householderQr().solve(
rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
observer.Predict(u, dt);

23
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
View File

@@ -4,32 +4,33 @@
#include <gtest/gtest.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
namespace drake::math {
namespace {
TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points =
sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
auto points = sigmaPoints.SigmaPoints(
Eigen::Vector<double, 2>{0.0, 0.0},
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
0.0, 0.0, 0.00173205, 0.0, -0.00173205))
(points -
Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
{0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
.norm() < 1e-3);
}
TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
auto points =
sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
auto points = sigmaPoints.SigmaPoints(
Eigen::Vector<double, 2>{1.0, 2.0},
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
EXPECT_TRUE(
(points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
2.0, 2.00548, 2.0, 1.99452))
(points -
Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
{2.0, 2.0, 2.00548, 2.0, 1.99452}})
.norm() < 1e-3);
}
} // namespace

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

@@ -20,8 +20,8 @@
namespace {
Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -42,49 +42,38 @@ Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
units::volt_t Vl{u(0)};
units::volt_t Vr{u(1)};
Eigen::Matrix<double, 5, 1> result;
auto v = 0.5 * (vl + vr);
result(0) = v.to<double>() * std::cos(x(2));
result(1) = v.to<double>() * std::sin(x(2));
result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
result(3) =
return Eigen::Vector<double, 5>{
v.to<double>() * std::cos(x(2)), v.to<double>() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).to<double>(),
k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
result(4) =
k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>()),
k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
return result;
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>())};
}
Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 3, 1> y;
y << x(2), x(3), x(4);
return y;
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
}
Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 5, 1> y;
y << x(0), x(1), x(2), x(3), x(4);
return y;
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(UnscentedKalmanFilterTest, Init) {
constexpr auto dt = 0.00505_s;
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Matrix<double, 2, 1> u;
u << 12.0, 12.0;
Eigen::Vector<double, 2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,7 +87,7 @@ TEST(UnscentedKalmanFilterTest, Init) {
}
TEST(UnscentedKalmanFilterTest, Convergence) {
constexpr auto dt = 0.00505_s;
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
@@ -113,19 +102,17 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
Eigen::Matrix<double, 5, 1> nextR;
Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
Eigen::Vector<double, 5>::Zero(),
Eigen::Vector<double, 2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(
Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
observer.SetXhat(frc::MakeMatrix<5, 1>(
observer.SetXhat(Eigen::Vector<double, 5>{
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0});
auto trueXhat = observer.Xhat();
@@ -137,19 +124,18 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).to<double>());
nextR(0) = ref.pose.Translation().X().to<double>();
nextR(1) = ref.pose.Translation().Y().to<double>();
nextR(2) = ref.pose.Rotation().Radians().to<double>();
nextR(3) = vl.to<double>();
nextR(4) = vr.to<double>();
Eigen::Vector<double, 5> nextR{ref.pose.Translation().X().to<double>(),
ref.pose.Translation().Y().to<double>(),
ref.pose.Rotation().Radians().to<double>(),
vl.to<double>(), vr.to<double>()};
auto localY =
LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::Zero());
LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
u = B.householderQr().solve(
rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.to<double>();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
observer.Predict(u, dt);

72
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -15,20 +15,17 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 1> x0;
x0 << 1, 1;
Eigen::Vector<double, 2> x0{1, 1};
Eigen::Matrix<double, 2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
Eigen::Vector<double, 2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1);
Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -36,26 +33,20 @@ TEST(DiscretizationTest, DiscretizeA) {
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeAB) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 1> contB{0, 1};
Eigen::Matrix<double, 2, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 1> x0;
x0 << 1, 1;
Eigen::Matrix<double, 1, 1> u;
u << 1;
Eigen::Vector<double, 2> x0{1, 1};
Eigen::Vector<double, 1> u{1};
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0 + discB * u;
Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(0) = 1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0);
x1Truth(1) = 0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0);
Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -64,11 +55,8 @@ TEST(DiscretizationTest, DiscretizeAB) {
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 1, 0, 0, 1;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
@@ -97,11 +85,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
TEST(DiscretizationTest, DiscretizeFastModelAQ) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, -1406.29;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
@@ -128,11 +113,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, 0;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 1, 0, 0, 1;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
constexpr auto dt = 1_s;
@@ -141,7 +123,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
for (int i = 0; i < contQ.rows(); ++i) {
EXPECT_GE(esCont.eigenvalues()[i], 0);
}
@@ -167,7 +149,7 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
for (int i = 0; i < discQTaylor.rows(); ++i) {
EXPECT_GE(esDisc.eigenvalues()[i], 0);
}
@@ -175,11 +157,8 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA;
contA << 0, 1, 0, -1500;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
@@ -222,11 +201,8 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
Eigen::Matrix<double, 2, 2> contR;
contR << 2.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> discRTruth;
discRTruth << 4.0, 0.0, 0.0, 2.0;
Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);

45
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
View File

@@ -7,7 +7,6 @@
#include <frc/system/plant/LinearSystemId.h>
#include <gtest/gtest.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/length.h"
#include "units/mass.h"
@@ -17,32 +16,37 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
ASSERT_TRUE(model.A().isApprox(
frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
0.001));
ASSERT_TRUE(model.B().isApprox(
frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
ASSERT_TRUE(
model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001));
ASSERT_TRUE(
model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001));
Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
0.001));
ASSERT_TRUE(model.C().isApprox(
Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(
Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
0.05_m, 12);
ASSERT_TRUE(model.A().isApprox(
frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001));
ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001));
ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001));
ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001));
ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001));
ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001));
ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
ASSERT_TRUE(
model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -53,9 +57,10 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
0.001));
ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001));
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -67,6 +72,6 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));
ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
}

50
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -10,14 +10,11 @@
// Tests that integrating dx/dt = e^x works.
TEST(NumericalIntegrationTest, Exponential) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Vector<double, 1> y0{0.0};
Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
[](Eigen::Matrix<double, 1, 1> x) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
return y;
Eigen::Vector<double, 1> y1 = frc::RK4(
[](const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
},
y0, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
@@ -25,45 +22,36 @@ TEST(NumericalIntegrationTest, Exponential) {
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(NumericalIntegrationTest, ExponentialWithU) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Vector<double, 1> y0{0.0};
Eigen::Matrix<double, 1, 1> y1 = frc::RK4(
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(u(0) * x(0));
return y;
Eigen::Vector<double, 1> y1 = frc::RK4(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
},
y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKF45.
TEST(NumericalIntegrationTest, ExponentialRKF45) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Vector<double, 1> y0{0.0};
Eigen::Matrix<double, 1, 1> y1 = frc::RKF45(
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
return y;
Eigen::Vector<double, 1> y1 = frc::RKF45(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
},
y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Vector<double, 1> y0{0.0};
Eigen::Matrix<double, 1, 1> y1 = frc::RKDP(
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
return y;
Eigen::Vector<double, 1> y1 = frc::RKDP(
[](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
},
y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}

49
wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
View File

@@ -6,60 +6,53 @@
#include "frc/system/NumericalJacobian.h"
Eigen::Matrix<double, 4, 4> A = (Eigen::Matrix<double, 4, 4>() << 1, 2, 4, 1, 5,
2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
.finished();
Eigen::Matrix<double, 4, 2> B =
(Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
Eigen::Matrix<double, 4, 4> A{
{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
// Function from which to recover A and B
Eigen::Matrix<double, 4, 1> AxBuFn(const Eigen::Matrix<double, 4, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax) {
Eigen::Matrix<double, 4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
Eigen::Matrix<double, 4, 4> newA =
frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu) {
Eigen::Matrix<double, 4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
Eigen::Matrix<double, 4, 2> newB =
frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
Eigen::Matrix<double, 3, 4> C =
(Eigen::Matrix<double, 3, 4>() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2)
.finished();
Eigen::Matrix<double, 3, 2> D =
(Eigen::Matrix<double, 3, 2>() << 1, 1, 2, 1, 3, 2).finished();
Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
// Function from which to recover C and D
Eigen::Matrix<double, 3, 1> CxDuFn(const Eigen::Matrix<double, 4, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
return C * x + D * u;
}
// Test that we can recover C from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Cx) {
Eigen::Matrix<double, 3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
Eigen::Matrix<double, 3, 4> newC =
frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newC.isApprox(C));
}
// Test that we can recover D from CxDuFn() pretty accurately
TEST(NumericalJacobianTest, Du) {
Eigen::Matrix<double, 3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
Eigen::Matrix<double, 3, 2> newD =
frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}

18
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -9,10 +9,9 @@
#include "frc/system/RungeKuttaTimeVarying.h"
namespace {
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
return (Eigen::Matrix<double, 1, 1>()
<< 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
.finished();
Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
(std::pow(std::exp(t) + 1.0, 2.0))};
}
} // namespace
@@ -24,13 +23,12 @@ Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
return (Eigen::Matrix<double, 1, 1>()
<< x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
.finished();
Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
[](units::second_t t, const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{
x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0)};
},
5_s, y0, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);