mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
[wpimath] Clean up Eigen usage
* 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
committed by
Peter Johnson
Peter Johnson
parent
72716f51ce
commit
9359431bad
@@ -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);
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user