mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +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
@@ -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}));
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user