Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

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