SCRIPT Move java files

wpimath/src/main/java/org/wpilib/math/system/Discretization.java

@@ -0,0 +1,125 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import org.ejml.simple.SimpleMatrix;
+
+/** Discretization helper functions. */
+public final class Discretization {
+  private Discretization() {
+    // Utility class
+  }
+
+  /**
+   * Discretizes the given continuous A matrix.
+   *
+   * @param <States> Num representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param dt Discretization timestep in seconds.
+   * @return the discrete matrix system.
+   */
+  public static <States extends Num> Matrix<States, States> discretizeA(
+      Matrix<States, States> contA, double dt) {
+    // A_d = eᴬᵀ
+    return contA.times(dt).exp();
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * @param <States> Nat representing the states of the system.
+   * @param <Inputs> Nat representing the inputs to the system.
+   * @param contA Continuous system matrix.
+   * @param contB Continuous input matrix.
+   * @param dt Discretization timestep in seconds.
+   * @return a Pair representing discA and diskB.
+   */
+  public static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
+          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dt) {
+    int states = contA.getNumRows();
+    int inputs = contB.getNumCols();
+
+    // M = [A  B]
+    //     [0  0]
+    var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
+    M.assignBlock(0, 0, contA);
+    M.assignBlock(0, contA.getNumCols(), contB);
+
+    //  ϕ = eᴹᵀ = [A_d  B_d]
+    //            [ 0    I ]
+    var phi = M.times(dt).exp();
+
+    var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
+    discA.extractFrom(0, 0, phi);
+
+    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
+    discB.extractFrom(0, contB.getNumRows(), phi);
+
+    return new Pair<>(discA, discB);
+  }
+
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * @param <States> Nat representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param contQ Continuous process noise covariance matrix.
+   * @param dt Discretization timestep in seconds.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  public static <States extends Num>
+      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
+          Matrix<States, States> contA, Matrix<States, States> contQ, double dt) {
+    int states = contA.getNumRows();
+
+    // Make continuous Q symmetric if it isn't already
+    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
+
+    // M = [−A  Q ]
+    //     [ 0  Aᵀ]
+    final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
+    M.assignBlock(0, 0, contA.times(-1.0));
+    M.assignBlock(0, states, Q);
+    M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
+    M.assignBlock(states, states, contA.transpose());
+
+    // ϕ = eᴹᵀ = [−A_d  A_d⁻¹Q_d]
+    //           [ 0      A_dᵀ  ]
+    final var phi = M.times(dt).exp();
+
+    // ϕ₁₂ = A_d⁻¹Q_d
+    final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
+
+    // ϕ₂₂ = A_dᵀ
+    final Matrix<States, States> phi22 = phi.block(states, states, states, states);
+
+    final var discA = phi22.transpose();
+
+    Q = discA.times(phi12);
+
+    // Make discrete Q symmetric if it isn't already
+    final var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
+  }
+
+  /**
+   * Returns a discretized version of the provided continuous measurement noise covariance matrix.
+   * Note that dt=0.0 divides R by zero.
+   *
+   * @param <O> Nat representing the number of outputs.
+   * @param contR Continuous measurement noise covariance matrix.
+   * @param dt Discretization timestep in seconds.
+   * @return Discretized version of the provided continuous measurement noise covariance matrix.
+   */
+  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dt) {
+    // R_d = 1/T R
+    return contR.div(dt);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java

@@ -0,0 +1,406 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+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 edu.wpi.first.math.system.proto.LinearSystemProto;
+import edu.wpi.first.math.system.struct.LinearSystemStruct;
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+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.
+ *
+ * <p>A plant is a mathematical model of a system's dynamics.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements ProtobufSerializable, StructSerializable {
+  /** Continuous system matrix. */
+  private final Matrix<States, States> m_A;
+
+  /** Continuous input matrix. */
+  private final Matrix<States, Inputs> m_B;
+
+  /** Output matrix. */
+  private final Matrix<Outputs, States> m_C;
+
+  /** Feedthrough matrix. */
+  private final Matrix<Outputs, Inputs> m_D;
+
+  /**
+   * Construct a new LinearSystem from the four system matrices.
+   *
+   * @param A The system matrix A.
+   * @param B The input matrix B.
+   * @param C The output matrix C.
+   * @param D The feedthrough matrix D.
+   * @throws IllegalArgumentException if any matrix element isn't finite.
+   */
+  public LinearSystem(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<Outputs, States> C,
+      Matrix<Outputs, Inputs> D) {
+    for (int row = 0; row < A.getNumRows(); ++row) {
+      for (int col = 0; col < A.getNumCols(); ++col) {
+        if (!Double.isFinite(A.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of A aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < B.getNumRows(); ++row) {
+      for (int col = 0; col < B.getNumCols(); ++col) {
+        if (!Double.isFinite(B.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of B aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < C.getNumRows(); ++row) {
+      for (int col = 0; col < C.getNumCols(); ++col) {
+        if (!Double.isFinite(C.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of C aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < D.getNumRows(); ++row) {
+      for (int col = 0; col < D.getNumCols(); ++col) {
+        if (!Double.isFinite(D.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of D aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+
+    this.m_A = A;
+    this.m_B = B;
+    this.m_C = C;
+    this.m_D = D;
+  }
+
+  /**
+   * Returns the system matrix A.
+   *
+   * @return the system matrix A.
+   */
+  public Matrix<States, States> getA() {
+    return m_A;
+  }
+
+  /**
+   * Returns an element of the system matrix A.
+   *
+   * @param row Row of A.
+   * @param col Column of A.
+   * @return the system matrix A at (i, j).
+   */
+  public double getA(int row, int col) {
+    return m_A.get(row, col);
+  }
+
+  /**
+   * Returns the input matrix B.
+   *
+   * @return the input matrix B.
+   */
+  public Matrix<States, Inputs> getB() {
+    return m_B;
+  }
+
+  /**
+   * Returns an element of the input matrix B.
+   *
+   * @param row Row of B.
+   * @param col Column of B.
+   * @return The value of the input matrix B at (i, j).
+   */
+  public double getB(int row, int col) {
+    return m_B.get(row, col);
+  }
+
+  /**
+   * Returns the output matrix C.
+   *
+   * @return Output matrix C.
+   */
+  public Matrix<Outputs, States> getC() {
+    return m_C;
+  }
+
+  /**
+   * Returns an element of the output matrix C.
+   *
+   * @param row Row of C.
+   * @param col Column of C.
+   * @return the double value of C at the given position.
+   */
+  public double getC(int row, int col) {
+    return m_C.get(row, col);
+  }
+
+  /**
+   * Returns the feedthrough matrix D.
+   *
+   * @return the feedthrough matrix D.
+   */
+  public Matrix<Outputs, Inputs> getD() {
+    return m_D;
+  }
+
+  /**
+   * Returns an element of the feedthrough matrix D.
+   *
+   * @param row Row of D.
+   * @param col Column of D.
+   * @return The feedthrough matrix D at (i, j).
+   */
+  public double getD(int row, int col) {
+    return m_D.get(row, col);
+  }
+
+  /**
+   * Computes the new x given the old x and the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @param dt Timestep for model update in seconds.
+   * @return the updated x.
+   */
+  public Matrix<States, N1> calculateX(
+      Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dt) {
+    var discABpair = Discretization.discretizeAB(m_A, m_B, dt);
+
+    return discABpair.getFirst().times(x).plus(discABpair.getSecond().times(clampedU));
+  }
+
+  /**
+   * Computes the new y given the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @return the updated output matrix Y.
+   */
+  public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
+    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(
+        "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
+        m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
+  }
+
+  /**
+   * Creates an implementation of the {@link Protobuf} interface for linear systems.
+   *
+   * @param <States> The number of states of the linear systems this serializer processes.
+   * @param <Inputs> The number of inputs of the linear systems this serializer processes.
+   * @param <Outputs> The number of outputs of the linear systems this serializer processes.
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   * @return The protobuf implementation.
+   */
+  public static <States extends Num, Inputs extends Num, Outputs extends Num>
+      LinearSystemProto<States, Inputs, Outputs> getProto(
+          Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    return new LinearSystemProto<>(states, inputs, outputs);
+  }
+
+  /**
+   * Creates an implementation of the {@link Struct} interface for linear systems.
+   *
+   * @param <States> The number of states of the linear systems this serializer processes.
+   * @param <Inputs> The number of inputs of the linear systems this serializer processes.
+   * @param <Outputs> The number of outputs of the linear systems this serializer processes.
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   * @return The struct implementation.
+   */
+  public static <States extends Num, Inputs extends Num, Outputs extends Num>
+      LinearSystemStruct<States, Inputs, Outputs> getStruct(
+          Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    return new LinearSystemStruct<>(states, inputs, outputs);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/LinearSystemLoop.java

@@ -0,0 +1,346 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.Function;
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Combines a controller, feedforward, and observer for controlling a mechanism with full state
+ * feedback.
+ *
+ * <p>For everything in this file, "inputs" and "outputs" are defined from the perspective of the
+ * plant. This means U is an input and Y is an output (because you give the plant U (powers) and it
+ * gives you back a Y (sensor values)). This is the opposite of what they mean from the perspective
+ * of the controller (U is an output because that's what goes to the motors and Y is an input
+ * because that's what comes back from the sensors).
+ *
+ * <p>For more on the underlying math, read <a
+ * href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">https://file.tavsys.net/control/controls-engineering-in-frc.pdf</a>.
+ *
+ * @param <States> Number of states.
+ * @param <Inputs> Number of inputs.
+ * @param <Outputs> Number of outputs.
+ */
+public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
+  private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
+  private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
+  private final KalmanFilter<States, Inputs, Outputs> m_observer;
+  private Matrix<States, N1> m_nextR;
+  private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop. This constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
+   * @param dt The nominal timestep in seconds.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts,
+      double dt) {
+    this(
+        controller,
+        new LinearPlantInversionFeedforward<>(plant, dt),
+        observer,
+        u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   * @param dt The nominal timestep in seconds.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
+      double dt) {
+    this(controller, new LinearPlantInversionFeedforward<>(plant, dt), observer, clampFunction);
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the inputs are
+   *     voltages.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts) {
+    this(
+        controller,
+        feedforward,
+        observer,
+        u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward, and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_controller = controller;
+    this.m_feedforward = feedforward;
+    this.m_observer = observer;
+    this.m_clampFunction = clampFunction;
+
+    m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
+    reset(m_nextR);
+  }
+
+  /**
+   * Returns the observer's state estimate x-hat.
+   *
+   * @return the observer's state estimate x-hat.
+   */
+  public Matrix<States, N1> getXHat() {
+    return getObserver().getXhat();
+  }
+
+  /**
+   * Returns an element of the observer's state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the i-th element of the observer's state estimate x-hat.
+   */
+  public double getXHat(int row) {
+    return getObserver().getXhat(row);
+  }
+
+  /**
+   * Set the initial state estimate x-hat.
+   *
+   * @param xhat The initial state estimate x-hat.
+   */
+  public void setXHat(Matrix<States, N1> xhat) {
+    getObserver().setXhat(xhat);
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXHat(int row, double value) {
+    getObserver().setXhat(row, value);
+  }
+
+  /**
+   * Returns an element of the controller's next reference r.
+   *
+   * @param row Row of r.
+   * @return the element i of the controller's next reference r.
+   */
+  public double getNextR(int row) {
+    return getNextR().get(row, 0);
+  }
+
+  /**
+   * Returns the controller's next reference r.
+   *
+   * @return the controller's next reference r.
+   */
+  public Matrix<States, N1> getNextR() {
+    return m_nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(Matrix<States, N1> nextR) {
+    m_nextR = nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(double... nextR) {
+    if (nextR.length != m_nextR.getNumRows()) {
+      throw new MatrixDimensionException(
+          String.format(
+              "The next reference does not have the "
+                  + "correct number of entries! Expected %s, but got %s.",
+              m_nextR.getNumRows(), nextR.length));
+    }
+    m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
+  }
+
+  /**
+   * Returns the controller's calculated control input u plus the calculated feedforward u_ff.
+   *
+   * @return the calculated control input u.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
+  }
+
+  /**
+   * Returns an element of the controller's calculated control input u.
+   *
+   * @param row Row of u.
+   * @return the calculated control input u at the row i.
+   */
+  public double getU(int row) {
+    return getU().get(row, 0);
+  }
+
+  /**
+   * Return the controller used internally.
+   *
+   * @return the controller used internally.
+   */
+  public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
+    return m_controller;
+  }
+
+  /**
+   * Return the feedforward used internally.
+   *
+   * @return the feedforward used internally.
+   */
+  public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
+    return m_feedforward;
+  }
+
+  /**
+   * Return the observer used internally.
+   *
+   * @return the observer used internally.
+   */
+  public KalmanFilter<States, Inputs, Outputs> getObserver() {
+    return m_observer;
+  }
+
+  /**
+   * Zeroes reference r and controller output u. The previous reference of the
+   * PlantInversionFeedforward and the initial state estimate of the KalmanFilter are set to the
+   * initial state provided.
+   *
+   * @param initialState The initial state.
+   */
+  public final void reset(Matrix<States, N1> initialState) {
+    m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @return The state error matrix.
+   */
+  public Matrix<States, N1> getError() {
+    return getController().getR().minus(m_observer.getXhat());
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @param index The index of the error matrix to return.
+   * @return The error at that index.
+   */
+  public double getError(int index) {
+    return getController().getR().minus(m_observer.getXhat()).get(index, 0);
+  }
+
+  /**
+   * Get the function used to clamp the input u.
+   *
+   * @return The clamping function.
+   */
+  public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
+    return m_clampFunction;
+  }
+
+  /**
+   * Set the clamping function used to clamp inputs.
+   *
+   * @param clampFunction The clamping function.
+   */
+  public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_clampFunction = clampFunction;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param y Measurement vector.
+   */
+  public void correct(Matrix<Outputs, N1> y) {
+    getObserver().correct(getU(), y);
+  }
+
+  /**
+   * Sets new controller output, projects model forward, and runs observer prediction.
+   *
+   * <p>After calling this, the user should send the elements of u to the actuators.
+   *
+   * @param dt Timestep for model update in seconds.
+   */
+  public void predict(double dt) {
+    var u =
+        clampInput(
+            m_controller
+                .calculate(getObserver().getXhat(), m_nextR)
+                .plus(m_feedforward.calculate(m_nextR)));
+    getObserver().predict(u, dt);
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param unclampedU The input to clamp.
+   * @return The clamped input.
+   */
+  public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
+    return m_clampFunction.apply(unclampedU);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/NumericalIntegration.java

@@ -0,0 +1,402 @@
+// 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.
+
+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 java.util.function.BiFunction;
+import java.util.function.DoubleBinaryOperator;
+import java.util.function.DoubleUnaryOperator;
+import java.util.function.UnaryOperator;
+
+/** Numerical integration utilities. */
+public final class NumericalIntegration {
+  private NumericalIntegration() {
+    // utility Class
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param f The function to integrate. It must take one argument x.
+   * @param x The initial value of x.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  public static double rk4(DoubleUnaryOperator f, double x, double dt) {
+    final var h = dt;
+    final var k1 = f.applyAsDouble(x);
+    final var k2 = f.applyAsDouble(x + h * k1 * 0.5);
+    final var k3 = f.applyAsDouble(x + h * k2 * 0.5);
+    final var k4 = f.applyAsDouble(x + h * k3);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  public static double rk4(DoubleBinaryOperator f, double x, double u, double dt) {
+    final var h = dt;
+
+    final var k1 = f.applyAsDouble(x, u);
+    final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u);
+    final var k3 = f.applyAsDouble(x + h * k2 * 0.5, u);
+    final var k4 = f.applyAsDouble(x + h * k3, u);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dt) {
+    final var h = dt;
+
+    Matrix<States, N1> k1 = f.apply(x, u);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)), u);
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param <States> A Num representing the states of the system.
+   * @param f The function to integrate. It must take one argument x.
+   * @param x The initial value of x.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  public static <States extends Num> Matrix<States, N1> rk4(
+      UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dt) {
+    final var h = dt;
+
+    Matrix<States, N1> k1 = f.apply(x);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)));
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt.
+   *
+   * @param <Rows> Rows in y.
+   * @param <Cols> Columns in y.
+   * @param f The function to integrate. It must take two arguments t and y.
+   * @param t The initial value of t.
+   * @param y The initial value of y.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rk4(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dt) {
+    final var h = dt;
+
+    Matrix<Rows, Cols> k1 = f.apply(t, y);
+    Matrix<Rows, Cols> k2 = f.apply(t + dt * 0.5, y.plus(k1.times(h * 0.5)));
+    Matrix<Rows, Cols> k3 = f.apply(t + dt * 0.5, y.plus(k2.times(h * 0.5)));
+    Matrix<Rows, Cols> k4 = f.apply(t + dt, y.plus(k3.times(h)));
+
+    return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max
+   * error is 1e-6.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dt The time over which to integrate in seconds.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dt) {
+    return rkdp(f, x, u, dt, 1e-6);
+  }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dt The time over which to integrate in seconds.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dt,
+      double maxError) {
+    // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
+    // Butcher tableau the following arrays came from.
+
+    // final double[6][6]
+    final double[][] A = {
+      {1.0 / 5.0},
+      {3.0 / 40.0, 9.0 / 40.0},
+      {44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
+      {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0},
+      {9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0},
+      {35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}
+    };
+
+    // final double[7]
+    final double[] b1 = {
+      35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0
+    };
+
+    // final double[7]
+    final double[] b2 = {
+      5179.0 / 57600.0,
+      0.0,
+      7571.0 / 16695.0,
+      393.0 / 640.0,
+      -92097.0 / 339200.0,
+      187.0 / 2100.0,
+      1.0 / 40.0
+    };
+
+    Matrix<States, N1> newX;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dt;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dt) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dt - dtElapsed);
+
+        var k1 = f.apply(x, u);
+        var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
+        var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
+        var k4 =
+            f.apply(
+                x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
+                u);
+        var k5 =
+            f.apply(
+                x.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)),
+                u);
+        var k6 =
+            f.apply(
+                x.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)),
+                u);
+
+        // Since the final row of A and the array b1 have the same coefficients
+        // and k7 has no effect on newX, we can reuse the calculation.
+        newX =
+            x.plus(
+                k1.times(A[5][0])
+                    .plus(k2.times(A[5][1]))
+                    .plus(k3.times(A[5][2]))
+                    .plus(k4.times(A[5][3]))
+                    .plus(k5.times(A[5][4]))
+                    .plus(k6.times(A[5][5]))
+                    .times(h));
+        var k7 = f.apply(newX, u);
+
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .plus(k7.times(b1[6] - b2[6]))
+                    .times(h))
+                .normF();
+
+        if (truncationError == 0.0) {
+          h = dt - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      x = newX;
+    }
+
+    return x;
+  }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(t, y) for dt.
+   *
+   * @param <Rows> Rows in y.
+   * @param <Cols> Columns in y.
+   * @param f The function to integrate. It must take two arguments t and y.
+   * @param t The initial value of t.
+   * @param y The initial value of y.
+   * @param dt The time over which to integrate in seconds.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rkdp(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dt,
+      double maxError) {
+    // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
+    // Butcher tableau the following arrays came from.
+
+    // final double[6][6]
+    final double[][] A = {
+      {1.0 / 5.0},
+      {3.0 / 40.0, 9.0 / 40.0},
+      {44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
+      {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0},
+      {9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0},
+      {35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}
+    };
+
+    // final double[7]
+    final double[] b1 = {
+      35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0
+    };
+
+    // final double[7]
+    final double[] b2 = {
+      5179.0 / 57600.0,
+      0.0,
+      7571.0 / 16695.0,
+      393.0 / 640.0,
+      -92097.0 / 339200.0,
+      187.0 / 2100.0,
+      1.0 / 40.0
+    };
+
+    // final double[6]
+    final double[] c = {1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
+
+    Matrix<Rows, Cols> newY;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dt;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dt) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dt - dtElapsed);
+
+        var k1 = f.apply(t, y);
+        var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h)));
+        var k3 = f.apply(t + h * c[1], y.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)));
+        var k4 =
+            f.apply(
+                t + h * c[2],
+                y.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)));
+        var k5 =
+            f.apply(
+                t + h * c[3],
+                y.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)));
+        var k6 =
+            f.apply(
+                t + h * c[4],
+                y.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)));
+
+        // Since the final row of A and the array b1 have the same coefficients
+        // and k7 has no effect on newY, we can reuse the calculation.
+        newY =
+            y.plus(
+                k1.times(A[5][0])
+                    .plus(k2.times(A[5][1]))
+                    .plus(k3.times(A[5][2]))
+                    .plus(k4.times(A[5][3]))
+                    .plus(k5.times(A[5][4]))
+                    .plus(k6.times(A[5][5]))
+                    .times(h));
+        var k7 = f.apply(t + h * c[5], newY);
+
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .plus(k7.times(b1[6] - b2[6]))
+                    .times(h))
+                .normF();
+
+        if (truncationError == 0.0) {
+          h = dt - dtElapsed;
+        } else {
+          h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+        }
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      y = newY;
+    }
+
+    return y;
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/NumericalJacobian.java

