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

[wpimath] Add core State-space classes (#2614)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
wpimath/src/test/native/cpp/StateSpaceTest.cpp Normal file
View File

@@ -0,0 +1,77 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <cmath>
#include <random>
#include "Eigen/Core"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/LinearSystemLoop.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
namespace frc {
constexpr double kPositionStddev = 0.0001;
constexpr auto kDt = 0.00505_s;
class StateSpace : public testing::Test {
public:
LinearSystem<2, 1, 1> plant = [] {
auto motors = DCMotor::Vex775Pro(2);
// Carriage mass
constexpr auto m = 5_kg;
// Radius of pulley
constexpr auto r = 0.0181864_m;
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};
LinearPlantInversionFeedforward<2, 1> feedforward{plant, kDt};
LinearSystemLoop<2, 1, 1> loop{plant, controller, feedforward, observer,
12_V};
};
void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) {
Eigen::Matrix<double, 1, 1> y =
loop.Plant().CalculateY(loop.Xhat(), loop.U()) +
Eigen::Matrix<double, 1, 1>(noise);
loop.Correct(y);
loop.Predict(kDt);
}
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;
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {
Update(loop, dist(generator));
EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0));
EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0);
}
EXPECT_NEAR(loop.Xhat(0), 2.0, 0.05);
EXPECT_NEAR(loop.Xhat(1), 0.0, 0.5);
}
} // namespace frc

148
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp Normal file
View File

@@ -0,0 +1,148 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <array>
#include "Eigen/Core"
#include "frc/StateSpaceUtil.h"
#include "frc/system/RungeKutta.h"
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector
Eigen::Matrix<double, 2, 1> 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);
EXPECT_NEAR(mat2(0), 1.0, 1e-3);
EXPECT_NEAR(mat2(1), 2.0, 1e-3);
// Square matrix
Eigen::Matrix<double, 2, 2> mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0);
EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3);
EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3);
// Nonsquare matrix with more rows than columns
Eigen::Matrix<double, 3, 2> mat4 =
frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3);
EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3);
EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3);
EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3);
// Nonsquare matrix with more columns than rows
Eigen::Matrix<double, 2, 3> mat5 =
frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3);
EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3);
EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3);
EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3);
EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3);
}
TEST(StateSpaceUtilTest, CostParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CostArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
Eigen::Matrix<double, 2, 1> 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});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
Eigen::Matrix<double, 2, 2> A;
Eigen::Matrix<double, 2, 1> B;
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);
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);
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);
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);
EXPECT_TRUE(ret);
}

72
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp Normal file
View File

@@ -0,0 +1,72 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#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::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;
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
const Eigen::Matrix<double, 1, 1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, units::second_t(0.02)};
Eigen::Matrix<double, 2, 1> r;
r << 2, 2;
Eigen::Matrix<double, 2, 1> nextR;
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>&)>
modelDynamics = [](auto& x) { return StateDynamics(x); };
Eigen::Matrix<double, 2, 1> B;
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;
EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
}
} // namespace frc

36
wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp Normal file
View File

@@ -0,0 +1,36 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <cmath>
#include "Eigen/Core"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/time.h"
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;
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;
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}
} // namespace frc

88
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp Normal file
View File

@@ -0,0 +1,88 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <cmath>
#include "Eigen/Core"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
namespace frc {
TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
LinearSystem<2, 1, 1> plant = [] {
auto motors = DCMotor::Vex775Pro(2);
// Carriage mass
constexpr auto m = 5_kg;
// Radius of pulley
constexpr auto r = 0.0181864_m;
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
LinearQuadraticRegulator<2, 1> controller{
plant, {0.02, 0.4}, {12.0}, 0.00505_s};
EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6);
EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6);
}
TEST(LinearQuadraticRegulatorTest, ArmGains) {
LinearSystem<2, 1, 1> plant = [] {
auto motors = DCMotor::Vex775Pro(2);
// Carriage mass
constexpr auto m = 4_kg;
// Radius of pulley
constexpr auto r = 0.4_m;
// Gear ratio
constexpr double G = 100.0;
return frc::LinearSystemId::SingleJointedArmSystem(
motors, 1.0 / 3.0 * m * r * r, G);
}();
LinearQuadraticRegulator<2, 1> controller{
plant, {0.01745, 0.08726}, {12.0}, 0.00505_s};
EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1);
EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1);
}
TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
LinearSystem<2, 1, 1> plant = [] {
auto motors = DCMotor::Vex775Pro(4);
// Carriage mass
constexpr auto m = 8_kg;
// Radius of pulley
constexpr auto r = 0.75_in;
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s};
EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1);
EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
}
} // namespace frc

