Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +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

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));