@@ -0,0 +1,101 @@
+// 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.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+/** Numerical Jacobian utilities. */
+public final class NumericalJacobian {
+  private NumericalJacobian() {
+    // Utility Class.
+  }
+
+  private static final double kEpsilon = 1e-5;
+
+  /**
+   * Computes the numerical Jacobian with respect to x for f(x).
+   *
+   * @param <Rows> Number of rows in the result of f(x).
+   * @param <States> Num representing the number of rows in the output of f.
+   * @param <Cols> Number of columns in the result of f(x).
+   * @param rows Number of rows in the result of f(x).
+   * @param cols Number of columns in the result of f(x).
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @param x Vector argument.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  public static <Rows extends Num, Cols extends Num, States extends Num>
+      Matrix<Rows, Cols> numericalJacobian(
+          Nat<Rows> rows,
+          Nat<Cols> cols,
+          Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
+          Matrix<Cols, N1> x) {
+    var result = new Matrix<>(rows, cols);
+
+    for (int i = 0; i < cols.getNum(); i++) {
+      var dxPlus = x.copy();
+      var dxMinus = x.copy();
+      dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
+      dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
+      var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
+
+      result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns numerical Jacobian with respect to x for f(x, u, ...).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param <States> Number of rows in x.
+   * @param <Inputs> Number of rows in the second input to f.
+   * @param <Outputs> Num representing the rows in the output of f.
+   * @param rows Number of rows in the result of f(x, u).
+   * @param states Number of rows in x.
+   * @param f Vector-valued function from which to compute Jacobian.
+   * @param x State vector.
+   * @param u Input vector.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
+      Matrix<Rows, States> numericalJacobianX(
+          Nat<Rows> rows,
+          Nat<States> states,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
+  }
+
+  /**
+   * Returns the numerical Jacobian with respect to u for f(x, u).
+   *
+   * @param <States> The states of the system.
+   * @param <Inputs> The inputs to the system.
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param inputs Number of rows in u.
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @param x State vector.
+   * @param u Input vector.
+   * @return the numerical Jacobian with respect to u for f(x, u).
+   */
+  public static <Rows extends Num, States extends Num, Inputs extends Num>
+      Matrix<Rows, Inputs> numericalJacobianU(
+          Nat<Rows> rows,
+          Nat<Inputs> inputs,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/plant/DCMotor.java

@@ -0,0 +1,358 @@
+// 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.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.system.plant.proto.DCMotorProto;
+import edu.wpi.first.math.system.plant.struct.DCMotorStruct;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
+/** Holds the constants for a DC motor. */
+public class DCMotor implements ProtobufSerializable, StructSerializable {
+  /** Voltage at which the motor constants were measured. */
+  public final double nominalVoltage;
+
+  /** Torque when stalled in Newton-meters. */
+  public final double stallTorque;
+
+  /** Current draw when stalled in amps. */
+  public final double stallCurrent;
+
+  /** Current draw under no load in amps. */
+  public final double freeCurrent;
+
+  /** Angular velocity under no load in radians per second. */
+  public final double freeSpeed;
+
+  /** Motor internal resistance in Ohms. */
+  public final double R;
+
+  /** Motor velocity constant in (rad/s)/V. */
+  public final double Kv;
+
+  /** Motor torque constant in Newton-meters per amp. */
+  public final double Kt;
+
+  /** DCMotor protobuf for serialization. */
+  public static final DCMotorProto proto = new DCMotorProto();
+
+  /** DCMotor struct for serialization. */
+  public static final DCMotorStruct struct = new DCMotorStruct();
+
+  /**
+   * Constructs a DC motor.
+   *
+   * @param nominalVoltage Voltage at which the motor constants were measured.
+   * @param stallTorque Torque when stalled.
+   * @param stallCurrent Current draw when stalled.
+   * @param freeCurrent Current draw under no load.
+   * @param freeSpeed Angular velocity under no load.
+   * @param numMotors Number of motors in a gearbox.
+   */
+  public DCMotor(
+      double nominalVoltage,
+      double stallTorque,
+      double stallCurrent,
+      double freeCurrent,
+      double freeSpeed,
+      int numMotors) {
+    this.nominalVoltage = nominalVoltage;
+    this.stallTorque = stallTorque * numMotors;
+    this.stallCurrent = stallCurrent * numMotors;
+    this.freeCurrent = freeCurrent * numMotors;
+    this.freeSpeed = freeSpeed;
+
+    this.R = nominalVoltage / this.stallCurrent;
+    this.Kv = freeSpeed / (nominalVoltage - R * this.freeCurrent);
+    this.Kt = this.stallTorque / this.stallCurrent;
+  }
+
+  /**
+   * Calculate current drawn by motor with given speed and input voltage.
+   *
+   * @param speed The current angular velocity of the motor.
+   * @param voltageInput The voltage being applied to the motor.
+   * @return The estimated current.
+   */
+  public double getCurrent(double speed, double voltageInput) {
+    return -1.0 / Kv / R * speed + 1.0 / R * voltageInput;
+  }
+
+  /**
+   * Calculate current drawn by motor for a given torque.
+   *
+   * @param torque The torque produced by the motor in Newton-meters.
+   * @return The current drawn by the motor.
+   */
+  public double getCurrent(double torque) {
+    return torque / Kt;
+  }
+
+  /**
+   * Calculate torque produced by the motor with a given current.
+   *
+   * @param current The current drawn by the motor in amps.
+   * @return The torque output in Newton-meters.
+   */
+  public double getTorque(double current) {
+    return current * Kt;
+  }
+
+  /**
+   * Calculate the voltage provided to the motor for a given torque and angular velocity.
+   *
+   * @param torque The torque produced by the motor in Newton-meters.
+   * @param speed The current angular velocity of the motor in radians per second.
+   * @return The voltage of the motor.
+   */
+  public double getVoltage(double torque, double speed) {
+    return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
+  }
+
+  /**
+   * Calculates the angular speed produced by the motor at a given torque and input voltage.
+   *
+   * @param torque The torque produced by the motor in Newton-meters.
+   * @param voltageInput The voltage applied to the motor.
+   * @return The angular speed of the motor.
+   */
+  public double getSpeed(double torque, double voltageInput) {
+    return voltageInput * Kv - 1.0 / Kt * torque * R * Kv;
+  }
+
+  /**
+   * Returns a copy of this motor with the given gearbox reduction applied.
+   *
+   * @param gearboxReduction The gearbox reduction.
+   * @return A motor with the gearbox reduction applied.
+   */
+  public DCMotor withReduction(double gearboxReduction) {
+    return new DCMotor(
+        nominalVoltage,
+        stallTorque * gearboxReduction,
+        stallCurrent,
+        freeCurrent,
+        freeSpeed / gearboxReduction,
+        1);
+  }
+
+  /**
+   * Return a gearbox of CIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of CIM motors.
+   */
+  public static DCMotor getCIM(int numMotors) {
+    return new DCMotor(
+        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
+  }
+
+  /**
+   * Return a gearbox of 775Pro motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of 775Pro motors.
+   */
+  public static DCMotor getVex775Pro(int numMotors) {
+    return new DCMotor(
+        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of NEO motors.
+   */
+  public static DCMotor getNEO(int numMotors) {
+    return new DCMotor(
+        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
+  }
+
+  /**
+   * Return a gearbox of MiniCIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of MiniCIM motors.
+   */
+  public static DCMotor getMiniCIM(int numMotors) {
+    return new DCMotor(
+        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Bag motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Bag motors.
+   */
+  public static DCMotor getBag(int numMotors) {
+    return new DCMotor(
+        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark RS775-125 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Andymark RS775-125 motors.
+   */
+  public static DCMotor getAndymarkRs775_125(int numMotors) {
+    return new DCMotor(
+        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS775 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Banebots RS775 motors.
+   */
+  public static DCMotor getBanebotsRs775(int numMotors) {
+    return new DCMotor(
+        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark 9015 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Andymark 9015 motors.
+   */
+  public static DCMotor getAndymark9015(int numMotors) {
+    return new DCMotor(
+        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Banebots RS 550 motors.
+   */
+  public static DCMotor getBanebotsRs550(int numMotors) {
+    return new DCMotor(
+        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of NEO 550 motors.
+   */
+  public static DCMotor getNeo550(int numMotors) {
+    return new DCMotor(
+        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Falcon 500 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 motors.
+   */
+  public static DCMotor getFalcon500(int numMotors) {
+    return new DCMotor(
+        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Falcon 500 motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 FOC enabled motors.
+   */
+  public static DCMotor getFalcon500Foc(int numMotors) {
+    // https://store.ctr-electronics.com/falcon-500-powered-by-talon-fx/
+    return new DCMotor(
+        12, 5.84, 304, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6080.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Romi/TI_RSLK MAX motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Romi/TI_RSLK MAX motors.
+   */
+  public static DCMotor getRomiBuiltIn(int numMotors) {
+    // From https://www.pololu.com/product/1520/specs
+    return new DCMotor(
+        4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Kraken X60 motors.
+   */
+  public static DCMotor getKrakenX60(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(6000), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X60 brushless motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Kraken X60 FOC enabled motors.
+   */
+  public static DCMotor getKrakenX60Foc(int numMotors) {
+    // From https://store.ctr-electronics.com/announcing-kraken-x60/
+    return new DCMotor(
+        12, 9.37, 483, 2, Units.rotationsPerMinuteToRadiansPerSecond(5800), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X44 brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Kraken X44 motors.
+   */
+  public static DCMotor getKrakenX44(int numMotors) {
+    // From https://motors.ctr-electronics.com/dyno/dynometer-testing/
+    return new DCMotor(
+        12, 4.11, 279, 2, Units.rotationsPerMinuteToRadiansPerSecond(7758), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Kraken X44 brushless motors with FOC (Field-Oriented Control) enabled.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Kraken X44 FOC enabled motors.
+   */
+  public static DCMotor getKrakenX44Foc(int numMotors) {
+    // From https://motors.ctr-electronics.com/dyno/dynometer-testing/
+    return new DCMotor(
+        12, 5.01, 329, 2, Units.rotationsPerMinuteToRadiansPerSecond(7368), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Minion brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Minion motors.
+   */
+  public static DCMotor getMinion(int numMotors) {
+    // From https://motors.ctr-electronics.com/dyno/dynometer-testing/
+    return new DCMotor(
+        12, 3.17, 211, 2, Units.rotationsPerMinuteToRadiansPerSecond(7704), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Neo Vortex brushless motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return a gearbox of Neo Vortex motors.
+   */
+  public static DCMotor getNeoVortex(int numMotors) {
+    // From https://www.revrobotics.com/next-generation-spark-neo/
+    return new DCMotor(
+        12, 3.60, 211, 3.6, Units.rotationsPerMinuteToRadiansPerSecond(6784), numMotors);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java

@@ -0,0 +1,390 @@
+// 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.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+/** Linear system ID utility functions. */
+public final class LinearSystemId {
+  private LinearSystemId() {
+    // Utility class
+  }
+
+  /**
+   * Create a state-space model of an elevator system. The states of the system are [position,
+   * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
+   *
+   * @param motor The motor (or gearbox) attached to the carriage.
+   * @param mass The mass of the elevator carriage, in kilograms.
+   * @param radius The radius of the elevator's driving drum, in meters.
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0.
+   */
+  public static LinearSystem<N2, N1, N2> createElevatorSystem(
+      DCMotor motor, double mass, double radius, double gearing) {
+    if (mass <= 0.0) {
+      throw new IllegalArgumentException("mass must be greater than zero.");
+    }
+    if (radius <= 0.0) {
+      throw new IllegalArgumentException("radius must be greater than zero.");
+    }
+    if (gearing <= 0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        MatBuilder.fill(
+            Nat.N2(),
+            Nat.N2(),
+            0,
+            1,
+            0,
+            -Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)),
+        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a flywheel system. The states of the system are [angular
+   * velocity], inputs are [voltage], and outputs are [angular velocity].
+   *
+   * @param motor The motor (or gearbox) attached to the flywheel.
+   * @param J The moment of inertia J of the flywheel in kg-m².
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
+   */
+  public static LinearSystem<N1, N1, N1> createFlywheelSystem(
+      DCMotor motor, double J, double gearing) {
+    if (J <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
+        VecBuilder.fill(gearing * motor.Kt / (motor.R * J)),
+        Matrix.eye(Nat.N1()),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a DC motor system. The states of the system are [angular
+   * position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
+   * velocity]ᵀ.
+   *
+   * @param motor The motor (or gearbox) attached to system.
+   * @param J The moment of inertia J of the DC motor in kg-m².
+   * @param gearing The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
+   */
+  public static LinearSystem<N2, N1, N2> createDCMotorSystem(
+      DCMotor motor, double J, double gearing) {
+    if (J <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        MatBuilder.fill(
+            Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
+        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
+   * (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
+   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * <p>The parameters provided by the user are from this feedforward model:
+   *
+   * <p>u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec)
+   * @param kA The acceleration gain, in volts/(unit/sec²)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
+   * @see <a
+   *     href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
+   */
+  public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
+    if (kV < 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
+        VecBuilder.fill(0, 1 / kA),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a differential drive drivetrain. In this model, the states are
+   * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
+   * [left velocity, right velocity]ᵀ.
+   *
+   * @param motor The motor (or gearbox) driving the drivetrain.
+   * @param mass The mass of the robot in kilograms.
+   * @param r The radius of the wheels in meters.
+   * @param rb The radius of the base (half the trackwidth) in meters.
+   * @param J The moment of inertia of the robot in kg-m².
+   * @param gearing The gearing reduction as output over input.
+   * @return A LinearSystem representing a differential drivetrain.
+   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
+   *     &lt;= 0.
+   */
+  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
+      DCMotor motor, double mass, double r, double rb, double J, double gearing) {
+    if (mass <= 0.0) {
+      throw new IllegalArgumentException("mass must be greater than zero.");
+    }
+    if (r <= 0.0) {
+      throw new IllegalArgumentException("r must be greater than zero.");
+    }
+    if (rb <= 0.0) {
+      throw new IllegalArgumentException("rb must be greater than zero.");
+    }
+    if (J <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
+    }
+
+    var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r);
+    var C2 = gearing * motor.Kt / (motor.R * r);
+
+    final double C3 = 1 / mass + rb * rb / J;
+    final double C4 = 1 / mass - rb * rb / J;
+    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
+    var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
+    var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
+    var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
+
+    return new LinearSystem<>(A, B, C, D);
+  }
+
+  /**
+   * Create a state-space model of a single jointed arm system. The states of the system are [angle,
+   * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param J The moment of inertia J of the arm in kg-m².
+   * @param gearing The gearing between the motor and arm, in output over input. Most of the time
+   *     this will be greater than 1.
+   * @return A LinearSystem representing the given characterized constants.
+   */
+  public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
+      DCMotor motor, double J, double gearing) {
+    if (J <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (gearing <= 0.0) {
+      throw new IllegalArgumentException("gearing must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        MatBuilder.fill(
+            Nat.N2(),
+            Nat.N2(),
+            0,
+            1,
+            0,
+            -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)),
+        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
+   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+   * [velocity], inputs are [voltage], and outputs are [velocity].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * <p>The parameters provided by the user are from this feedforward model:
+   *
+   * <p>u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec)
+   * @param kA The acceleration gain, in volts/(unit/sec²)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
+   * @see <a
+   *     href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
+   */
+  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
+    if (kV < 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        VecBuilder.fill(-kV / kA),
+        VecBuilder.fill(1.0 / kA),
+        VecBuilder.fill(1.0),
+        VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
+   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
+   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * <p>The parameters provided by the user are from this feedforward model:
+   *
+   * <p>u = K_v v + K_a a
+   *
+   * @param kV The velocity gain, in volts/(unit/sec)
+   * @param kA The acceleration gain, in volts/(unit/sec²)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
+   * @see <a
+   *     href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
+   */
+  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.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
+        VecBuilder.fill(0.0, 1.0 / kA),
+        Matrix.eye(Nat.N2()),
+        new Matrix<>(Nat.N2(), Nat.N1()));
+  }
+
+  /**
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
+   * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
+   * (volts/(radian/sec²))} cases. These constants can be found using SysId.
+   *
+   * <p>States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (meters per second).
+   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
+   *     kAAngular &lt;= 0.
+   * @see <a
+   *     href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
+   */
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+
+    final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
+    final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
+    final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
+    final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
+
+    return new LinearSystem<>(
+        MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
+        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
+  }
+
+  /**
+   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
+   * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
+   * (volts/(radian/sec²))} cases. This can be found using SysId.
+   *
+   * <p>States: [[left velocity], [right velocity]]<br>
+   * Inputs: [[left voltage], [right voltage]]<br>
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVLinear The linear velocity gain in volts per (meters per second).
+   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
+   * @param kVAngular The angular velocity gain in volts per (radians per second).
+   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
+   * @param trackwidth The distance between the differential drive's left and right wheels, in
+   *     meters.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
+   *     kAAngular &lt;= 0, or trackwidth &lt;= 0.
+   * @see <a
+   *     href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
+   */
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+    if (trackwidth <= 0.0) {
+      throw new IllegalArgumentException("trackwidth must be greater than zero.");
+    }
+
+    // We want to find a factor to include in Kv,angular that will convert
+    // `u = Kv,angular omega` to `u = Kv,angular v`.
+    //
+    // v = omega r
+    // omega = v/r
+    // omega = 1/r v
+    // omega = 1/(trackwidth/2) v
+    // omega = 2/trackwidth v
+    //
+    // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
+    // to V/(m/s).
+    return identifyDrivetrainSystem(
+        kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/plant/proto/DCMotorProto.java

@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.proto;
+
+import edu.wpi.first.math.proto.Plant.ProtobufDCMotor;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DCMotorProto implements Protobuf<DCMotor, ProtobufDCMotor> {
+  @Override
+  public Class<DCMotor> getTypeClass() {
+    return DCMotor.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDCMotor.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDCMotor createMessage() {
+    return ProtobufDCMotor.newInstance();
+  }
+
+  @Override
+  public DCMotor unpack(ProtobufDCMotor msg) {
+    return new DCMotor(
+        msg.getNominalVoltage(),
+        msg.getStallTorque(),
+        msg.getStallCurrent(),
+        msg.getFreeCurrent(),
+        msg.getFreeSpeed(),
+        1);
+  }
+
+  @Override
+  public void pack(ProtobufDCMotor msg, DCMotor value) {
+    msg.setNominalVoltage(value.nominalVoltage);
+    msg.setStallTorque(value.stallTorque);
+    msg.setStallCurrent(value.stallCurrent);
+    msg.setFreeCurrent(value.freeCurrent);
+    msg.setFreeSpeed(value.freeSpeed);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/plant/struct/DCMotorStruct.java

@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.math.system.plant.struct;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DCMotorStruct implements Struct<DCMotor> {
+  @Override
+  public Class<DCMotor> getTypeClass() {
+    return DCMotor.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "DCMotor";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 5;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double nominal_voltage;double stall_torque;double stall_current;"
+        + "double free_current;double free_speed";
+  }
+
+  @Override
+  public DCMotor unpack(ByteBuffer bb) {
+    double nominalVoltage = bb.getDouble();
+    double stallTorque = bb.getDouble();
+    double stallCurrent = bb.getDouble();
+    double freeCurrent = bb.getDouble();
+    double freeSpeed = bb.getDouble();
+    return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed, 1);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DCMotor value) {
+    bb.putDouble(value.nominalVoltage);
+    bb.putDouble(value.stallTorque);
+    bb.putDouble(value.stallCurrent);
+    bb.putDouble(value.freeCurrent);
+    bb.putDouble(value.freeSpeed);
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/proto/LinearSystemProto.java

@@ -0,0 +1,99 @@
+// 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.
+
+package edu.wpi.first.math.system.proto;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.proto.System.ProtobufLinearSystem;
+import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class LinearSystemProto<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements Protobuf<LinearSystem<States, Inputs, Outputs>, ProtobufLinearSystem> {
+  private final Nat<States> m_states;
+  private final Nat<Inputs> m_inputs;
+  private final Nat<Outputs> m_outputs;
+  private final Protobuf<Matrix<States, States>, ProtobufMatrix> m_AProto;
+  private final Protobuf<Matrix<States, Inputs>, ProtobufMatrix> m_BProto;
+  private final Protobuf<Matrix<Outputs, States>, ProtobufMatrix> m_CProto;
+  private final Protobuf<Matrix<Outputs, Inputs>, ProtobufMatrix> m_DProto;
+
+  /**
+   * Constructs the {@link Protobuf} implementation.
+   *
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   */
+  public LinearSystemProto(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    m_states = states;
+    m_inputs = inputs;
+    m_outputs = outputs;
+    m_AProto = Matrix.getProto(states, states);
+    m_BProto = Matrix.getProto(states, inputs);
+    m_CProto = Matrix.getProto(outputs, states);
+    m_DProto = Matrix.getProto(outputs, inputs);
+  }
+
+  @Override
+  public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
+    @SuppressWarnings("unchecked")
+    var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
+    return clazz;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufLinearSystem.getDescriptor();
+  }
+
+  @Override
+  public ProtobufLinearSystem createMessage() {
+    return ProtobufLinearSystem.newInstance();
+  }
+
+  @Override
+  public LinearSystem<States, Inputs, Outputs> unpack(ProtobufLinearSystem msg) {
+    if (msg.getNumStates() != m_states.getNum()
+        || msg.getNumInputs() != m_inputs.getNum()
+        || msg.getNumOutputs() != m_outputs.getNum()) {
+      throw new IllegalArgumentException(
+          "Tried to unpack msg "
+              + msg
+              + " with "
+              + msg.getNumStates()
+              + " states and "
+              + msg.getNumInputs()
+              + " inputs and "
+              + msg.getNumOutputs()
+              + " outputs into LinearSystem with "
+              + m_states.getNum()
+              + " states "
+              + m_inputs.getNum()
+              + " inputs "
+              + m_outputs.getNum()
+              + " outputs");
+    }
+    return new LinearSystem<>(
+        m_AProto.unpack(msg.getA()),
+        m_BProto.unpack(msg.getB()),
+        m_CProto.unpack(msg.getC()),
+        m_DProto.unpack(msg.getD()));
+  }
+
+  @Override
+  public void pack(ProtobufLinearSystem msg, LinearSystem<States, Inputs, Outputs> value) {
+    msg.setNumStates(m_states.getNum())
+        .setNumInputs(m_inputs.getNum())
+        .setNumOutputs(m_outputs.getNum());
+    m_AProto.pack(msg.getMutableA(), value.getA());
+    m_BProto.pack(msg.getMutableB(), value.getB());
+    m_CProto.pack(msg.getMutableC(), value.getC());
+    m_DProto.pack(msg.getMutableD(), value.getD());
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/struct/LinearSystemStruct.java

@@ -0,0 +1,89 @@
+// 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.
+
+package edu.wpi.first.math.system.struct;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.struct.MatrixStruct;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class LinearSystemStruct<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements Struct<LinearSystem<States, Inputs, Outputs>> {
+  private final int m_states;
+  private final int m_inputs;
+  private final int m_outputs;
+  private final MatrixStruct<States, States> m_AStruct;
+  private final MatrixStruct<States, Inputs> m_BStruct;
+  private final MatrixStruct<Outputs, States> m_CStruct;
+  private final MatrixStruct<Outputs, Inputs> m_DStruct;
+
+  /**
+   * Constructs the {@link Struct} implementation.
+   *
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   */
+  public LinearSystemStruct(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    m_states = states.getNum();
+    m_inputs = inputs.getNum();
+    m_outputs = outputs.getNum();
+    m_AStruct = Matrix.getStruct(states, states);
+    m_BStruct = Matrix.getStruct(states, inputs);
+    m_CStruct = Matrix.getStruct(outputs, states);
+    m_DStruct = Matrix.getStruct(outputs, inputs);
+  }
+
+  @Override
+  public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
+    @SuppressWarnings("unchecked")
+    var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
+    return clazz;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "LinearSystem__" + m_states + "_" + m_inputs + "_" + m_outputs;
+  }
+
+  @Override
+  public int getSize() {
+    return m_AStruct.getSize() + m_BStruct.getSize() + m_CStruct.getSize() + m_DStruct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return m_AStruct.getTypeName()
+        + " a;"
+        + m_BStruct.getTypeName()
+        + " b;"
+        + m_CStruct.getTypeName()
+        + " c;"
+        + m_DStruct.getTypeName()
+        + " d";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {m_AStruct, m_BStruct, m_CStruct, m_DStruct};
+  }
+
+  @Override
+  public LinearSystem<States, Inputs, Outputs> unpack(ByteBuffer bb) {
+    return new LinearSystem<>(
+        m_AStruct.unpack(bb), m_BStruct.unpack(bb), m_CStruct.unpack(bb), m_DStruct.unpack(bb));
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, LinearSystem<States, Inputs, Outputs> value) {
+    m_AStruct.pack(bb, value.getA());
+    m_BStruct.pack(bb, value.getB());
+    m_CStruct.pack(bb, value.getC());
+    m_DStruct.pack(bb, value.getD());
+  }
+}