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

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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

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

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

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

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

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

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

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

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

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