172
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp Normal file
View File

@@ -0,0 +1,172 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
constexpr auto m = 63.503_kg; // Robot mass
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
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) =
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 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
return result;
}
Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 3, 1> y;
y << x(2), x(3), x(4);
return y;
}
Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 5, 1> y;
y << x(0), x(1), x(2), x(3), x(4);
return y;
}
} // namespace
TEST(ExtendedKalmanFilterTest, Init) {
constexpr auto dt = 0.00505_s;
frc::ExtendedKalmanFilter<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;
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
}
TEST(ExtendedKalmanFilterTest, Convergence) {
constexpr auto dt = 0.00505_s;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
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::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::Matrix<double, 5, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
observer.SetXhat(frc::MakeMatrix<5, 1>(
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
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) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).to<double>());
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>();
auto localY =
LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::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()));
observer.Predict(u, dt);
r = nextR;
}
auto localY = LocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
observer.Xhat(0), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
observer.Xhat(1), 1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
observer.Xhat(2), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}

24
wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp Normal file
View File

@@ -0,0 +1,24 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "frc/estimator/KalmanFilter.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/moment_of_inertia.h"
#include "units/time.h"
TEST(KalmanFilterTest, Flywheel) {
auto motor = frc::DCMotor::NEO();
auto flywheel = frc::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
frc::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
}

41
wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
namespace drake {
namespace 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));
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))
.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));
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))
.norm() < 1e-3);
}
} // namespace
} // namespace math
} // namespace drake

176
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp Normal file
View File

@@ -0,0 +1,176 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
constexpr double Ghigh = 7.08; // High gear ratio
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
constexpr auto r = 0.0746125_m; // Wheel radius
constexpr auto m = 63.503_kg; // Robot mass
constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia
auto C1 = -std::pow(Ghigh, 2) * motors.Kt /
(motors.Kv * motors.R * units::math::pow<2>(r));
auto C2 = Ghigh * motors.Kt / (motors.R * r);
auto k1 = (1 / m + units::math::pow<2>(rb) / J);
auto k2 = (1 / m - units::math::pow<2>(rb) / J);
units::meters_per_second_t vl{x(3)};
units::meters_per_second_t vr{x(4)};
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) =
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 * vl).to<double>() + (C2 * Vl).to<double>()) +
k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
return result;
}
Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 3, 1> y;
y << x(2), x(3), x(4);
return y;
}
Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {
static_cast<void>(u);
Eigen::Matrix<double, 5, 1> y;
y << x(0), x(1), x(2), x(3), x(4);
return y;
}
} // namespace
TEST(UnscentedKalmanFilterTest, Init) {
constexpr auto dt = 0.00505_s;
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;
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
}
TEST(UnscentedKalmanFilterTest, Convergence) {
constexpr auto dt = 0.00505_s;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
dt};
auto waypoints =
std::vector<frc::Pose2d>{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
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::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::Matrix<double, 5, 1>::Zero(),
Eigen::Matrix<double, 2, 1>::Zero());
observer.SetXhat(frc::MakeMatrix<5, 1>(
trajectory.InitialPose().Translation().X().to<double>(),
trajectory.InitialPose().Translation().Y().to<double>(),
trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
auto trueXhat = observer.Xhat();
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
auto ref = trajectory.Sample(dt * i);
units::meters_per_second_t vl =
ref.velocity * (1 - (ref.curvature * rb).to<double>());
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>();
auto localY =
LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::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()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
}
auto localY = LocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
auto finalPosition = trajectory.Sample(trajectory.TotalTime());
ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
observer.Xhat(0), 1.0);
ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
observer.Xhat(1), 1.0);
ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
observer.Xhat(2), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
}

244
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp Normal file
View File

