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

[wpimath] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -42,5 +42,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
}
void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}

20
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -41,8 +41,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector<double, 2>& u) {
Vectord<2> DifferentialDrivetrainSim::ClampInput(const Vectord<2>& u) {
return frc::DesaturateInputVector<2>(u,
frc::RobotController::GetInputVoltage());
}
@@ -66,11 +65,11 @@ double DifferentialDrivetrainSim::GetGearing() const {
return m_currentGearing;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetOutput() const {
Vectord<7> DifferentialDrivetrainSim::GetOutput() const {
return m_y;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetState() const {
Vectord<7> DifferentialDrivetrainSim::GetState() const {
return m_x;
}
@@ -116,8 +115,7 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
return GetLeftCurrentDraw() + GetRightCurrentDraw();
}
void DifferentialDrivetrainSim::SetState(
const Eigen::Vector<double, 7>& state) {
void DifferentialDrivetrainSim::SetState(const Vectord<7>& state) {
m_x = state;
}
@@ -129,19 +127,19 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
m_x(State::kRightPosition) = 0;
}
Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
const Eigen::Vector<double, 7>& x, const Eigen::Vector<double, 2>& u) {
Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x,
const Vectord<2>& u) {
// Because G^2 can be factored out of A, we can divide by the old ratio
// squared and multiply by the new ratio squared to get a new drivetrain
// model.
Eigen::Matrix<double, 4, 2> B;
Matrixd<4, 2> B;
B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
m_originalGearing / m_originalGearing;
B.block<2, 2>(2, 0).setZero();
// Because G can be factored out of B, we can divide by the old ratio and
// multiply by the new ratio to get a new drivetrain model.
Eigen::Matrix<double, 4, 4> A;
Matrixd<4, 4> A;
A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
A.block<2, 2>(2, 0).setIdentity();
@@ -149,7 +147,7 @@ Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
Eigen::Vector<double, 7> xdot;
Vectord<7> xdot;
xdot(0) = v * std::cos(x(State::kHeading));
xdot(1) = v * std::sin(x(State::kHeading));
xdot(2) =

18
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -80,29 +80,27 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
}
void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}
Eigen::Vector<double, 2> ElevatorSim::UpdateX(
const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u, units::second_t dt) {
Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
const Vectord<1>& u, units::second_t dt) {
auto updatedXhat = RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
[&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{0.0, -9.8};
xdot += Vectord<2>{0.0, -9.8};
}
return xdot;
},
currentXhat, u, dt);
// Check for collision after updating x-hat.
if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_minHeight.value(), 0.0};
return Vectord<2>{m_minHeight.value(), 0.0};
}
if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_maxHeight.value(), 0.0};
return Vectord<2>{m_maxHeight.value(), 0.0};
}
return updatedXhat;
}

2
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
View File

@@ -38,5 +38,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
}
void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}

25
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -72,12 +72,12 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
}
void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}
Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u, units::second_t dt) {
Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,
const Vectord<1>& u,
units::second_t dt) {
// Horizontal case:
// Torque = F * r = I * alpha
// alpha = F * r / I
@@ -88,15 +88,14 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
// std::cos(theta)]]
Eigen::Vector<double, 2> updatedXhat = RKDP(
[&](const auto& x, const auto& u) -> Eigen::Vector<double, 2> {
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
Vectord<2> updatedXhat = RKDP(
[&](const auto& x, const auto& u) -> Vectord<2> {
Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{
0.0, (m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) *
std::cos(x(0)))
.value()};
xdot += Vectord<2>{0.0, (m_armMass * m_r * -9.8 * 3.0 /
(m_armMass * m_r * m_r) * std::cos(x(0)))
.value()};
}
return xdot;
},
@@ -104,9 +103,9 @@ Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
// Check for collisions.
if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_minAngle.value(), 0.0};
return Vectord<2>{m_minAngle.value(), 0.0};
} else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_maxAngle.value(), 0.0};
return Vectord<2>{m_maxAngle.value(), 0.0};
}
return updatedXhat;
}

19
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -4,11 +4,11 @@
#pragma once
#include <frc/EigenCore.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <Eigen/Core>
#include <units/length.h>
#include <units/moment_of_inertia.h>
#include <units/time.h>
@@ -80,7 +80,7 @@ class DifferentialDrivetrainSim {
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Vector<double, 2> ClampInput(const Eigen::Vector<double, 2>& u);
Vectord<2> ClampInput(const Vectord<2>& u);
/**
* Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -178,7 +178,7 @@ class DifferentialDrivetrainSim {
*
* @param state The state.
*/
void SetState(const Eigen::Vector<double, 7>& state);
void SetState(const Vectord<7>& state);
/**
* Sets the system pose.
@@ -187,8 +187,7 @@ class DifferentialDrivetrainSim {
*/
void SetPose(const frc::Pose2d& pose);
Eigen::Vector<double, 7> Dynamics(const Eigen::Vector<double, 7>& x,
const Eigen::Vector<double, 2>& u);
Vectord<7> Dynamics(const Vectord<7>& x, const Vectord<2>& u);
class State {
public:
@@ -301,7 +300,7 @@ class DifferentialDrivetrainSim {
/**
* Returns the current output vector y.
*/
Eigen::Vector<double, 7> GetOutput() const;
Vectord<7> GetOutput() const;
/**
* Returns an element of the state vector. Note that this will not include
@@ -314,7 +313,7 @@ class DifferentialDrivetrainSim {
/**
* Returns the current state vector x. Note that this will not include noise!
*/
Eigen::Vector<double, 7> GetState() const;
Vectord<7> GetState() const;
LinearSystem<2, 2, 2> m_plant;
units::meter_t m_rb;
@@ -325,9 +324,9 @@ class DifferentialDrivetrainSim {
double m_originalGearing;
double m_currentGearing;
Eigen::Vector<double, 7> m_x;
Eigen::Vector<double, 2> m_u;
Eigen::Vector<double, 7> m_y;
Vectord<7> m_x;
Vectord<2> m_u;
Vectord<7> m_y;
std::array<double, 7> m_measurementStdDevs;
};
} // namespace frc::sim

5
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -127,9 +127,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u,
units::second_t dt) override;
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
DCMotor m_gearbox;

28
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -6,10 +6,10 @@
#include <array>
#include <Eigen/Core>
#include <units/current.h>
#include <units/time.h>
#include "frc/EigenCore.h"
#include "frc/RobotController.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
@@ -40,9 +40,9 @@ class LinearSystemSim {
const LinearSystem<States, Inputs, Outputs>& system,
const std::array<double, Outputs>& measurementStdDevs = {})
: m_plant(system), m_measurementStdDevs(measurementStdDevs) {
m_x = Eigen::Vector<double, States>::Zero();
m_y = Eigen::Vector<double, Outputs>::Zero();
m_u = Eigen::Vector<double, Inputs>::Zero();
m_x = Vectord<States>::Zero();
m_y = Vectord<Outputs>::Zero();
m_u = Vectord<Inputs>::Zero();
}
virtual ~LinearSystemSim() = default;
@@ -71,7 +71,7 @@ class LinearSystemSim {
*
* @return The current output of the plant.
*/
const Eigen::Vector<double, Outputs>& GetOutput() const { return m_y; }
const Vectord<Outputs>& GetOutput() const { return m_y; }
/**
* Returns an element of the current output of the plant.
@@ -86,7 +86,7 @@ class LinearSystemSim {
*
* @param u The system inputs.
*/
void SetInput(const Eigen::Vector<double, Inputs>& u) { m_u = ClampInput(u); }
void SetInput(const Vectord<Inputs>& u) { m_u = ClampInput(u); }
/*
* Sets the system inputs.
@@ -104,7 +104,7 @@ class LinearSystemSim {
*
* @param state The new state.
*/
void SetState(const Eigen::Vector<double, States>& state) { m_x = state; }
void SetState(const Vectord<States>& state) { m_x = state; }
/**
* Returns the current drawn by this simulated system. Override this method to
@@ -124,9 +124,9 @@ class LinearSystemSim {
* @param u The system inputs (usually voltage).
* @param dt The time difference between controller updates.
*/
virtual Eigen::Vector<double, States> UpdateX(
const Eigen::Vector<double, States>& currentXhat,
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
const Vectord<Inputs>& u,
units::second_t dt) {
return m_plant.CalculateX(currentXhat, u, dt);
}
@@ -137,16 +137,16 @@ class LinearSystemSim {
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
Vectord<Inputs> ClampInput(Vectord<Inputs> u) {
return frc::DesaturateInputVector<Inputs>(
u, frc::RobotController::GetInputVoltage());
}
LinearSystem<States, Inputs, Outputs> m_plant;
Eigen::Vector<double, States> m_x;
Eigen::Vector<double, Outputs> m_y;
Eigen::Vector<double, Inputs> m_u;
Vectord<States> m_x;
Vectord<Outputs> m_y;
Vectord<Inputs> m_u;
std::array<double, Outputs> m_measurementStdDevs;
};
} // namespace frc::sim

5
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -142,9 +142,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u,
units::second_t dt) override;
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
units::meter_t m_r;

8
wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
View File

@@ -30,10 +30,10 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
frc::RamseteController ramsete;
feedforward.Reset(Eigen::Vector<double, 2>{0.0, 0.0});
feedforward.Reset(frc::Vectord<2>{0.0, 0.0});
// Ground truth.
Eigen::Vector<double, 7> groundTruthX = Eigen::Vector<double, 7>::Zero();
frc::Vectord<7> groundTruthX = frc::Vectord<7>::Zero();
frc::TrajectoryConfig config{1_mps, 1_mps_sq};
config.AddConstraint(
@@ -48,7 +48,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
auto voltages =
feedforward.Calculate(Eigen::Vector<double, 2>{l.value(), r.value()});
feedforward.Calculate(frc::Vectord<2>{l.value(), r.value()});
// Sim periodic code.
sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
@@ -56,7 +56,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
// Update ground truth.
groundTruthX = frc::RK4(
[&sim](const auto& x, const auto& u) -> Eigen::Vector<double, 7> {
[&sim](const auto& x, const auto& u) -> frc::Vectord<7> {
return sim.Dynamics(x, u);
},
groundTruthX, voltages, 20_ms);

19
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -34,8 +34,7 @@ TEST(ElevatorSimTest, StateSpaceSim) {
auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
Eigen::Vector<double, 1> u{motor.Get() *
frc::RobotController::GetInputVoltage()};
frc::Vectord<1> u{motor.Get() * frc::RobotController::GetInputVoltage()};
sim.SetInput(u);
sim.Update(20_ms);
@@ -51,7 +50,7 @@ TEST(ElevatorSimTest, MinMax) {
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
true, {0.01});
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{0.0});
sim.SetInput(frc::Vectord<1>{0.0});
sim.Update(20_ms);
auto height = sim.GetPosition();
@@ -59,7 +58,7 @@ TEST(ElevatorSimTest, MinMax) {
}
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{12.0});
sim.SetInput(frc::Vectord<1>{12.0});
sim.Update(20_ms);
auto height = sim.GetPosition();
@@ -71,16 +70,16 @@ TEST(ElevatorSimTest, Stability) {
frc::sim::ElevatorSim sim{
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true};
sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
sim.SetInput(Eigen::Vector<double, 1>{12.0});
sim.SetState(frc::Vectord<2>{0.0, 0.0});
sim.SetInput(frc::Vectord<1>{12.0});
for (int i = 0; i < 50; ++i) {
sim.Update(20_ms);
}
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100);
EXPECT_NEAR_UNITS(units::meter_t{system.CalculateX(
Eigen::Vector<double, 2>{0.0, 0.0},
Eigen::Vector<double, 1>{12.0}, 20_ms * 50)(0)},
sim.GetPosition(), 1_cm);
EXPECT_NEAR_UNITS(
units::meter_t{system.CalculateX(frc::Vectord<2>{0.0, 0.0},
frc::Vectord<1>{12.0}, 20_ms * 50)(0)},
sim.GetPosition(), 1_cm);
}

