Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,7 +41,7 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.
return m_gearbox.Current(GetAngularVelocity() * m_gearing,
return m_gearbox.Current(units::radians_per_second_t{m_x(1)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

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

@@ -12,11 +12,11 @@
using namespace frc;
using namespace frc::sim;
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 2>& plant,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_minHeight(minHeight),
@@ -30,7 +30,7 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
gearbox, minHeight, maxHeight, simulateGravity,
@@ -44,7 +44,7 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox,
minHeight, maxHeight, simulateGravity, startingHeight,
measurementStdDevs) {}

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

@@ -36,7 +36,7 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.
return m_gearbox.Current(GetAngularVelocity() * m_gearing,
return m_gearbox.Current(units::radians_per_second_t{m_x(0)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

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

@@ -16,12 +16,12 @@ using namespace frc;
using namespace frc::sim;
SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
const LinearSystem<2, 1, 2>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim<2, 1, 2>(system, measurementStdDevs),
m_armLen(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
@@ -36,7 +36,7 @@ SingleJointedArmSim::SingleJointedArmSim(
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,

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

@@ -17,7 +17,7 @@ namespace frc::sim {
/**
* Represents a simulated elevator mechanism.
*/
class ElevatorSim : public LinearSystemSim<2, 1, 1> {
class ElevatorSim : public LinearSystemSim<2, 1, 2> {
public:
template <typename Distance>
using Velocity_t = units::unit_t<
@@ -42,10 +42,10 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
@@ -67,7 +67,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
units::kilogram_t carriageMass, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
@@ -90,7 +90,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
using LinearSystemSim::SetState;
/**

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

@@ -18,7 +18,7 @@ namespace frc::sim {
/**
* Represents a simulated arm mechanism.
*/
class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
public:
/**
* Creates a simulated arm mechanism.
@@ -36,12 +36,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviations of the measurements.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
SingleJointedArmSim(const LinearSystem<2, 1, 2>& system,
const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
/**
* Creates a simulated arm mechanism.
*
@@ -62,7 +63,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
using LinearSystemSim::SetState;

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

@@ -74,8 +74,10 @@ TEST(ElevatorSimTest, Stability) {
sim.Update(20_ms);
}
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
frc::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100);
frc::LinearSystem<2, 1, 1> system =
frc::LinearSystemId::ElevatorSystem(frc::DCMotor::Vex775Pro(4), 4_kg,
0.5_in, 100)
.Slice(0);
EXPECT_NEAR_UNITS(
units::meter_t{system.CalculateX(frc::Vectord<2>{0.0, 0.0},
frc::Vectord<1>{12.0}, 20_ms * 50)(0)},

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

@@ -47,7 +47,8 @@ class Robot : public frc::TimedRobot {
// Outputs (what we can measure): [position], in radians.
frc::LinearSystem<2, 1, 1> m_armPlant =
frc::LinearSystemId::SingleJointedArmSystem(frc::DCMotor::NEO(2), kArmMOI,
kArmGearing);
kArmGearing)
.Slice(0);
// The observer fuses our encoder data and voltage inputs to reject noise.
frc::KalmanFilter<2, 1, 1> m_observer{

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

@@ -45,7 +45,8 @@ class Robot : public frc::TimedRobot {
// Outputs (what we can measure): [position], in meters.
frc::LinearSystem<2, 1, 1> m_elevatorPlant =
frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), kCarriageMass,
kDrumRadius, kGearRatio);
kDrumRadius, kGearRatio)
.Slice(0);
// The observer fuses our encoder data and voltage inputs to reject noise.
frc::KalmanFilter<2, 1, 1> m_observer{

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -141,7 +141,7 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the output
return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated elevator mechanism. */
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
// Gearbox for the elevator.
private final DCMotor m_gearbox;
@@ -43,13 +43,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
*/
@SuppressWarnings("this-escape")
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_minHeight = minHeightMeters;
@@ -72,7 +72,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @param simulateGravity Whether gravity should be simulated or not.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
@@ -138,7 +138,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
@@ -171,7 +171,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
@@ -281,7 +281,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
* @return The velocity of the elevator.
*/
public double getVelocityMetersPerSecond() {
return m_x.get(1, 0);
return getOutput(1);
}
/**
@@ -297,8 +297,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// v = r w, so w = v/r
double kA = 1 / m_plant.getB().get(1, 0);
double kV = -m_plant.getA().get(1, 1) * kA;
double motorVelocityRadPerSec =
getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt;
double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt;
var appliedVoltage = m_u.get(0, 0);
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
* Math.signum(appliedVoltage);

14
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -15,7 +15,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated single jointed arm mechanism. */
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
// The gearbox for the arm.
private final DCMotor m_gearbox;
@@ -51,7 +51,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
*/
@SuppressWarnings("this-escape")
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
@@ -59,7 +59,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
@@ -86,7 +86,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @param startingAngleRads The initial position of the Arm simulation in radians.
*/
public SingleJointedArmSim(
LinearSystem<N2, N1, N1> plant,
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
@@ -161,7 +161,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N1, N1> measurementStdDevs) {
Matrix<N2, N1> measurementStdDevs) {
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
gearbox,
@@ -230,7 +230,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @return The current arm angle.
*/
public double getAngleRads() {
return m_y.get(0, 0);
return getOutput(0);
}
/**
@@ -239,7 +239,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
* @return The current arm velocity.
*/
public double getVelocityRadPerSec() {
return m_x.get(1, 0);
return getOutput(1);
}
/**

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

@@ -35,7 +35,7 @@ class ElevatorSimTest {
3.0,
true,
0.0,
VecBuilder.fill(0.01));
VecBuilder.fill(0.01, 0.00));
try (var motor = new PWMVictorSPX(0);
var encoder = new Encoder(0, 1)) {
@@ -74,7 +74,7 @@ class ElevatorSimTest {
1.0,
true,
0.0,
VecBuilder.fill(0.01));
VecBuilder.fill(0.01, 0.00));
for (int i = 0; i < 100; i++) {
sim.setInput(VecBuilder.fill(0));

3
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java
View File

@@ -51,7 +51,8 @@ public class Arm implements AutoCloseable {
Constants.kMaxAngleRads,
true,
0,
VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
VecBuilder.fill(
Constants.kArmEncoderDistPerPulse, 0.0) // Add noise with a std-dev of 1 tick
);
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java
View File

@@ -60,7 +60,7 @@ public class Elevator implements AutoCloseable {
Constants.kMaxElevatorHeightMeters,
true,
0,
VecBuilder.fill(0.005));
VecBuilder.fill(0.005, 0.0));
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMSim m_motorSim = new PWMSim(m_motor);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java
View File

@@ -55,7 +55,7 @@ public class Elevator implements AutoCloseable {
Constants.kMaxElevatorHeightMeters,
true,
0,
VecBuilder.fill(0.01));
VecBuilder.fill(0.01, 0.0));
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMSim m_motorSim = new PWMSim(m_motor);

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
View File

@@ -52,15 +52,16 @@ public class Robot extends TimedRobot {
// States: [position, velocity], in radians and radians per second.
// Inputs (what we can "put in"): [voltage], in volts.
// Outputs (what we can measure): [position], in radians.
private final LinearSystem<N2, N1, N1> m_armPlant =
private final LinearSystem<N2, N1, N2> m_armPlant =
LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
@SuppressWarnings("unchecked")
private final KalmanFilter<N2, N1, N1> m_observer =
new KalmanFilter<>(
Nat.N2(),
Nat.N1(),
m_armPlant,
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0),
VecBuilder.fill(0.015, 0.17), // How accurate we
// think our model is, in radians and radians/sec
VecBuilder.fill(0.01), // How accurate we think our encoder position
@@ -68,9 +69,10 @@ public class Robot extends TimedRobot {
0.020);
// A LQR uses feedback to create voltage commands.
@SuppressWarnings("unchecked")
private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
new LinearQuadraticRegulator<>(
m_armPlant,
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0),
VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
// Position and velocity error tolerances, in radians and radians per second. Decrease
// this
@@ -82,11 +84,14 @@ public class Robot extends TimedRobot {
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
// starting point because that is the (approximate) maximum voltage of a battery.
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_armPlant, m_controller, m_observer, 12.0, 0.020);
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_armPlant.slice(0), m_controller, m_observer, 12.0, 0.020);
// An encoder set up to measure arm position in radians.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
View File

@@ -57,16 +57,17 @@ public class Robot extends TimedRobot {
This elevator is driven by two NEO motors.
*/
private final LinearSystem<N2, N1, N1> m_elevatorPlant =
private final LinearSystem<N2, N1, N2> m_elevatorPlant =
LinearSystemId.createElevatorSystem(
DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing);
// The observer fuses our encoder data and voltage inputs to reject noise.
@SuppressWarnings("unchecked")
private final KalmanFilter<N2, N1, N1> m_observer =
new KalmanFilter<>(
Nat.N2(),
Nat.N1(),
m_elevatorPlant,
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
// think our model is, in meters and meters/second.
VecBuilder.fill(0.001), // How accurate we think our encoder position
@@ -74,9 +75,10 @@ public class Robot extends TimedRobot {
0.020);
// A LQR uses feedback to create voltage commands.
@SuppressWarnings("unchecked")
private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
new LinearQuadraticRegulator<>(
m_elevatorPlant,
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
// and velocity error tolerances, in meters and meters per second. Decrease this to more
// heavily penalize state excursion, or make the controller behave more aggressively. In
@@ -86,11 +88,18 @@ public class Robot extends TimedRobot {
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
// starting point because that is the (approximate) maximum voltage of a battery.
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
// lower if using notifiers.
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_elevatorPlant, m_controller, m_observer, 12.0, 0.020);
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_elevatorPlant.slice(0),
m_controller,
m_observer,
12.0,
0.020);
// An encoder set up to measure elevator height in meters.
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);

159
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -7,6 +7,30 @@ package edu.wpi.first.math.system;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N10;
import edu.wpi.first.math.numbers.N11;
import edu.wpi.first.math.numbers.N12;
import edu.wpi.first.math.numbers.N13;
import edu.wpi.first.math.numbers.N14;
import edu.wpi.first.math.numbers.N15;
import edu.wpi.first.math.numbers.N16;
import edu.wpi.first.math.numbers.N17;
import edu.wpi.first.math.numbers.N18;
import edu.wpi.first.math.numbers.N19;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N20;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.math.numbers.N9;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
import org.ejml.simple.SimpleMatrix;
/**
* A plant defined using state-space notation.
@@ -196,6 +220,141 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
return m_C.times(x).plus(m_D.times(clampedU));
}
/**
* Returns the LinearSystem with the outputs listed in outputIndices.
*
* <p>This is used by state observers such as the Kalman Filter.
*
* @param outputIndices the list of output indices to include in the sliced system.
* @return the sliced LinearSystem with outputs set to row vectors of LinearSystem.
* @throws IllegalArgumentException if any outputIndices are outside the range of system outputs.
* @throws IllegalArgumentException if number of outputIndices exceeds the number of system
* outputs.
* @throws IllegalArgumentException if duplication exists in outputIndices.
*/
public LinearSystem<States, Inputs, ? extends Num> slice(int... outputIndices) {
for (int index : outputIndices) {
if (index < 0 || index >= m_C.getNumRows()) {
throw new IllegalArgumentException(
"Output indices out of range. This is usually due to model implementation errors.");
}
}
if (outputIndices.length >= m_C.getNumRows()) {
throw new IllegalArgumentException(
"More outputs requested than available. This is usually due to model implementation "
+ "errors.");
}
List<Integer> outputIndicesList =
Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList());
Collections.sort(outputIndicesList);
if (outputIndices.length != outputIndicesList.size()) {
throw new IllegalArgumentException(
"Duplicate indices exist. This is usually due to model implementation " + "errors.");
}
SimpleMatrix new_C_Storage = new SimpleMatrix(outputIndices.length, m_C.getNumCols());
int row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_C.extractRowVector(index).getData();
new_C_Storage.setRow(row, 0, current_row_data);
row++;
}
SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols());
row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_D.extractRowVector(index).getData();
new_D_Storage.setRow(row, 0, current_row_data);
row++;
}
switch (outputIndices.length) {
case 20:
Matrix<N20, States> new_C20 = new Matrix<>(new_C_Storage);
Matrix<N20, Inputs> new_D20 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C20, new_D20);
case 19:
Matrix<N19, States> new_C19 = new Matrix<>(new_C_Storage);
Matrix<N19, Inputs> new_D19 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C19, new_D19);
case 18:
Matrix<N18, States> new_C18 = new Matrix<>(new_C_Storage);
Matrix<N18, Inputs> new_D18 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C18, new_D18);
case 17:
Matrix<N17, States> new_C17 = new Matrix<>(new_C_Storage);
Matrix<N17, Inputs> new_D17 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C17, new_D17);
case 16:
Matrix<N16, States> new_C16 = new Matrix<>(new_C_Storage);
Matrix<N16, Inputs> new_D16 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C16, new_D16);
case 15:
Matrix<N15, States> new_C15 = new Matrix<>(new_C_Storage);
Matrix<N15, Inputs> new_D15 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C15, new_D15);
case 14:
Matrix<N14, States> new_C14 = new Matrix<>(new_C_Storage);
Matrix<N14, Inputs> new_D14 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C14, new_D14);
case 13:
Matrix<N13, States> new_C13 = new Matrix<>(new_C_Storage);
Matrix<N13, Inputs> new_D13 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C13, new_D13);
case 12:
Matrix<N12, States> new_C12 = new Matrix<>(new_C_Storage);
Matrix<N12, Inputs> new_D12 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C12, new_D12);
case 11:
Matrix<N11, States> new_C11 = new Matrix<>(new_C_Storage);
Matrix<N11, Inputs> new_D11 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C11, new_D11);
case 10:
Matrix<N10, States> new_C10 = new Matrix<>(new_C_Storage);
Matrix<N10, Inputs> new_D10 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C10, new_D10);
case 9:
Matrix<N9, States> new_C9 = new Matrix<>(new_C_Storage);
Matrix<N9, Inputs> new_D9 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C9, new_D9);
case 8:
Matrix<N8, States> new_C8 = new Matrix<>(new_C_Storage);
Matrix<N8, Inputs> new_D8 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C8, new_D8);
case 7:
Matrix<N7, States> new_C7 = new Matrix<>(new_C_Storage);
Matrix<N7, Inputs> new_D7 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C7, new_D7);
case 6:
Matrix<N6, States> new_C6 = new Matrix<>(new_C_Storage);
Matrix<N6, Inputs> new_D6 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C6, new_D6);
case 5:
Matrix<N5, States> new_C5 = new Matrix<>(new_C_Storage);
Matrix<N5, Inputs> new_D5 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C5, new_D5);
case 4:
Matrix<N4, States> new_C4 = new Matrix<>(new_C_Storage);
Matrix<N4, Inputs> new_D4 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C4, new_D4);
case 3:
Matrix<N3, States> new_C3 = new Matrix<>(new_C_Storage);
Matrix<N3, Inputs> new_D3 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C3, new_D3);
case 2:
Matrix<N2, States> new_C2 = new Matrix<>(new_C_Storage);
Matrix<N2, Inputs> new_D2 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C2, new_D2);
default:
Matrix<N1, States> new_C1 = new Matrix<>(new_C_Storage);
Matrix<N1, Inputs> new_D1 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C1, new_D1);
}
}
@Override
public String toString() {
return String.format(

18
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -29,7 +29,7 @@ public final class LinearSystemId {
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N1> createElevatorSystem(
public static LinearSystem<N2, N1, N2> createElevatorSystem(
DCMotor motor, double massKg, double radiusMeters, double gearing) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
@@ -52,8 +52,8 @@ public final class LinearSystemId {
* motor.KtNMPerAmp
/ (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
@@ -219,7 +219,7 @@ public final class LinearSystemId {
* this will be greater than 1.
* @return A LinearSystem representing the given characterized constants.
*/
public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
DCMotor motor, double JKgSquaredMeters, double gearing) {
if (JKgSquaredMeters <= 0.0) {
throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
@@ -239,8 +239,8 @@ public final class LinearSystemId {
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
@@ -294,7 +294,7 @@ public final class LinearSystemId {
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
public static LinearSystem<N2, N1, N2> identifyPositionSystem(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
@@ -305,8 +305,8 @@ public final class LinearSystemId {
return new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
VecBuilder.fill(0.0, 1.0 / kA),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0),
VecBuilder.fill(0.0));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**

16
wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
View File

@@ -6,7 +6,7 @@
using namespace frc;
LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
LinearSystem<2, 1, 2> LinearSystemId::ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing) {
@@ -27,13 +27,13 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
.value()}};
Matrixd<2, 1> B{0.0,
(gearing * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
LinearSystem<2, 1, 2> LinearSystemId::SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
@@ -47,10 +47,10 @@ LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
{0.0,
(-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(

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

@@ -5,9 +5,13 @@
#pragma once
#include <algorithm>
#include <concepts>
#include <functional>
#include <stdexcept>
#include <wpi/Algorithm.h>
#include <wpi/SmallVector.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
@@ -164,6 +168,56 @@ class LinearSystem {
return m_C * x + m_D * clampedU;
}
/**
* Returns the LinearSystem with the outputs listed in outputIndices.
*
* <p>This is used by state observers such as the Kalman Filter.
*
* @param outputIndices the list of output indices to include in the sliced
* system.
* @return the sliced LinearSystem with outputs set to row vectors of
* LinearSystem.
* @throws std::domain_error if any outputIndices are outside the range of
* system outputs.
* @throws std::domain_error if number of outputIndices exceeds the system
* outputs.
* @throws std::domain_error if duplication exists in outputIndices.
*/
template <std::same_as<int>... OutputIndices>
LinearSystem<States, Inputs, sizeof...(OutputIndices)> Slice(
OutputIndices... outputIndices) {
static_assert(sizeof...(OutputIndices) <= Outputs,
"More outputs requested than available. This is usually due "
"to model implementation errors.");
wpi::for_each(
[](size_t i, const auto& elem) {
if (elem < 0 || elem >= Outputs) {
throw std::domain_error(
"Slice indices out of range. This is usually due to model "
"implementation errors.");
}
},
outputIndices...);
// Sort and deduplicate output indices
wpi::SmallVector<int> outputIndicesArray{outputIndices...};
std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
auto last =
std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
outputIndicesArray.erase(last, outputIndicesArray.end());
if (outputIndicesArray.size() != sizeof...(outputIndices)) {
throw std::domain_error(
"Duplicate indices exist. This is usually due to model "
"implementation errors.");
}
return LinearSystem<States, Inputs, sizeof...(OutputIndices)>{
m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all),
m_D(outputIndicesArray, Eigen::placeholders::all)};
}
private:
/**
* Continuous system matrix.

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

@@ -44,7 +44,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to carriage.
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing);
@@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
static LinearSystem<2, 1, 2> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing);
/**

26
wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
View File

@@ -26,19 +26,31 @@ class LinearSystemLoopTest {
private static final double kPositionStddev = 0.0001;
private static final Random random = new Random();
LinearSystem<N2, N1, N1> m_plant =
LinearSystem<N2, N1, N2> m_plant =
LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
@SuppressWarnings("unchecked")
KalmanFilter<N2, N1, N1> m_observer =
new KalmanFilter<>(
Nat.N2(), Nat.N1(), m_plant, VecBuilder.fill(0.05, 1.0), VecBuilder.fill(0.0001), kDt);
Nat.N2(),
Nat.N1(),
(LinearSystem<N2, N1, N1>) m_plant.slice(0),
VecBuilder.fill(0.05, 1.0),
VecBuilder.fill(0.0001),
kDt);
@SuppressWarnings("unchecked")
LinearQuadraticRegulator<N2, N1, N1> m_controller =
new LinearQuadraticRegulator<>(
m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), 0.00505);
(LinearSystem<N2, N1, N1>) m_plant.slice(0),
VecBuilder.fill(0.02, 0.4),
VecBuilder.fill(12.0),
0.00505);
@SuppressWarnings("unchecked")
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
new LinearSystemLoop<>(
(LinearSystem<N2, N1, N1>) m_plant.slice(0), m_controller, m_observer, 12, 0.00505);
private static void updateTwoState(
LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
@@ -49,6 +61,7 @@ class LinearSystemLoopTest {
}
@Test
@SuppressWarnings("unchecked")
void testStateSpaceEnabled() {
m_loop.reset(VecBuilder.fill(0, 0));
Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
@@ -66,7 +79,10 @@ class LinearSystemLoopTest {
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
updateTwoState(
(LinearSystem<N2, N1, N1>) m_plant.slice(0),
m_loop,
(random.nextGaussian()) * kPositionStddev);
var u = m_loop.getU(0);
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);

8
wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
View File

@@ -27,7 +27,7 @@ import java.util.Random;
import org.junit.jupiter.api.Test;
class KalmanFilterTest {
private static LinearSystem<N2, N1, N1> elevatorPlant;
private static LinearSystem<N2, N1, N2> elevatorPlant;
private static final double kDt = 0.00505;
@@ -61,11 +61,15 @@ class KalmanFilterTest {
new Matrix<>(Nat.N3(), Nat.N3())); // D
@Test
@SuppressWarnings("unchecked")
void testElevatorKalmanFilter() {
var Q = VecBuilder.fill(0.05, 1.0);
var R = VecBuilder.fill(0.0001);
assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
assertDoesNotThrow(
() ->
new KalmanFilter<>(
Nat.N2(), Nat.N1(), (LinearSystem<N2, N1, N1>) elevatorPlant.slice(0), Q, R, kDt));
}
@Test

4
wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
View File

@@ -48,9 +48,9 @@ class LinearSystemIDTest {
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001));
assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0, 0), 0.001));
}
@Test

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

@@ -36,7 +36,7 @@ class StateSpaceTest : public testing::Test {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};

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

@@ -28,7 +28,7 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K();
@@ -50,8 +50,9 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return frc::LinearSystemId::SingleJointedArmSystem(
motors, 1.0 / 3.0 * m * r * r, G);
return frc::LinearSystemId::SingleJointedArmSystem(motors,
1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();
Matrixd<1, 2> K =
@@ -75,7 +76,7 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
@@ -177,7 +178,7 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
// Gear ratio
constexpr double G = 14.67;
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
return frc::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};

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

@@ -28,7 +28,8 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
0.05_m, 12);
0.05_m, 12)
.Slice(0);
ASSERT_TRUE(model.A().isApprox(
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));