@@ -0,0 +1,244 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <functional>
#include "Eigen/Core"
#include "frc/system/Discretization.h"
#include "frc/system/RungeKutta.h"
// 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, 1> x0;
x0 << 1, 1;
Eigen::Matrix<double, 2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1);
x1Truth(0) = x0(0) + 1.0 * x0(1);
EXPECT_EQ(x1Truth, x1Discrete);
}
// 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, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 1> x0;
x0 << 1, 1;
Eigen::Matrix<double, 1, 1> u;
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;
// We now have pos = vel = accel = 1, which should give us:
Eigen::Matrix<double, 2, 1> x1Truth;
x1Truth(1) = x0(1) + 1.0 * u(0);
x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
EXPECT_EQ(x1Truth, x1Discrete);
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
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;
constexpr auto dt = 1_s;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscQ:\n"
<< discQ << "\ndiscQIntegrated:\n"
<< discQIntegrated;
}
// Test that the discrete approximation of Q is roughly equal to
// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
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;
constexpr auto dt = 5.05_ms;
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
<< "Expected these to be nearly equal:\ndiscQ:\n"
<< discQ << "\ndiscQIntegrated:\n"
<< discQIntegrated;
}
// 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, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 1, 0, 0, 1;
constexpr auto dt = 1_s;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscQTaylor:\n"
<< discQTaylor << "\ndiscQIntegrated:\n"
<< discQIntegrated;
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
}
}
// 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, 1> contB;
contB << 0, 1;
Eigen::Matrix<double, 2, 2> contQ;
contQ << 0.0025, 0, 0, 1;
constexpr auto dt = 5.05_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
Eigen::Matrix<double, 2, 1> discB;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
for (int i = 0; i < contQ.rows(); i++) {
EXPECT_GT(esCont.eigenvalues()[i], 0);
}
Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<Eigen::Matrix<double, 2, 2>(
units::second_t, const Eigen::Matrix<double, 2, 2>&)>,
Eigen::Matrix<double, 2, 2>>(
[&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
return Eigen::Matrix<double, 2, 2>(
(contA * t.to<double>()).exp() * contQ *
(contA.transpose() * t.to<double>()).exp());
},
Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
<< "Expected these to be nearly equal:\ndiscQTaylor:\n"
<< discQTaylor << "\ndiscQIntegrated:\n"
<< discQIntegrated;
EXPECT_LT((discA - discATaylor).norm(), 1e-10);
// Discrete Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
for (int i = 0; i < discQTaylor.rows(); i++) {
EXPECT_GT(esDisc.eigenvalues()[i], 0);
}
}
// 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> discR = frc::DiscretizeR<2>(contR, 500_ms);
EXPECT_LT((discRTruth - discR).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscR:\n"
<< discR << "\ndiscRTruth:\n"
<< discRTruth;
}

77
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp Normal file
View File

@@ -0,0 +1,77 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <gtest/gtest.h>
#include <iostream>
#include "frc/StateSpaceUtil.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/length.h"
#include "units/mass.h"
TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
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));
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));
}
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));
}
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));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
double kv = 1.0;
double ka = 0.5;
auto model = frc::LinearSystemId::IdentifyPositionSystem(kv, ka);
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));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
double kv = 1.0;
double ka = 0.5;
auto model = frc::LinearSystemId::IdentifyVelocitySystem(kv, ka);
std::cout << model.A() << std::endl << model.B() << std::endl;
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));
}

68
wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp Normal file
View File

@@ -0,0 +1,68 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#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();
// 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) {
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());
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());
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();
// 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) {
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());
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());
EXPECT_TRUE(newD.isApprox(D));
}

71
wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp Normal file
View File

@@ -0,0 +1,71 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <gtest/gtest.h>
#include <cmath>
#include "frc/system/RungeKutta.h"
// Tests that integrating dx/dt = e^x works.
TEST(RungeKuttaTest, Exponential) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
[](Eigen::Matrix<double, 1, 1> x) {
Eigen::Matrix<double, 1, 1> y;
y(0) = std::exp(x(0));
return y;
},
y0, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
TEST(RungeKuttaTest, ExponentialWithU) {
Eigen::Matrix<double, 1, 1> y0;
y0(0) = 0.0;
Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
[](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;
},
y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
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();
}
} // namespace
// Tests RungeKutta with a time varying solution.
// Now, lets test RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
Eigen::Matrix<double, 1, 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();
},
y0, 5_s, 1_s);
EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
}