4
wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
View File

@@ -10,10 +10,10 @@
TEST(SingleJointedArmTest, Disabled) {
frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m,
9.5_in, -180_deg, 0_deg, 10_lb, true);
sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
sim.SetState(frc::Vectord<2>{0.0, 0.0});
for (size_t i = 0; i < 12 / 0.02; ++i) {
sim.SetInput(Eigen::Vector<double, 1>{0.0});
sim.SetInput(frc::Vectord<1>{0.0});
sim.Update(20_ms);
}

4
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
View File

@@ -46,8 +46,8 @@ TEST(StateSpaceSimTest, FlywheelSim) {
// Then, SimulationPeriodic runs
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInput(Eigen::Vector<double, 1>{
motor.Get() * frc::RobotController::GetInputVoltage()});
sim.SetInput(
frc::Vectord<1>{motor.Get() * frc::RobotController::GetInputVoltage()});
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().value());
}

4
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
View File

@@ -101,8 +101,8 @@ class Robot : public frc::TimedRobot {
void SimulationPeriodic() override {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.SetInput(Eigen::Vector<double, 1>{
m_motor.Get() * frc::RobotController::GetInputVoltage()});
m_armSim.SetInput(frc::Vectord<1>{m_motor.Get() *
frc::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_armSim.Update(20_ms);

2
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
View File

@@ -88,7 +88,7 @@ class Robot : public frc::TimedRobot {
void SimulationPeriodic() override {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.SetInput(Eigen::Vector<double, 1>{
m_elevatorSim.SetInput(frc::Vectord<1>{
m_motor.Get() * frc::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.

10
wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
View File

@@ -97,8 +97,7 @@ class Robot : public frc::TimedRobot {
}
void TeleopInit() override {
m_loop.Reset(
Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_loop.Reset(frc::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_lastProfiledReference = {
units::radian_t(m_encoder.GetDistance()),
@@ -121,12 +120,11 @@ class Robot : public frc::TimedRobot {
m_lastProfiledReference))
.Calculate(20_ms);
m_loop.SetNextR(
Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
m_loop.Correct(frc::Vectord<1>{m_encoder.GetDistance()});
// Update our LQR to generate new voltage commands and use the voltages to
// predict the next state with out Kalman filter.

10
wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
View File

@@ -96,8 +96,7 @@ class Robot : public frc::TimedRobot {
void TeleopInit() override {
// Reset our loop to make sure it's in a known state.
m_loop.Reset(
Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_loop.Reset(frc::Vectord<2>{m_encoder.GetDistance(), m_encoder.GetRate()});
m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()),
units::meters_per_second_t(m_encoder.GetRate())};
@@ -119,12 +118,11 @@ class Robot : public frc::TimedRobot {
m_lastProfiledReference))
.Calculate(20_ms);
m_loop.SetNextR(
Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
m_loop.Correct(frc::Vectord<1>{m_encoder.GetDistance()});
// Update our LQR to generate new voltage commands and use the voltages to
// predict the next state with out Kalman filter.

8
wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
View File

@@ -85,7 +85,7 @@ class Robot : public frc::TimedRobot {
}
void TeleopInit() override {
m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
m_loop.Reset(frc::Vectord<1>{m_encoder.GetRate()});
}
void TeleopPeriodic() override {
@@ -93,14 +93,14 @@ class Robot : public frc::TimedRobot {
// setpoint of a PID controller.
if (m_joystick.GetRightBumper()) {
// We pressed the bumper, so let's set our next reference
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
m_loop.SetNextR(frc::Vectord<1>{kSpinup.value()});
} else {
// We released the bumper, so let's spin down
m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
m_loop.SetNextR(frc::Vectord<1>{0.0});
}
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
m_loop.Correct(frc::Vectord<1>{m_encoder.GetRate()});
// Update our LQR to generate new voltage commands and use the voltages to
// predict the next state with out Kalman filter.

8
wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
View File

@@ -85,7 +85,7 @@ class Robot : public frc::TimedRobot {
}
void TeleopInit() override {
m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
m_loop.Reset(frc::Vectord<1>{m_encoder.GetRate()});
}
void TeleopPeriodic() override {
@@ -93,14 +93,14 @@ class Robot : public frc::TimedRobot {
// setpoint of a PID controller.
if (m_joystick.GetRightBumper()) {
// We pressed the bumper, so let's set our next reference
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
m_loop.SetNextR(frc::Vectord<1>{kSpinup.value()});
} else {
// We released the bumper, so let's spin down
m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
m_loop.SetNextR(frc::Vectord<1>{0.0});
}
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
m_loop.Correct(frc::Vectord<1>{m_encoder.GetRate()});
// Update our LQR to generate new voltage commands and use the voltages to
// predict the next state with out Kalman filter.

28
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -6,33 +6,31 @@
namespace frc {
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
Vectord<3> PoseTo3dVector(const Pose2d& pose) {
return Vectord<3>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
}
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector<double, 4>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Cos(), pose.Rotation().Sin()};
Vectord<4> PoseTo4dVector(const Pose2d& pose) {
return Vectord<4>{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()};
}
template <>
bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B) {
bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
return detail::IsStabilizableImpl<1, 1>(A, B);
}
template <>
bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B) {
bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
return detail::IsStabilizableImpl<2, 1>(A, B);
}
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
Vectord<3> PoseToVector(const Pose2d& pose) {
return Vectord<3>{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
}
} // namespace frc

12
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -24,21 +24,21 @@ DifferentialDriveAccelerationLimiter::Calculate(
units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
units::volt_t rightVoltage) {
Eigen::Vector<double, 2> u{leftVoltage.value(), rightVoltage.value()};
Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
// Find unconstrained wheel accelerations
Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
Eigen::Vector<double, 2> dxdt = m_system.A() * x + m_system.B() * u;
Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
// Converts from wheel accelerations to linear and angular acceleration
// a = (dxdt(0) + dxdt(1)) / 2.0
// alpha = (dxdt(1) - dxdt(0)) / trackwidth
Eigen::Matrix<double, 2, 2> M{
{0.5, 0.5}, {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
Matrixd<2, 2> M{{0.5, 0.5},
{-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
// Convert to linear and angular accelerations, constrain them, then convert
// back
Eigen::Vector<double, 2> accels = M * dxdt;
Vectord<2> accels = M * dxdt;
if (accels(0) > m_maxLinearAccel.value()) {
accels(0) = m_maxLinearAccel.value();
} else if (accels(0) < -m_maxLinearAccel.value()) {

48
wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
View File

@@ -7,60 +7,60 @@
namespace frc {
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt)
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(const Matrixd<1, 1>& A,
const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q,
const Matrixd<1, 1>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
const Matrixd<1, 1>& A, const Matrixd<1, 1>& B, const Matrixd<1, 1>& Q,
const Matrixd<1, 1>& R, const Matrixd<1, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt)
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q,
const Matrixd<1, 1>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
const Matrixd<2, 2>& A, const Matrixd<2, 1>& B, const Matrixd<2, 2>& Q,
const Matrixd<1, 1>& R, const Matrixd<2, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
units::second_t dt)
: LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
units::second_t dt)
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(const Matrixd<2, 2>& A,
const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q,
const Matrixd<2, 2>& R,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N, units::second_t dt)
const Matrixd<2, 2>& A, const Matrixd<2, 2>& B, const Matrixd<2, 2>& Q,
const Matrixd<2, 2>& R, const Matrixd<2, 2>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
} // namespace frc

40
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -19,9 +19,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
units::second_t nominalDt)
: m_observer(
&DifferentialDrivePoseEstimator::F,
[](const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 3>& u) {
return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
[](const Vectord<5>& x, const Vectord<3>& u) {
return Vectord<3>{x(3, 0), x(4, 0), x(2, 0)};
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
@@ -30,11 +29,10 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
[](const Vectord<5>& x, const Vectord<3>&) {
return x.block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
@@ -73,7 +71,7 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -97,13 +95,13 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto angle = gyroAngle + m_gyroOffset;
auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
auto u = Eigen::Vector<double, 3>{
(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
auto u = Vectord<3>{(wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0,
omega.value()};
m_previousAngle = angle;
auto localY = Eigen::Vector<double, 3>{
leftDistance.value(), rightDistance.value(), angle.Radians().value()};
auto localY = Vectord<3>{leftDistance.value(), rightDistance.value(),
angle.Radians().value()};
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
m_observer.Predict(u, dt);
@@ -112,24 +110,24 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
return GetEstimatedPosition();
}
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
Vectord<5> DifferentialDrivePoseEstimator::F(const Vectord<5>& x,
const Vectord<3>& u) {
// Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
// that for us.
auto& theta = x(2);
Eigen::Matrix<double, 5, 5> toFieldRotation{
Matrixd<5, 5> toFieldRotation{
{std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
{std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
{0.0, 0.0, 1.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 1.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 1.0}};
return toFieldRotation *
Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
Vectord<5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
}
template <int Dim>
wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& stdDevs) {
const Vectord<Dim>& stdDevs) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = stdDevs(i);
@@ -137,11 +135,11 @@ wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
return array;
}
Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
Vectord<5> DifferentialDrivePoseEstimator::FillStateVector(
const Pose2d& pose, units::meter_t leftDistance,
units::meter_t rightDistance) {
return Eigen::Vector<double, 5>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value(),
leftDistance.value(), rightDistance.value()};
return Vectord<5>{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value(), leftDistance.value(),
rightDistance.value()};
}

32
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -18,26 +18,21 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer(
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return u; },
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create vision correction mechanism.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
return x;
},
u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
@@ -76,7 +71,7 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -102,11 +97,10 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(),
omega.value()};
Vectord<3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(), omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
Vectord<1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());

14
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -25,8 +25,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Vector<double, 4> wheelsVector =
m_inverseKinematics * chassisSpeedsVector;
Vectord<4> wheelsVector = m_inverseKinematics * chassisSpeedsVector;
MecanumDriveWheelSpeeds wheelSpeeds;
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
@@ -38,7 +37,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const {
Eigen::Vector<double, 4> wheelSpeedsVector{
Vectord<4> wheelSpeedsVector{
wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
@@ -54,9 +53,8 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,
Translation2d rr) const {
m_inverseKinematics =
Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
{1, 1, (fr.X() - fr.Y()).value()},
{1, 1, (rl.X() - rl.Y()).value()},
{1, -1, (-(rr.X() + rr.Y())).value()}};
m_inverseKinematics = Matrixd<4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
{1, 1, (fr.X() - fr.Y()).value()},
{1, 1, (rl.X() - rl.Y()).value()},
{1, -1, (-(rr.X() + rr.Y())).value()}};
}

23
wpimath/src/main/native/include/frc/EigenCore.h Normal file
View File

@@ -0,0 +1,23 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "Eigen/Core"
namespace frc {
template <int Size>
using Vectord = Eigen::Vector<double, Size>;
template <int Rows, int Cols,
int Options = Eigen::AutoAlign |
((Rows == 1 && Cols != 1) ? Eigen::RowMajor
: (Cols == 1 && Rows != 1)
? Eigen::ColMajor
: EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION),
int MaxRows = Rows, int MaxCols = Cols>
using Matrixd = Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
} // namespace frc

67
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -13,9 +13,9 @@
#include <wpi/SymbolExports.h>
#include <wpi/deprecated.h>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
namespace frc {
@@ -64,9 +64,9 @@ void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) {
}
template <int States, int Inputs>
bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
bool IsStabilizableImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> es{A};
for (int i = 0; i < States; ++i) {
if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -107,12 +107,12 @@ template <int Rows, int Cols, typename... Ts,
std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
WPI_DEPRECATED(
"Use Eigen::Matrix or Eigen::Vector initializer list constructor")
Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
Matrixd<Rows, Cols> MakeMatrix(Ts... elems) {
static_assert(
sizeof...(elems) == Rows * Cols,
"Number of provided elements doesn't match matrix dimensionality");
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
detail::MatrixImpl<Rows, Cols>(result, elems...);
return result;
}
@@ -132,8 +132,7 @@ Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
Ts... tolerances) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(Ts... tolerances) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CostMatrixImpl(result.diagonal(), tolerances...);
return result;
@@ -153,8 +152,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
*/
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
Ts... stdDevs) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Eigen::DiagonalMatrix<double, sizeof...(Ts)> result;
detail::CovMatrixImpl(result.diagonal(), stdDevs...);
return result;
@@ -173,7 +171,7 @@ Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(
* @return State excursion or control effort cost matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -195,8 +193,7 @@ Eigen::Matrix<double, N, N> MakeCostMatrix(const std::array<double, N>& costs) {
* @return Process noise or measurement noise covariance matrix.
*/
template <size_t N>
Eigen::Matrix<double, N, N> MakeCovMatrix(
const std::array<double, N>& stdDevs) {
Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
Eigen::DiagonalMatrix<double, N> result;
auto& diag = result.diagonal();
for (size_t i = 0; i < N; ++i) {
@@ -207,8 +204,8 @@ Eigen::Matrix<double, N, N> MakeCovMatrix(
template <typename... Ts, typename = std::enable_if_t<
std::conjunction_v<std::is_same<double, Ts>...>>>
Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Eigen::Matrix<double, sizeof...(Ts), 1> result;
Matrixd<sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
Matrixd<sizeof...(Ts), 1> result;
detail::WhiteNoiseVectorImpl(result, stdDevs...);
return result;
}
@@ -222,12 +219,11 @@ Eigen::Matrix<double, sizeof...(Ts), 1> MakeWhiteNoiseVector(Ts... stdDevs) {
* @return White noise vector.
*/
template <int N>
Eigen::Vector<double, N> MakeWhiteNoiseVector(
const std::array<double, N>& stdDevs) {
Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Eigen::Vector<double, N> result;
Vectord<N> result;
for (int i = 0; i < N; ++i) {
// Passing a standard deviation of 0.0 to std::normal_distribution is
// undefined behavior
@@ -249,7 +245,7 @@ Eigen::Vector<double, N> MakeWhiteNoiseVector(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
Vectord<3> PoseTo3dVector(const Pose2d& pose);
/**
* Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
@@ -259,7 +255,7 @@ Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
Vectord<4> PoseTo4dVector(const Pose2d& pose);
/**
* Returns true if (A, B) is a stabilizable pair.
@@ -274,8 +270,8 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
* @param B Input matrix.
*/
template <int States, int Inputs>
bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
bool IsStabilizable(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
return detail::IsStabilizableImpl<States, Inputs>(A, B);
}
@@ -292,8 +288,8 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
* @param C Output matrix.
*/
template <int States, int Outputs>
bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, Outputs, States>& C) {
bool IsDetectable(const Matrixd<States, States>& A,
const Matrixd<Outputs, States>& C) {
return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
C.transpose());
}
@@ -301,14 +297,14 @@ bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
const Matrixd<1, 1>& B);
// Template specializations are used here to make common state-input pairs
// compile faster.
template <>
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
const Matrixd<2, 1>& B);
/**
* Converts a Pose2d into a vector of [x, y, theta].
@@ -318,7 +314,7 @@ WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
* @return The vector.
*/
WPILIB_DLLEXPORT
Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
Vectord<3> PoseToVector(const Pose2d& pose);
/**
* Clamps input vector between system's minimum and maximum allowable input.
@@ -330,11 +326,10 @@ Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
* @return Clamped input vector.
*/
template <int Inputs>
Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Inputs>& umin,
const Eigen::Vector<double, Inputs>& umax) {
Eigen::Vector<double, Inputs> result;
Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
result(i) = std::clamp(u(i), umin(i), umax(i));
}
@@ -351,8 +346,8 @@ Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
* @return The normalizedInput
*/
template <int Inputs>
Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
if (maxValue > maxMagnitude) {

58
wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
@@ -39,6 +39,9 @@ namespace frc {
template <int States, int Inputs>
class ControlAffinePlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with given model dynamics as a function
* of state and input.
@@ -50,15 +53,11 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<StateVector(const StateVector&, const InputVector&)> f,
units::second_t dt)
: m_dt(dt), m_f(f) {
m_B = NumericalJacobianU<States, States, Inputs>(
f, Eigen::Vector<double, States>::Zero(),
Eigen::Vector<double, Inputs>::Zero());
m_B = NumericalJacobianU<States, States, Inputs>(f, StateVector::Zero(),
InputVector::Zero());
Reset();
}
@@ -73,14 +72,12 @@ class ControlAffinePlantInversionFeedforward {
* @param dt The timestep between calls of calculate().
*/
ControlAffinePlantInversionFeedforward(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
f,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
std::function<StateVector(const StateVector&)> f,
const Matrixd<States, Inputs>& B, units::second_t dt)
: m_B(B), m_dt(dt) {
m_f = [=](const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u)
-> Eigen::Vector<double, States> { return f(x); };
m_f = [=](const StateVector& x, const InputVector& u) -> StateVector {
return f(x);
};
Reset();
}
@@ -95,7 +92,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -111,7 +108,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -127,7 +124,7 @@ class ControlAffinePlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -146,15 +143,14 @@ class ControlAffinePlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -166,36 +162,30 @@ class ControlAffinePlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
StateVector rDot = (nextR - r) / m_dt.value();
m_uff = m_B.householderQr().solve(
rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
m_uff = m_B.householderQr().solve(rDot - m_f(r, InputVector::Zero()));
m_r = nextR;
return m_uff;
}
private:
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
/**
* The model dynamics.
*/
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

25
wpimath/src/main/native/include/frc/controller/ImplicitModelFollower.h
View File

@@ -6,8 +6,8 @@
#include <frc/system/LinearSystem.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -27,6 +27,9 @@ namespace frc {
template <int States, int Inputs>
class ImplicitModelFollower {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -47,10 +50,10 @@ class ImplicitModelFollower {
* @param Aref Continuous system matrix whose dynamics should be followed.
* @param Bref Continuous input matrix whose dynamics should be followed.
*/
ImplicitModelFollower(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Aref,
const Eigen::Matrix<double, States, Inputs>& Bref) {
ImplicitModelFollower(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Aref,
const Matrixd<States, Inputs>& Bref) {
// Find u_imf that makes real model match reference model.
//
// dx/dt = Ax + Bu_imf
@@ -79,7 +82,7 @@ class ImplicitModelFollower {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -101,22 +104,20 @@ class ImplicitModelFollower {
* @param x The current state x.
* @param u The current input for the original model.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u) {
InputVector Calculate(const StateVector& x, const InputVector& u) {
m_u = m_A * x + m_B * u;
return m_u;
}
private:
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// State space conversion gain
Eigen::Matrix<double, Inputs, States> m_A;
Matrixd<Inputs, States> m_A;
// Input space conversion gain
Eigen::Matrix<double, Inputs, Inputs> m_B;
Matrixd<Inputs, Inputs> m_B;
};
} // namespace frc

34
wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
View File

@@ -7,8 +7,8 @@
#include <array>
#include <functional>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -31,6 +31,9 @@ namespace frc {
template <int States, int Inputs>
class LinearPlantInversionFeedforward {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
/**
* Constructs a feedforward with the given plant.
*
@@ -50,9 +53,9 @@ class LinearPlantInversionFeedforward {
* @param B Continuous input matrix of the plant being controlled.
* @param dt Discretization timestep.
*/
LinearPlantInversionFeedforward(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
LinearPlantInversionFeedforward(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
units::second_t dt)
: m_dt(dt) {
DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
Reset();
@@ -63,7 +66,7 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
const InputVector& Uff() const { return m_uff; }
/**
* Returns an element of the previously calculated feedforward.
@@ -79,7 +82,7 @@ class LinearPlantInversionFeedforward {
*
* @return The current reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -95,7 +98,7 @@ class LinearPlantInversionFeedforward {
*
* @param initialState The initial state vector.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_r = initialState;
m_uff.setZero();
}
@@ -114,15 +117,14 @@ class LinearPlantInversionFeedforward {
* reference.
*
* If this method is used the initial state of the system is the one set using
* Reset(const Eigen::Vector<double, States>&). If the initial state is not
* Reset(const StateVector&). If the initial state is not
* set it defaults to a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& nextR) {
return Calculate(m_r, nextR);
}
@@ -134,25 +136,23 @@ class LinearPlantInversionFeedforward {
*
* @return The calculated feedforward.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& r,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& r, const StateVector& nextR) {
m_uff = m_B.householderQr().solve(nextR - (m_A * r));
m_r = nextR;
return m_uff;
}
private:
Eigen::Matrix<double, States, States> m_A;
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, States> m_A;
Matrixd<States, Inputs> m_B;
units::second_t m_dt;
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed feedforward
Eigen::Vector<double, Inputs> m_uff;
InputVector m_uff;
};
} // namespace frc

151
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -12,9 +12,9 @@
#include <wpi/array.h>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
@@ -39,6 +39,12 @@ namespace detail {
template <int States, int Inputs>
class LinearQuadraticRegulatorImpl {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using StateArray = wpi::array<double, States>;
using InputArray = wpi::array<double, Inputs>;
/**
* Constructs a controller with the given coefficients and plant.
*
@@ -50,8 +56,7 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
LinearQuadraticRegulatorImpl(
const LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems, units::second_t dt)
const StateArray& Qelems, const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
}
@@ -64,11 +69,10 @@ class LinearQuadraticRegulatorImpl {
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const StateArray& Qelems,
const InputArray& Relems, units::second_t dt)
: LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
MakeCostMatrix(Relems), dt) {}
@@ -81,13 +85,13 @@ class LinearQuadraticRegulatorImpl {
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
if (!IsStabilizable<States, Inputs>(discA, discB)) {
@@ -100,7 +104,7 @@ class LinearQuadraticRegulatorImpl {
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
@@ -121,17 +125,17 @@ class LinearQuadraticRegulatorImpl {
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
LinearQuadraticRegulatorImpl(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Eigen::Matrix<double, States, States> S =
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
@@ -149,7 +153,7 @@ class LinearQuadraticRegulatorImpl {
/**
* Returns the controller matrix K.
*/
const Eigen::Matrix<double, Inputs, States>& K() const { return m_K; }
const Matrixd<Inputs, States>& K() const { return m_K; }
/**
* Returns an element of the controller matrix K.
@@ -164,7 +168,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The reference vector.
*/
const Eigen::Vector<double, States>& R() const { return m_r; }
const StateVector& R() const { return m_r; }
/**
* Returns an element of the reference vector r.
@@ -180,7 +184,7 @@ class LinearQuadraticRegulatorImpl {
*
* @return The control input.
*/
const Eigen::Vector<double, Inputs>& U() const { return m_u; }
const InputVector& U() const { return m_u; }
/**
* Returns an element of the control input vector u.
@@ -204,8 +208,7 @@ class LinearQuadraticRegulatorImpl {
*
* @param x The current state x.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x) {
InputVector Calculate(const StateVector& x) {
m_u = m_K * (m_r - x);
return m_u;
}
@@ -216,9 +219,7 @@ class LinearQuadraticRegulatorImpl {
* @param x The current state x.
* @param nextR The next reference vector r.
*/
Eigen::Vector<double, Inputs> Calculate(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, States>& nextR) {
InputVector Calculate(const StateVector& x, const StateVector& nextR) {
m_r = nextR;
return Calculate(x);
}
@@ -242,8 +243,8 @@ class LinearQuadraticRegulatorImpl {
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
@@ -251,13 +252,13 @@ class LinearQuadraticRegulatorImpl {
private:
// Current reference
Eigen::Vector<double, States> m_r;
StateVector m_r;
// Computed controller output
Eigen::Vector<double, Inputs> m_u;
InputVector m_u;
// Controller gain
Eigen::Matrix<double, Inputs, States> m_K;
Matrixd<Inputs, States> m_K;
};
} // namespace detail
@@ -291,8 +292,8 @@ class LinearQuadraticRegulator
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const wpi::array<double, States>& Qelems,
const wpi::array<double, Inputs>& Relems,
units::second_t dt)
@@ -308,11 +309,10 @@ class LinearQuadraticRegulator
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
units::second_t dt)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
/**
@@ -325,12 +325,11 @@ class LinearQuadraticRegulator
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
units::second_t dt)
LinearQuadraticRegulator(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q,
const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, Inputs>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
R, N, dt} {}
@@ -351,24 +350,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const wpi::array<double, 1>& Qelems,
const wpi::array<double, 1>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q,
const Eigen::Matrix<double, 1, 1>& R,
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B,
const Matrixd<1, 1>& Q, const Matrixd<1, 1>& R,
const Matrixd<1, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -387,24 +380,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const wpi::array<double, 2>& Qelems,
const wpi::array<double, 1>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B,
const Matrixd<2, 2>& Q, const Matrixd<1, 1>& R,
const Matrixd<2, 1>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
@@ -423,24 +410,18 @@ class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
units::second_t dt)
: LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const wpi::array<double, 2>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 2>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 2, 2>& R,
const Eigen::Matrix<double, 2, 2>& N,
units::second_t dt);
LinearQuadraticRegulator(const Matrixd<2, 2>& A, const Matrixd<2, 2>& B,
const Matrixd<2, 2>& Q, const Matrixd<2, 2>& R,
const Matrixd<2, 2>& N, units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;

6
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -6,7 +6,7 @@
#include <wpi/MathExtras.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
@@ -70,8 +70,8 @@ class SimpleMotorFeedforward {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Eigen::Vector<double, 1> r{currentVelocity.value()};
Eigen::Vector<double, 1> nextR{nextVelocity.value()};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};

35
wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
View File

@@ -6,7 +6,7 @@
#include <wpi/numbers>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/MathUtil.h"
namespace frc {
@@ -21,10 +21,9 @@ namespace frc {
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Vector<double, States> AngleResidual(
const Eigen::Vector<double, States>& a,
const Eigen::Vector<double, States>& b, int angleStateIdx) {
Eigen::Vector<double, States> ret = a - b;
Vectord<States> AngleResidual(const Vectord<States>& a,
const Vectord<States>& b, int angleStateIdx) {
Vectord<States> ret = a - b;
ret[angleStateIdx] =
AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
return ret;
@@ -38,8 +37,7 @@ Eigen::Vector<double, States> AngleResidual(
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleResidual(int angleStateIdx) {
return [=](auto a, auto b) {
return AngleResidual<States>(a, b, angleStateIdx);
@@ -56,10 +54,9 @@ AngleResidual(int angleStateIdx) {
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
const Eigen::Vector<double, States>& b,
int angleStateIdx) {
Eigen::Vector<double, States> ret = a + b;
Vectord<States> AngleAdd(const Vectord<States>& a, const Vectord<States>& b,
int angleStateIdx) {
Vectord<States> ret = a + b;
ret[angleStateIdx] =
InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
return ret;
@@ -73,8 +70,7 @@ Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
* @param angleStateIdx The row containing angles to be normalized.
*/
template <int States>
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
AngleAdd(int angleStateIdx) {
return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
}
@@ -91,9 +87,9 @@ AngleAdd(int angleStateIdx) {
* @param angleStatesIdx The row containing the angles.
*/
template <int CovDim, int States>
Eigen::Vector<double, CovDim> AngleMean(
const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm,
int angleStatesIdx) {
double sumSin = sigmas.row(angleStatesIdx)
.unaryExpr([](auto it) { return std::sin(it); })
.sum();
@@ -101,7 +97,7 @@ Eigen::Vector<double, CovDim> AngleMean(
.unaryExpr([](auto it) { return std::cos(it); })
.sum();
Eigen::Vector<double, CovDim> ret = sigmas * Wm;
Vectord<CovDim> ret = sigmas * Wm;
ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
return ret;
}
@@ -116,9 +112,8 @@ Eigen::Vector<double, CovDim> AngleMean(
* @param angleStateIdx The row containing the angles.
*/
template <int CovDim, int States>
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
AngleMean(int angleStateIdx) {
return [=](auto sigmas, auto Wm) {
return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);

19
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
@@ -223,11 +223,9 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
private:
UnscentedKalmanFilter<5, 3, 3> m_observer;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix<double, 3, 3> m_visionContR;
Matrixd<3, 3> m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;
@@ -237,13 +235,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& stdDevs);
const Vectord<Dim>& stdDevs);
static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 3>& u);
static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
units::meter_t leftDistance,
units::meter_t rightDistance);
static Vectord<5> F(const Vectord<5>& x, const Vectord<3>& u);
static Vectord<5> FillStateVector(const Pose2d& pose,
units::meter_t leftDistance,
units::meter_t rightDistance);
};
} // namespace frc

131
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -8,7 +8,7 @@
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {
@@ -40,6 +40,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class ExtendedKalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
/**
* Constructs an extended Kalman filter.
*
@@ -51,17 +60,11 @@ class ExtendedKalmanFilter {
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
/**
* Constructs an extended Kalman filter.
@@ -77,30 +80,20 @@ class ExtendedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
* @param dt Nominal discretization timestep.
*/
ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
units::second_t dt);
ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
/**
* Returns the error covariance matrix P.
*/
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
const StateMatrix& P() const { return m_P; }
/**
* Returns an element of the error covariance matrix P.
@@ -115,12 +108,12 @@ class ExtendedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_P = P; }
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -134,7 +127,7 @@ class ExtendedKalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -158,7 +151,7 @@ class ExtendedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -166,19 +159,15 @@ class ExtendedKalmanFilter {
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
}
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -197,46 +186,28 @@ class ExtendedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&,
const Eigen::Vector<double, Rows>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
addFuncX);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
private:
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_h;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>)>
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
m_addFuncX;
Eigen::Vector<double, States> m_xHat;
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
units::second_t m_dt;
Eigen::Matrix<double, States, States> m_initP;
StateMatrix m_initP;
};
} // namespace frc

138
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -16,72 +16,47 @@ namespace frc {
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt)
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
return a - b;
};
m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
m_dt = dt;
Reset();
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
Eigen::Matrix<double, Outputs, States> C =
NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
m_contQ = MakeCovMatrix(stateStdDevs);
@@ -90,39 +65,36 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Reset();
Eigen::Matrix<double, States, States> contA =
NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
Eigen::Matrix<double, Outputs, States> C =
NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, Outputs, Outputs> discR =
DiscretizeR<Outputs>(m_contR, dt);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = Eigen::Matrix<double, States, States>::Zero();
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
const InputVector& u, units::second_t dt) {
// Find continuous A
Eigen::Matrix<double, States, States> contA =
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
@@ -136,44 +108,29 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
return a - b;
};
auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Eigen::Matrix<double, Rows, States> C =
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
@@ -187,7 +144,7 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Eigen::Matrix<double, States, Rows> K =
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
@@ -195,9 +152,8 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P *
(Eigen::Matrix<double, States, States>::Identity() - K * C)
.transpose() +
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}

29
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
@@ -36,6 +36,13 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class KalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
/**
* Constructs a state-space observer with the given plant.
*
@@ -45,9 +52,8 @@ class KalmanFilter {
* @param dt Nominal discretization timestep.
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs, units::second_t dt);
KalmanFilter(KalmanFilter&&) = default;
KalmanFilter& operator=(KalmanFilter&&) = default;
@@ -55,7 +61,7 @@ class KalmanFilter {
/**
* Returns the steady-state Kalman gain matrix K.
*/
const Eigen::Matrix<double, States, Outputs>& K() const { return m_K; }
const Matrixd<States, Outputs>& K() const { return m_K; }
/**
* Returns an element of the steady-state Kalman gain matrix K.
@@ -68,7 +74,7 @@ class KalmanFilter {
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -82,7 +88,7 @@ class KalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -103,7 +109,7 @@ class KalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -111,8 +117,7 @@ class KalmanFilter {
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y);
void Correct(const InputVector& u, const OutputVector& y);
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -120,12 +125,12 @@ class KalmanFilter {
/**
* The steady-state Kalman gain matrix.
*/
Eigen::Matrix<double, States, Outputs> m_K;
Matrixd<States, Outputs> m_K;
/**
* The state estimate.
*/
Eigen::Vector<double, States> m_xHat;
StateVector m_xHat;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

24
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -21,15 +21,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt) {
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
@@ -46,12 +46,11 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
throw std::invalid_argument(msg);
}
Eigen::Matrix<double, States, States> P =
drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
@@ -71,15 +70,14 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void KalmanFilter<States, Inputs, Outputs>::Correct(const InputVector& u,
const OutputVector& y) {
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
}

27
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -10,7 +10,7 @@
#include <utility>
#include <vector>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/math.h"
#include "units/time.h"
@@ -20,14 +20,13 @@ template <int States, int Inputs, int Outputs, typename KalmanFilterType>
class KalmanFilterLatencyCompensator {
public:
struct ObserverSnapshot {
Eigen::Vector<double, States> xHat;
Eigen::Matrix<double, States, States> errorCovariances;
Eigen::Vector<double, Inputs> inputs;
Eigen::Vector<double, Outputs> localMeasurements;
Vectord<States> xHat;
Matrixd<States, States> errorCovariances;
Vectord<Inputs> inputs;
Vectord<Outputs> localMeasurements;
ObserverSnapshot(const KalmanFilterType& observer,
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& localY)
ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
const Vectord<Outputs>& localY)
: xHat(observer.Xhat()),
errorCovariances(observer.P()),
inputs(u),
@@ -47,10 +46,8 @@ class KalmanFilterLatencyCompensator {
* @param localY The local output at the timestamp
* @param timestamp The timesnap of the state.
*/
void AddObserverState(const KalmanFilterType& observer,
Eigen::Vector<double, Inputs> u,
Eigen::Vector<double, Outputs> localY,
units::second_t timestamp) {
void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
Vectord<Outputs> localY, units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -74,10 +71,8 @@ class KalmanFilterLatencyCompensator {
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, units::second_t nominalDt,
Eigen::Vector<double, Rows> y,
std::function<void(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y)>
KalmanFilterType* observer, units::second_t nominalDt, Vectord<Rows> y,
std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
units::second_t timestamp) {
if (m_pastObserverSnapshots.size() == 0) {

8
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -9,7 +9,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
@@ -213,9 +213,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
UnscentedKalmanFilter<3, 3, 1> m_observer;
MecanumDriveKinematics m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;
@@ -227,7 +225,7 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& vector) {
const Vectord<Dim>& vector) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);

24
wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
View File

@@ -7,7 +7,7 @@
#include <cmath>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -62,14 +62,12 @@ class MerweScaledSigmaPoints {
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*
*/
Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
const Eigen::Vector<double, States>& x,
const Eigen::Matrix<double, States, States>& P) {
Matrixd<States, 2 * States + 1> SigmaPoints(
const Vectord<States>& x, const Matrixd<States, States>& P) {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
Eigen::Matrix<double, States, States> U =
((lambda + States) * P).llt().matrixL();
Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
Eigen::Matrix<double, States, 2 * States + 1> sigmas;
Matrixd<States, 2 * States + 1> sigmas;
sigmas.template block<States, 1>(0, 0) = x;
for (int k = 0; k < States; ++k) {
sigmas.template block<States, 1>(0, k + 1) =
@@ -84,7 +82,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the mean.
*/
const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
/**
* Returns an element of the weight for each sigma point for the mean.
@@ -96,7 +94,7 @@ class MerweScaledSigmaPoints {
/**
* Returns the weight for each sigma point for the covariance.
*/
const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
/**
* Returns an element of the weight for each sigma point for the covariance.
@@ -106,8 +104,8 @@ class MerweScaledSigmaPoints {
double Wc(int i) const { return m_Wc(i, 0); }
private:
Eigen::Vector<double, 2 * States + 1> m_Wm;
Eigen::Vector<double, 2 * States + 1> m_Wc;
Vectord<2 * States + 1> m_Wm;
Vectord<2 * States + 1> m_Wc;
double m_alpha;
int m_kappa;
@@ -120,8 +118,8 @@ class MerweScaledSigmaPoints {
double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
double c = 0.5 / (States + lambda);
m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
m_Wm = Vectord<2 * States + 1>::Constant(c);
m_Wc = Vectord<2 * States + 1>::Constant(c);
m_Wm(0) = lambda / (States + lambda);
m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);

29
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -10,7 +10,7 @@
#include <wpi/array.h>
#include <wpi/timestamp.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
@@ -83,10 +83,8 @@ class SwerveDrivePoseEstimator {
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s)
: m_observer([](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return u; },
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) {
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs,
@@ -98,12 +96,9 @@ class SwerveDrivePoseEstimator {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y) {
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y,
[](const Eigen::Vector<double, 3>& x,
const Eigen::Vector<double, 3>& u) { return x; },
u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
};
@@ -190,7 +185,7 @@ class SwerveDrivePoseEstimator {
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
m_visionCorrect(Vectord<3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -280,10 +275,10 @@ class SwerveDrivePoseEstimator {
Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
.RotateBy(angle);
Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Vectord<3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Eigen::Vector<double, 1> localY{angle.Radians().value()};
Vectord<1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
@@ -298,9 +293,7 @@ class SwerveDrivePoseEstimator {
UnscentedKalmanFilter<3, 3, 1> m_observer;
SwerveDriveKinematics<NumModules>& m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Eigen::Vector<double, 3>& u,
const Eigen::Vector<double, 3>& y)>
m_visionCorrect;
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;
@@ -312,7 +305,7 @@ class SwerveDrivePoseEstimator {
template <int Dim>
static wpi::array<double, Dim> StdDevMatrixToArray(
const Eigen::Vector<double, Dim>& vector) {
const Vectord<Dim>& vector) {
wpi::array<double, Dim> array;
for (size_t i = 0; i < Dim; ++i) {
array[i] = vector(i);

165
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -9,7 +9,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "units/time.h"
@@ -42,6 +42,15 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class UnscentedKalmanFilter {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
using StateArray = wpi::array<double, States>;
using OutputArray = wpi::array<double, Outputs>;
using StateMatrix = Matrixd<States, States>;
/**
* Constructs an unscented Kalman filter.
*
@@ -53,17 +62,11 @@ class UnscentedKalmanFilter {
* @param measurementStdDevs Standard deviations of measurements.
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
units::second_t dt);
UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
@@ -90,42 +93,27 @@ class UnscentedKalmanFilter {
* @param dt Nominal discretization timestep.
*/
UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
/**
* Returns the error covariance matrix P.
*/
const Eigen::Matrix<double, States, States>& P() const { return m_P; }
const StateMatrix& P() const { return m_P; }
/**
* Returns an element of the error covariance matrix P.
@@ -140,12 +128,12 @@ class UnscentedKalmanFilter {
*
* @param P The error covariance matrix P.
*/
void SetP(const Eigen::Matrix<double, States, States>& P) { m_P = P; }
void SetP(const StateMatrix& P) { m_P = P; }
/**
* Returns the state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
const StateVector& Xhat() const { return m_xHat; }
/**
* Returns an element of the state estimate x-hat.
@@ -159,7 +147,7 @@ class UnscentedKalmanFilter {
*
* @param xHat The state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
void SetXhat(const StateVector& xHat) { m_xHat = xHat; }
/**
* Set an element of the initial state estimate x-hat.
@@ -184,7 +172,7 @@ class UnscentedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -192,8 +180,7 @@ class UnscentedKalmanFilter {
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Outputs>& y) {
void Correct(const InputVector& u, const OutputVector& y) {
Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
m_residualFuncX, m_addFuncX);
}
@@ -212,13 +199,10 @@ class UnscentedKalmanFilter {
* @param R Measurement noise covariance matrix (continuous-time).
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -241,64 +225,39 @@ class UnscentedKalmanFilter {
* @param addFuncX A function that adds two state vectors.
*/
template <int Rows>
void Correct(const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Matrix<double, Rows, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
meanFuncY,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&,
const Eigen::Vector<double, Rows>&)>
residualFuncY,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
residualFuncX,
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
addFuncX);
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
private:
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_f;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
m_h;
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
std::function<OutputVector(const StateVector&, const InputVector&)> m_h;
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
m_meanFuncX;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
m_meanFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
m_residualFuncX;
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
std::function<Eigen::Vector<double, States>(
const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
m_addFuncX;
Eigen::Vector<double, States> m_xHat;
Eigen::Matrix<double, States, States> m_P;
Eigen::Matrix<double, States, States> m_contQ;
Eigen::Matrix<double, Outputs, Outputs> m_contR;
Eigen::Matrix<double, States, 2 * States + 1> m_sigmasF;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;
Matrixd<States, 2 * States + 1> m_sigmasF;
units::second_t m_dt;
MerweScaledSigmaPoints<States> m_pts;

142
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -16,34 +16,20 @@ namespace frc {
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs, units::second_t dt)
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
return sigmas * Wm;
};
m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a - b;
};
m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
return a - b;
};
m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
m_residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
m_dt = dt;
Reset();
@@ -51,36 +37,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
f,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& measurementStdDevs,
std::function<Eigen::Vector<double, States>(
const Eigen::Matrix<double, States, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<Eigen::Vector<double, Outputs>(
const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
const Eigen::Vector<double, Outputs>&)>
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
addFuncX,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f),
m_h(h),
@@ -98,21 +68,20 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
template <int States, int Inputs, int Outputs>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
Eigen::Matrix<double, States, States> contA =
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, States> discQ;
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
@@ -127,59 +96,38 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R) {
auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](auto sigmas, auto Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a - b;
};
auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
return a - b;
};
auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
return a + b;
};
auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const Eigen::Vector<double, Inputs>& u,
const Eigen::Vector<double, Rows>& y,
std::function<
Eigen::Vector<double, Rows>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, Inputs>&)>
h,
const Eigen::Matrix<double, Rows, Rows>& R,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Matrix<double, Rows, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Eigen::Vector<double, Rows>(
const Eigen::Vector<double, Rows>&, const Eigen::Vector<double, Rows>&)>
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>&)>
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<
Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
const Eigen::Vector<double, States>)>
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
// Transform sigma points into measurement space
Eigen::Matrix<double, Rows, 2 * States + 1> sigmasH;
Eigen::Matrix<double, States, 2 * States + 1> sigmas =
m_pts.SigmaPoints(m_xHat, m_P);
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
@@ -191,7 +139,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
Py += discR;
// Compute cross covariance of the state and the measurements
Eigen::Matrix<double, States, Rows> Pxy;
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
@@ -206,7 +154,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
// P_yᵀKᵀ = P_{xy}ᵀ
// Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
// K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
Eigen::Matrix<double, States, Rows> K =
Matrixd<States, Rows> K =
Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)

29
wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
View File

@@ -6,7 +6,7 @@
#include <tuple>
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -31,33 +31,30 @@ namespace frc {
* passing through the transform.
*/
template <int CovDim, int States>
std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
const Eigen::Vector<double, 2 * States + 1>& Wm,
const Eigen::Vector<double, 2 * States + 1>& Wc,
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
const Eigen::Vector<double, 2 * States + 1>&)>
meanFunc,
std::function<Eigen::Vector<double, CovDim>(
const Eigen::Vector<double, CovDim>&,
const Eigen::Vector<double, CovDim>&)>
residualFunc) {
std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
const Matrixd<CovDim, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFunc,
std::function<Vectord<CovDim>(const Vectord<CovDim>&,
const Vectord<CovDim>&)>
residualFunc) {
// New mean is usually just the sum of the sigmas * weight:
// n
// dot = Σ W[k] Xᵢ[k]
// k=1
Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
Vectord<CovDim> x = meanFunc(sigmas, Wm);
// New covariance is the sum of the outer product of the residuals times the
// weights
Eigen::Matrix<double, CovDim, 2 * States + 1> y;
Matrixd<CovDim, 2 * States + 1> y;
for (int i = 0; i < 2 * States + 1; ++i) {
// y[:, i] = sigmas[:, i] - x
y.template block<CovDim, 1>(0, i) =
residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
}
Eigen::Matrix<double, CovDim, CovDim> P =
Matrixd<CovDim, CovDim> P =
y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
return std::make_tuple(x, P);

8
wpimath/src/main/native/include/frc/filter/LinearFilter.h
View File

@@ -14,8 +14,8 @@
#include <wpi/circular_buffer.h>
#include <wpi/span.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -209,7 +209,7 @@ class LinearFilter {
static_assert(Derivative < Samples,
"Order of derivative must be less than number of samples.");
Eigen::Matrix<double, Samples, Samples> S;
Matrixd<Samples, Samples> S;
for (int row = 0; row < Samples; ++row) {
for (int col = 0; col < Samples; ++col) {
S(row, col) = std::pow(stencil[col], row);
@@ -217,12 +217,12 @@ class LinearFilter {
}
// Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
Eigen::Vector<double, Samples> d;
Vectord<Samples> d;
for (int i = 0; i < Samples; ++i) {
d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
}
Eigen::Vector<double, Samples> a =
Vectord<Samples> a =
S.householderQr().solve(d) / std::pow(period.value(), Derivative);
// Reverse gains list

6
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -6,8 +6,8 @@
#include <wpi/SymbolExports.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
@@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
private:
mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
mutable Matrixd<4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;
Translation2d m_frontLeftWheel;
Translation2d m_frontRightWheel;
Translation2d m_rearLeftWheel;

7
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -9,8 +9,8 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -179,9 +179,8 @@ class SwerveDriveKinematics {
units::meters_per_second_t attainableMaxSpeed);
private:
mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
m_forwardKinematics;
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable Translation2d m_previousCoR;

17
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -25,7 +25,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
for (size_t i = 0; i < NumModules; i++) {
// clang-format off
m_inverseKinematics.template block<2, 3>(i * 2, 0) =
Eigen::Matrix<double, 2, 3>{
Matrixd<2, 3>{
{1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
{0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
// clang-format on
@@ -37,13 +37,13 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_inverseKinematics * chassisSpeedsVector;
wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
for (size_t i = 0; i < NumModules; i++) {
units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
auto speed = units::math::hypot(x, y);
Rotation2d rotation{x.value(), y.value()};
@@ -70,17 +70,16 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
template <size_t NumModules>
ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
wpi::array<SwerveModuleState, NumModules> moduleStates) const {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
Matrixd<NumModules * 2, 1> moduleStateMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStatesMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) = module.speed.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_forwardKinematics.solve(moduleStatesMatrix);
m_forwardKinematics.solve(moduleStateMatrix);
return {units::meters_per_second_t{chassisSpeedsVector(0)},
units::meters_per_second_t{chassisSpeedsVector(1)},

19
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 4> m_coefficients =
Eigen::Matrix<double, 6, 4>::Zero();
Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
static Matrixd<4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
@@ -74,10 +71,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
static const Eigen::Matrix<double, 4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},
{+0.0, +1.0, +0.0, +0.0},
{+1.0, +0.0, +0.0, +0.0}};
return basis;
}

22
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -7,7 +7,7 @@
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/spline/Spline.h"
namespace frc {
@@ -40,19 +40,16 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* Returns the coefficients matrix.
* @return The coefficients matrix.
*/
Eigen::Matrix<double, 6, 6> Coefficients() const override {
return m_coefficients;
}
Matrixd<6, 6> Coefficients() const override { return m_coefficients; }
private:
Eigen::Matrix<double, 6, 6> m_coefficients =
Eigen::Matrix<double, 6, 6>::Zero();
Matrixd<6, 6> m_coefficients = Matrixd<6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
static Matrixd<6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
@@ -81,7 +78,7 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
static const Eigen::Matrix<double, 6, 6> basis{
static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
@@ -100,11 +97,10 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector<double, 6> ControlVectorFromArrays(
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
initialVector[2], finalVector[0],
finalVector[1], finalVector[2]};
static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
wpi::array<double, 3> finalVector) {
return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]};
}
};
} // namespace frc

8
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -9,7 +9,7 @@
#include <wpi/array.h>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "units/curvature.h"
#include "units/length.h"
@@ -55,7 +55,7 @@ class Spline {
* @return The pose and curvature at that point.
*/
PoseWithCurvature GetPoint(double t) const {
Eigen::Vector<double, Degree + 1> polynomialBases;
Vectord<Degree + 1> polynomialBases;
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
@@ -64,7 +64,7 @@ class Spline {
// This simply multiplies by the coefficients. We need to divide out t some
// n number of times where n is the derivative we want to take.
Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
Vectord<6> combined = Coefficients() * polynomialBases;
double dx, dy, ddx, ddy;
@@ -100,7 +100,7 @@ class Spline {
*
* @return The coefficients of the spline.
*/
virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
/**
* Converts a Translation2d into a vector that is compatible with Eigen.

62
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
#include "unsupported/Eigen/MatrixFunctions"
@@ -19,9 +19,8 @@ namespace frc {
* @param discA Storage for discrete system matrix.
*/
template <int States>
void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA) {
void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
Matrixd<States, States>* discA) {
*discA = (contA * dt.value()).exp();
}
@@ -37,19 +36,18 @@ void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
* @param discB Storage for discrete input matrix.
*/
template <int States, int Inputs>
void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, Inputs>& contB,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, Inputs>* discB) {
void DiscretizeAB(const Matrixd<States, States>& contA,
const Matrixd<States, Inputs>& contB, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, Inputs>* discB) {
// Matrices are blocked here to minimize matrix exponentiation calculations
Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
Matrixd<States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// Discretize A and B with the given timestep
Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
Matrixd<States + Inputs, States + Inputs> Mdisc = Mcont.exp();
*discA = Mdisc.template block<States, States>(0, 0);
*discB = Mdisc.template block<States, Inputs>(0, States);
}
@@ -65,29 +63,26 @@ void DiscretizeAB(const Eigen::Matrix<double, States, States>& contA,
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
void DiscretizeAQ(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
// Set up the matrix M = [[-A, Q], [0, A.T]]
Eigen::Matrix<double, 2 * States, 2 * States> M;
Matrixd<2 * States, 2 * States> M;
M.template block<States, States>(0, 0) = -contA;
M.template block<States, States>(0, States) = Q;
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
Eigen::Matrix<double, States, States> phi12 =
phi.block(0, States, States, States);
Eigen::Matrix<double, States, States> phi22 =
phi.block(States, States, States, States);
Matrixd<States, States> phi12 = phi.block(0, States, States, States);
Matrixd<States, States> phi22 = phi.block(States, States, States, States);
*discA = phi22.transpose();
@@ -117,21 +112,20 @@ void DiscretizeAQ(const Eigen::Matrix<double, States, States>& contA,
* @param discQ Storage for discrete process noise covariance matrix.
*/
template <int States>
void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
const Eigen::Matrix<double, States, States>& contQ,
units::second_t dt,
Eigen::Matrix<double, States, States>* discA,
Eigen::Matrix<double, States, States>* discQ) {
void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ,
units::second_t dt, Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// Make continuous Q symmetric if it isn't already
Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
Eigen::Matrix<double, States, States> lastTerm = Q;
Matrixd<States, States> lastTerm = Q;
double lastCoeff = dt.value();
// Aᵀⁿ
Eigen::Matrix<double, States, States> Atn = contA.transpose();
Matrixd<States, States> Atn = contA.transpose();
Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
Matrixd<States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
@@ -159,8 +153,8 @@ void DiscretizeAQTaylor(const Eigen::Matrix<double, States, States>& contA,
* @param dt Discretization timestep.
*/
template <int Outputs>
Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
units::second_t dt) {
return R / dt.value();
}

44
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -8,7 +8,7 @@
#include <functional>
#include <stdexcept>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "units/time.h"
@@ -30,6 +30,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystem {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a discrete plant with the given continuous system coefficients.
*
@@ -39,10 +43,10 @@ class LinearSystem {
* @param D Feedthrough matrix.
* @throws std::domain_error if any matrix element isn't finite.
*/
LinearSystem(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, Outputs, States>& C,
const Eigen::Matrix<double, Outputs, Inputs>& D) {
LinearSystem(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B,
const Matrixd<Outputs, States>& C,
const Matrixd<Outputs, Inputs>& D) {
if (!A.allFinite()) {
throw std::domain_error(
"Elements of A aren't finite. This is usually due to model "
@@ -78,7 +82,7 @@ class LinearSystem {
/**
* Returns the system matrix A.
*/
const Eigen::Matrix<double, States, States>& A() const { return m_A; }
const Matrixd<States, States>& A() const { return m_A; }
/**
* Returns an element of the system matrix A.
@@ -91,7 +95,7 @@ class LinearSystem {
/**
* Returns the input matrix B.
*/
const Eigen::Matrix<double, States, Inputs>& B() const { return m_B; }
const Matrixd<States, Inputs>& B() const { return m_B; }
/**
* Returns an element of the input matrix B.
@@ -104,7 +108,7 @@ class LinearSystem {
/**
* Returns the output matrix C.
*/
const Eigen::Matrix<double, Outputs, States>& C() const { return m_C; }
const Matrixd<Outputs, States>& C() const { return m_C; }
/**
* Returns an element of the output matrix C.
@@ -117,7 +121,7 @@ class LinearSystem {
/**
* Returns the feedthrough matrix D.
*/
const Eigen::Matrix<double, Outputs, Inputs>& D() const { return m_D; }
const Matrixd<Outputs, Inputs>& D() const { return m_D; }
/**
* Returns an element of the feedthrough matrix D.
@@ -137,11 +141,10 @@ class LinearSystem {
* @param clampedU The control input.
* @param dt Timestep for model update.
*/
Eigen::Vector<double, States> CalculateX(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
StateVector CalculateX(const StateVector& x, const InputVector& clampedU,
units::second_t dt) const {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
return discA * x + discB * clampedU;
@@ -156,9 +159,8 @@ class LinearSystem {
* @param x The current state.
* @param clampedU The control input.
*/
Eigen::Vector<double, Outputs> CalculateY(
const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& clampedU) const {
OutputVector CalculateY(const StateVector& x,
const InputVector& clampedU) const {
return m_C * x + m_D * clampedU;
}
@@ -166,22 +168,22 @@ class LinearSystem {
/**
* Continuous system matrix.
*/
Eigen::Matrix<double, States, States> m_A;
Matrixd<States, States> m_A;
/**
* Continuous input matrix.
*/
Eigen::Matrix<double, States, Inputs> m_B;
Matrixd<States, Inputs> m_B;
/**
* Output matrix.
*/
Eigen::Matrix<double, Outputs, States> m_C;
Matrixd<Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
Eigen::Matrix<double, Outputs, Inputs> m_D;
Matrixd<Outputs, Inputs> m_D;
};
} // namespace frc

62
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -35,6 +35,10 @@ namespace frc {
template <int States, int Inputs, int Outputs>
class LinearSystemLoop {
public:
using StateVector = Vectord<States>;
using InputVector = Vectord<Inputs>;
using OutputVector = Vectord<Outputs>;
/**
* Constructs a state-space loop with the given plant, controller, and
* observer. By default, the initial reference is all zeros. Users should
@@ -53,7 +57,7 @@ class LinearSystemLoop {
units::volt_t maxVoltage, units::second_t dt)
: LinearSystemLoop(
plant, controller, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
[=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
},
dt) {}
@@ -73,9 +77,7 @@ class LinearSystemLoop {
LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
LinearQuadraticRegulator<States, Inputs>& controller,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
clampFunction,
std::function<InputVector(const InputVector&)> clampFunction,
units::second_t dt)
: LinearSystemLoop(
controller,
@@ -97,11 +99,10 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
: LinearSystemLoop(controller, feedforward, observer,
[=](const Eigen::Vector<double, Inputs>& u) {
return frc::DesaturateInputVector<Inputs>(
u, maxVoltage.value());
}) {}
: LinearSystemLoop(
controller, feedforward, observer, [=](const InputVector& u) {
return frc::DesaturateInputVector<Inputs>(u, maxVoltage.value());
}) {}
/**
* Constructs a state-space loop with the given controller, feedforward,
@@ -117,9 +118,7 @@ class LinearSystemLoop {
LinearQuadraticRegulator<States, Inputs>& controller,
const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
KalmanFilter<States, Inputs, Outputs>& observer,
std::function<
Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
clampFunction)
std::function<InputVector(const InputVector&)> clampFunction)
: m_controller(&controller),
m_feedforward(feedforward),
m_observer(&observer),
@@ -134,9 +133,7 @@ class LinearSystemLoop {
/**
* Returns the observer's state estimate x-hat.
*/
const Eigen::Vector<double, States>& Xhat() const {
return m_observer->Xhat();
}
const StateVector& Xhat() const { return m_observer->Xhat(); }
/**
* Returns an element of the observer's state estimate x-hat.
@@ -148,7 +145,7 @@ class LinearSystemLoop {
/**
* Returns the controller's next reference r.
*/
const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
const StateVector& NextR() const { return m_nextR; }
/**
* Returns an element of the controller's next reference r.
@@ -160,7 +157,7 @@ class LinearSystemLoop {
/**
* Returns the controller's calculated control input u.
*/
Eigen::Vector<double, Inputs> U() const {
InputVector U() const {
return ClampInput(m_controller->U() + m_feedforward.Uff());
}
@@ -176,9 +173,7 @@ class LinearSystemLoop {
*
* @param xHat The initial state estimate x-hat.
*/
void SetXhat(const Eigen::Vector<double, States>& xHat) {
m_observer->SetXhat(xHat);
}
void SetXhat(const StateVector& xHat) { m_observer->SetXhat(xHat); }
/**
* Set an element of the initial state estimate x-hat.
@@ -193,7 +188,7 @@ class LinearSystemLoop {
*
* @param nextR Next reference.
*/
void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
void SetNextR(const StateVector& nextR) { m_nextR = nextR; }
/**
* Return the controller used internally.
@@ -225,7 +220,7 @@ class LinearSystemLoop {
*
* @param initialState The initial state.
*/
void Reset(const Eigen::Vector<double, States>& initialState) {
void Reset(const StateVector& initialState) {
m_nextR.setZero();
m_controller->Reset();
m_feedforward.Reset(initialState);
@@ -235,18 +230,14 @@ class LinearSystemLoop {
/**
* Returns difference between reference r and current state x-hat.
*/
Eigen::Vector<double, States> Error() const {
return m_controller->R() - m_observer->Xhat();
}
StateVector Error() const { return m_controller->R() - m_observer->Xhat(); }
/**
* Correct the state estimate x-hat using the measurements in y.
*
* @param y Measurement vector.
*/
void Correct(const Eigen::Vector<double, Outputs>& y) {
m_observer->Correct(U(), y);
}
void Correct(const OutputVector& y) { m_observer->Correct(U(), y); }
/**
* Sets new controller output, projects model forward, and runs observer
@@ -258,7 +249,7 @@ class LinearSystemLoop {
* @param dt Timestep for model update.
*/
void Predict(units::second_t dt) {
Eigen::Vector<double, Inputs> u =
InputVector u =
ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
m_feedforward.Calculate(m_nextR));
m_observer->Predict(u, dt);
@@ -270,10 +261,7 @@ class LinearSystemLoop {
* @param u Input vector to clamp.
* @return Clamped input vector.
*/
Eigen::Vector<double, Inputs> ClampInput(
const Eigen::Vector<double, Inputs>& u) const {
return m_clampFunc(u);
}
InputVector ClampInput(const InputVector& u) const { return m_clampFunc(u); }
protected:
LinearQuadraticRegulator<States, Inputs>* m_controller;
@@ -283,12 +271,10 @@ class LinearSystemLoop {
/**
* Clamping function.
*/
std::function<Eigen::Vector<double, Inputs>(
const Eigen::Vector<double, Inputs>&)>
m_clampFunc;
std::function<InputVector(const InputVector&)> m_clampFunc;
// Reference to go to in the next cycle (used by feedforward controller).
Eigen::Vector<double, States> m_nextR;
StateVector m_nextR;
// These are accessible from non-templated subclasses.
static constexpr int kStates = States;

26
wpimath/src/main/native/include/frc/system/NumericalJacobian.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include "Eigen/Core"
#include "frc/EigenCore.h"
namespace frc {
@@ -17,16 +17,16 @@ namespace frc {
* @param x Vector argument.
*/
template <int Rows, int Cols, typename F>
auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
auto NumericalJacobian(F&& f, const Vectord<Cols>& x) {
constexpr double kEpsilon = 1e-5;
Eigen::Matrix<double, Rows, Cols> result;
Matrixd<Rows, Cols> result;
result.setZero();
// It's more expensive, but +- epsilon will be more accurate
for (int i = 0; i < Cols; ++i) {
Eigen::Vector<double, Cols> dX_plus = x;
Vectord<Cols> dX_plus = x;
dX_plus(i) += kEpsilon;
Eigen::Vector<double, Cols> dX_minus = x;
Vectord<Cols> dX_minus = x;
dX_minus(i) -= kEpsilon;
result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
}
@@ -48,12 +48,10 @@ auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianX(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, States>(
[&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
x);
[&](const Vectord<States>& x) { return f(x, u, args...); }, x);
}
/**
@@ -70,12 +68,10 @@ auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
* @param args Remaining arguments to f(x, u, ...).
*/
template <int Rows, int States, int Inputs, typename F, typename... Args>
auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
const Eigen::Vector<double, Inputs>& u,
Args&&... args) {
auto NumericalJacobianU(F&& f, const Vectord<States>& x,
const Vectord<Inputs>& u, Args&&... args) {
return NumericalJacobian<Rows, Inputs>(
[&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
u);
[&](const Vectord<Inputs>& u) { return f(x, u, args...); }, u);
}
} // namespace frc

88
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -56,15 +56,13 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Eigen::Matrix<double, 2, 1> B{0.0,
(G * motor.Kt / (motor.R * r * m)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -90,12 +88,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -134,10 +132,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<1, 1> A{-kV.value() / kA.value()};
Matrixd<1, 1> B{1.0 / kA.value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
@@ -176,10 +174,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("Ka must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
@@ -224,12 +222,10 @@ class WPILIB_DLLEXPORT LinearSystemId {
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Eigen::Matrix<double, 2, 2> A =
0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
Eigen::Matrix<double, 2, 2> B =
0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
@@ -311,11 +307,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 1, 1> A{
Matrixd<1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 1, 1> C{1.0};
Eigen::Matrix<double, 1, 1> D{0.0};
Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
@@ -342,12 +338,12 @@ class WPILIB_DLLEXPORT LinearSystemId {
throw std::domain_error("G must be greater than zero.");
}
Eigen::Matrix<double, 2, 2> A{
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 1> D{0.0, 0.0};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
@@ -391,18 +387,16 @@ class WPILIB_DLLEXPORT LinearSystemId {
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
Eigen::Matrix<double, 2, 2> A{
{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Eigen::Matrix<double, 2, 2> B{
{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}

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

@@ -2,34 +2,34 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Eigen/Core"
#include "Eigen/LU"
#include "frc/EigenCore.h"
#include "gtest/gtest.h"
TEST(EigenTest, Multiplication) {
Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
frc::Matrixd<2, 2> m1{{2, 1}, {0, 1}};
frc::Matrixd<2, 2> m2{{3, 0}, {0, 2.5}};
const auto result = m1 * m2;
Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
frc::Matrixd<2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
EXPECT_TRUE(expectedResult.isApprox(result));
Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
Eigen::Matrix<double, 3, 4> m4{
frc::Matrixd<2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
frc::Matrixd<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{{12.5, 5.55, 7.8, 14.3},
{22.13, 9.82, 13.28, 23.53}};
frc::Matrixd<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, Transpose) {
Eigen::Vector<double, 3> vec{1, 2, 3};
frc::Vectord<3> vec{1, 2, 3};
const auto transpose = vec.transpose();
@@ -39,8 +39,7 @@ TEST(EigenTest, Transpose) {
}
TEST(EigenTest, Inverse) {
Eigen::Matrix<double, 3, 3> mat{
{1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
frc::Matrixd<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);

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

@@ -7,7 +7,7 @@
#include <cmath>
#include <random>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/estimator/KalmanFilter.h"
@@ -45,8 +45,7 @@ class StateSpaceTest : public testing::Test {
void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
double noise) {
Eigen::Vector<double, 1> y =
plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
Vectord<1> y = plant.CalculateY(loop.Xhat(), loop.U()) + Vectord<1>{noise};
loop.Correct(y);
loop.Predict(kDt);
}
@@ -55,7 +54,7 @@ TEST_F(StateSpaceTest, CorrectPredictLoop) {
std::default_random_engine generator;
std::normal_distribution<double> dist{0.0, kPositionStddev};
Eigen::Vector<double, 2> references{2.0, 0.0};
Vectord<2> references{2.0, 0.0};
loop.SetNextR(references);
for (int i = 0; i < 1000; i++) {

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

@@ -6,13 +6,13 @@
#include <array>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/NumericalIntegration.h"
TEST(StateSpaceUtilTest, MakeMatrix) {
// Column vector
Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
frc::Vectord<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);
@@ -22,15 +22,14 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
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);
frc::Matrixd<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);
frc::Matrixd<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);
@@ -39,8 +38,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
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);
frc::Matrixd<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);
@@ -50,7 +48,7 @@ TEST(StateSpaceUtilTest, MakeMatrix) {
}
TEST(StateSpaceUtilTest, CostParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix(1.0, 2.0, 3.0);
frc::Matrixd<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);
@@ -63,7 +61,7 @@ TEST(StateSpaceUtilTest, CostParameterPack) {
}
TEST(StateSpaceUtilTest, CostArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0});
frc::Matrixd<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);
@@ -76,7 +74,7 @@ TEST(StateSpaceUtilTest, CostArray) {
}
TEST(StateSpaceUtilTest, CovParameterPack) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
frc::Matrixd<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);
@@ -89,7 +87,7 @@ TEST(StateSpaceUtilTest, CovParameterPack) {
}
TEST(StateSpaceUtilTest, CovArray) {
Eigen::Matrix<double, 3, 3> mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0});
frc::Matrixd<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);
@@ -102,59 +100,59 @@ TEST(StateSpaceUtilTest, CovArray) {
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
static_cast<void>(vec);
}
TEST(StateSpaceUtilTest, IsStabilizable) {
Eigen::Matrix<double, 2, 1> B{0, 1};
frc::Matrixd<2, 1> B{0, 1};
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
EXPECT_FALSE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
EXPECT_FALSE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
EXPECT_TRUE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
EXPECT_TRUE((frc::IsStabilizable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
EXPECT_TRUE(
(frc::IsStabilizable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
}
TEST(StateSpaceUtilTest, IsDetectable) {
Eigen::Matrix<double, 1, 2> C{0, 1};
frc::Matrixd<1, 2> C{0, 1};
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
EXPECT_FALSE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
EXPECT_FALSE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
EXPECT_TRUE((frc::IsDetectable<2, 1>(
Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
EXPECT_TRUE(
(frc::IsDetectable<2, 1>(frc::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
}

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

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

69
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

@@ -24,40 +24,37 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
maxAlpha};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Ensure voltage exceeds acceleration before limiting
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{12.0, 12.0};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{12.0, 12.0};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
EXPECT_GT(units::math::abs(a), maxA);
}
{
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{-12.0, 12.0};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{-12.0, 12.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
EXPECT_GT(units::math::abs(alpha), maxAlpha);
}
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -66,19 +63,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Backward
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -87,19 +83,18 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
}
// Rotate CCW
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
Eigen::Vector<double, 2> accels =
plant.A() * xAccelLimiter +
plant.B() * Eigen::Vector<double, 2>{left, right};
Vectord<2> accels =
plant.A() * xAccelLimiter + plant.B() * Vectord<2>{left, right};
units::meters_per_second_squared_t a{(accels(0) + accels(1)) / 2.0};
units::radians_per_second_squared_t alpha{(accels(1) - accels(0)) /
trackwidth.value()};
@@ -123,19 +118,19 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
DifferentialDriveAccelerationLimiter accelLimiter{
plant, trackwidth, 1e3_mps_sq, 1e3_rad_per_s_sq};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xAccelLimiter{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xAccelLimiter{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -144,15 +139,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Backward
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));
@@ -161,15 +156,15 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
// Rotate CCW
x.setZero();
xAccelLimiter.setZero();
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
auto [left, right] =
accelLimiter.Calculate(units::meters_per_second_t{xAccelLimiter(0)},
units::meters_per_second_t{xAccelLimiter(1)},
units::volt_t{u(0)}, units::volt_t{u(1)});
xAccelLimiter = plant.CalculateX(xAccelLimiter,
Eigen::Vector<double, 2>{left, right}, dt);
xAccelLimiter =
plant.CalculateX(xAccelLimiter, Vectord<2>{left, right}, dt);
EXPECT_DOUBLE_EQ(x(0), xAccelLimiter(0));
EXPECT_DOUBLE_EQ(x(1), xAccelLimiter(1));

20
wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
View File

@@ -19,11 +19,11 @@ TEST(ImplicitModelFollowerTest, SameModel) {
ImplicitModelFollower<2, 2> imf{plant, plant};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xImf{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xImf{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -33,7 +33,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
// Backward
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -43,7 +43,7 @@ TEST(ImplicitModelFollowerTest, SameModel) {
}
// Rotate CCW
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -68,11 +68,11 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
ImplicitModelFollower<2, 2> imf{plant, plantRef};
Eigen::Vector<double, 2> x{0.0, 0.0};
Eigen::Vector<double, 2> xImf{0.0, 0.0};
Vectord<2> x{0.0, 0.0};
Vectord<2> xImf{0.0, 0.0};
// Forward
Eigen::Vector<double, 2> u{12.0, 12.0};
Vectord<2> u{12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -84,7 +84,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
// Backward
x.setZero();
xImf.setZero();
u = Eigen::Vector<double, 2>{-12.0, -12.0};
u = Vectord<2>{-12.0, -12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);
@@ -96,7 +96,7 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
// Rotate CCW
x.setZero();
xImf.setZero();
u = Eigen::Vector<double, 2>{-12.0, 12.0};
u = Vectord<2>{-12.0, 12.0};
for (auto t = 0_s; t < 3_s; t += dt) {
x = plant.CalculateX(x, u, dt);
xImf = plant.CalculateX(xImf, imf.Calculate(xImf, u), dt);

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

@@ -6,21 +6,21 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "units/time.h"
namespace frc {
TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
Eigen::Matrix<double, 2, 1> B{0, 1};
Matrixd<2, 2> A{{1, 0}, {0, 1}};
Matrixd<2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
units::second_t(0.02)};
Eigen::Vector<double, 2> r{2, 2};
Eigen::Vector<double, 2> nextR{3, 3};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};
EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
}

64
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -6,7 +6,7 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
@@ -30,7 +30,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
EXPECT_NEAR(522.15314269, K(0, 0), 1e-6);
@@ -54,7 +54,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
motors, 1.0 / 3.0 * m * r * r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms}
.K();
@@ -77,7 +77,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
}();
Eigen::Matrix<double, 1, 2> K =
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
EXPECT_NEAR(10.38, K(0, 0), 1e-1);
@@ -99,50 +99,44 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
* @param dt Discretization timestep.
*/
template <int States, int Inputs>
Eigen::Matrix<double, Inputs, States> GetImplicitModelFollowingK(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, States>& Aref, units::second_t dt) {
Matrixd<Inputs, States> GetImplicitModelFollowingK(
const Matrixd<States, States>& A, const Matrixd<States, Inputs>& B,
const Matrixd<States, States>& Q, const Matrixd<Inputs, Inputs>& R,
const Matrixd<States, States>& Aref, units::second_t dt) {
// Discretize real dynamics
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
// Discretize desired dynamics
Eigen::Matrix<double, States, States> discAref;
Matrixd<States, States> discAref;
DiscretizeA<States>(Aref, dt, &discAref);
Eigen::Matrix<double, States, States> Qimf =
Matrixd<States, States> Qimf =
(discA - discAref).transpose() * Q * (discA - discAref);
Eigen::Matrix<double, Inputs, Inputs> Rimf =
discB.transpose() * Q * discB + R;
Eigen::Matrix<double, States, Inputs> Nimf =
(discA - discAref).transpose() * Q * discB;
Matrixd<Inputs, Inputs> Rimf = discB.transpose() * Q * discB + R;
Matrixd<States, Inputs> Nimf = (discA - discAref).transpose() * Q * discB;
return LinearQuadraticRegulator<States, Inputs>{A, B, Qimf, Rimf, Nimf, dt}
.K();
}
TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) {
Eigen::Matrix<double, 2, 2> A{Eigen::Matrix<double, 2, 2>::Zero()};
Eigen::Matrix<double, 2, 2> B{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> Q{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> R{Eigen::Matrix<double, 2, 2>::Identity()};
Matrixd<2, 2> A{Matrixd<2, 2>::Zero()};
Matrixd<2, 2> B{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> Q{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> R{Matrixd<2, 2>::Identity()};
// QR overload
Eigen::Matrix<double, 2, 2> K =
LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
Matrixd<2, 2> K = LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10);
EXPECT_NEAR(0.0, K(0, 1), 1e-10);
EXPECT_NEAR(0.0, K(1, 0), 1e-10);
EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10);
// QRN overload
Eigen::Matrix<double, 2, 2> N{Eigen::Matrix<double, 2, 2>::Identity()};
Eigen::Matrix<double, 2, 2> Kimf =
LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
Matrixd<2, 2> N{Matrixd<2, 2>::Identity()};
Matrixd<2, 2> Kimf = LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K();
EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10);
EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10);
@@ -153,21 +147,19 @@ TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) {
double Kv = 3.02;
double Ka = 0.642;
Eigen::Matrix<double, 2, 2> A{{0, 1}, {0, -Kv / Ka}};
Eigen::Matrix<double, 2, 1> B{{0}, {1.0 / Ka}};
Eigen::Matrix<double, 2, 2> Q{{1, 0}, {0, 0.2}};
Eigen::Matrix<double, 1, 1> R{0.25};
Matrixd<2, 2> A{{0, 1}, {0, -Kv / Ka}};
Matrixd<2, 1> B{{0}, {1.0 / Ka}};
Matrixd<2, 2> Q{{1, 0}, {0, 0.2}};
Matrixd<1, 1> R{0.25};
// QR overload
Eigen::Matrix<double, 1, 2> K =
LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K();
EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10);
EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
// QRN overload
Eigen::Matrix<double, 2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
Eigen::Matrix<double, 1, 2> Kimf =
GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
}

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

@@ -6,7 +6,7 @@
#include <cmath>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/acceleration.h"
@@ -21,16 +21,16 @@ TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ka = 0.6;
auto dt = 0.02_s;
Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
Matrixd<1, 1> A{-Kv / Ka};
Matrixd<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::Vector<double, 1> r{2.0};
Eigen::Vector<double, 1> nextR{3.0};
Vectord<1> r{2.0};
Vectord<1> nextR{3.0};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);

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

@@ -6,11 +6,11 @@
#include <wpi/numbers>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "frc/estimator/AngleStatistics.h"
TEST(AngleStatisticsTest, Mean) {
Eigen::Matrix<double, 3, 3> sigmas{
frc::Matrixd<3, 3> sigmas{
{1, 1.2, 0},
{359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
{1, 2, 0}};

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

@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/NumericalJacobian.h"
@@ -18,8 +18,7 @@
namespace {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -41,7 +40,7 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -50,16 +49,16 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
return frc::Vectord<3>{x(2), x(3), x(4)};
}
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -71,7 +70,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Vector<double, 2> u{12.0, 12.0};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -98,14 +97,13 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::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, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -118,17 +116,15 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
auto localY = LocalMeasurementModel(nextR, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);

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

@@ -11,12 +11,10 @@ namespace {
TEST(MerweScaledSigmaPointsTest, ZeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
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}});
frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
EXPECT_TRUE(
(points -
Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
(points - frc::Matrixd<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);
}
@@ -24,12 +22,10 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) {
TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
frc::MerweScaledSigmaPoints<2> sigmaPoints;
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}});
frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}});
EXPECT_TRUE(
(points -
Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
(points - frc::Matrixd<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);
}

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

@@ -7,8 +7,8 @@
#include <array>
#include <cmath>
#include "Eigen/Core"
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
@@ -20,8 +20,7 @@
namespace {
Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -43,7 +42,7 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
units::volt_t Vr{u(1)};
auto v = 0.5 * (vl + vr);
return Eigen::Vector<double, 5>{
return frc::Vectord<5>{
v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
((vr - vl) / (2.0 * rb)).value(),
k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
@@ -52,16 +51,16 @@ Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
Eigen::Vector<double, 3> LocalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> LocalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
return frc::Vectord<3>{x(2), x(3), x(4)};
}
Eigen::Vector<double, 5> GlobalMeasurementModel(
const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
frc::Vectord<5> GlobalMeasurementModel(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
static_cast<void>(u);
return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
@@ -73,7 +72,7 @@ TEST(UnscentedKalmanFilterTest, Init) {
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
dt};
Eigen::Vector<double, 2> u{12.0, 12.0};
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -102,14 +101,13 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.8_mps, 0.1_mps_sq});
Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::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, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
observer.SetXhat(Eigen::Vector<double, 5>{
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
trajectory.InitialPose().Translation().Y().value(),
trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
@@ -124,17 +122,15 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
units::meters_per_second_t vr =
ref.velocity * (1 + (ref.curvature * rb).value());
Eigen::Vector<double, 5> nextR{
frc::Vectord<5> nextR{
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY =
LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot -
Dynamics(r, Eigen::Vector<double, 2>::Zero()));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);

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

@@ -6,8 +6,8 @@
#include <functional>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
@@ -15,17 +15,16 @@
// Check that for a simple second-order system that we can easily analyze
// analytically,
TEST(DiscretizationTest, DiscretizeA) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
Eigen::Vector<double, 2> x0{1, 1};
Eigen::Matrix<double, 2, 2> discA;
frc::Vectord<2> x0{1, 1};
frc::Matrixd<2, 2> discA;
frc::DiscretizeA<2>(contA, 1_s, &discA);
Eigen::Vector<double, 2> x1Discrete = discA * x0;
frc::Vectord<2> x1Discrete = discA * x0;
// We now have pos = vel = 1 and accel = 0, which should give us:
Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
0.0 * x0(0) + 1.0 * x0(1)};
frc::Vectord<2> x1Truth{1.0 * x0(0) + 1.0 * x0(1), 0.0 * x0(0) + 1.0 * x0(1)};
EXPECT_EQ(x1Truth, x1Discrete);
}
@@ -33,20 +32,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{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 1> contB{0, 1};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 1> contB{0, 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::Vectord<2> x0{1, 1};
frc::Vectord<1> u{1};
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 1> discB;
frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
frc::Vectord<2> x1Discrete = discA * x0 + discB * u;
// We now have pos = vel = accel = 1, which should give us:
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)};
frc::Vectord<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);
}
@@ -55,24 +54,23 @@ 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{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> 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.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10)
@@ -85,24 +83,23 @@ 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{{0, 1}, {0, -1406.29}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_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.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discQ;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ);
EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3)
@@ -113,14 +110,14 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}};
frc::Matrixd<2, 2> 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;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
@@ -128,16 +125,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
EXPECT_GE(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.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -157,14 +153,14 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
// Test that the Taylor series discretization produces nearly identical results.
TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}};
frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}};
constexpr auto dt = 5_ms;
Eigen::Matrix<double, 2, 2> discQTaylor;
Eigen::Matrix<double, 2, 2> discA;
Eigen::Matrix<double, 2, 2> discATaylor;
frc::Matrixd<2, 2> discQTaylor;
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discATaylor;
// Continuous Q should be positive semidefinite
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
@@ -172,16 +168,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
EXPECT_GE(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.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::DiscretizeA<2>(contA, dt, &discA);
frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
@@ -201,10 +196,10 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
// Test that DiscretizeR() works
TEST(DiscretizationTest, DiscretizeR) {
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}};
frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
frc::Matrixd<2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
frc::Matrixd<2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
EXPECT_LT((discRTruth - discR).norm(), 1e-10)
<< "Expected these to be nearly equal:\ndiscR:\n"

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

@@ -16,49 +16,43 @@ 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(
Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
0.001));
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
ASSERT_TRUE(model.B().isApprox(
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));
frc::Matrixd<2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}}, 0.001));
ASSERT_TRUE(
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(
model.D().isApprox(frc::Matrixd<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(
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));
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 20.8}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 2>{1.0, 0.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<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(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));
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, DCMotorSystem) {
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
ASSERT_TRUE(model.A().isApprox(
Eigen::Matrix<double, 2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(Eigen::Matrix<double, 2, 1>{0, 1354.166667}, 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, 1>{0.0, 0.0}, 0.001));
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
ASSERT_TRUE(
model.C().isApprox(frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(frc::Matrixd<2, 1>{0.0, 0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -70,9 +64,8 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
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));
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -84,6 +77,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(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));
}

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

@@ -10,48 +10,46 @@
// Tests that integrating dx/dt = e^x works.
TEST(NumericalIntegrationTest, Exponential) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
Eigen::Vector<double, 1> y1 = frc::RK4(
[](const Eigen::Vector<double, 1>& x) {
return Eigen::Vector<double, 1>{std::exp(x(0))};
},
frc::Vectord<1> y1 = frc::RK4(
[](const frc::Vectord<1>& x) { return frc::Vectord<1>{std::exp(x(0))}; },
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(NumericalIntegrationTest, ExponentialWithU) {
Eigen::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
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))};
frc::Vectord<1> y1 = frc::RK4(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(u(0) * x(0))};
},
y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
y0, frc::Vectord<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::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
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))};
frc::Vectord<1> y1 = frc::RKF45(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(x(0))};
},
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
y0, frc::Vectord<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::Vector<double, 1> y0{0.0};
frc::Vectord<1> y0{0.0};
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))};
frc::Vectord<1> y1 = frc::RKDP(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>{std::exp(x(0))};
},
y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
y0, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}

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

@@ -6,53 +6,46 @@
#include "frc/system/NumericalJacobian.h"
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}};
frc::Matrixd<4, 4> A{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
frc::Matrixd<4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
// Function from which to recover A and B
Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<4> AxBuFn(const frc::Vectord<4>& x, const frc::Vectord<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::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<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::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
AxBuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
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}};
frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
// Function from which to recover C and D
Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
const Eigen::Vector<double, 2>& u) {
frc::Vectord<3> CxDuFn(const frc::Vectord<4>& x, const frc::Vectord<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::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<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::Vector<double, 4>::Zero(),
Eigen::Vector<double, 2>::Zero());
frc::Matrixd<3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}

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

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

2
wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
View File

@@ -6,7 +6,7 @@
#include <array>
#include "Eigen/Core"
#include "frc/EigenCore.h"
#include "units/time.h"
namespace frc {