diff --git a/wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java b/wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java
index dcdb0e7899..1d364f692e 100644
--- a/wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java
+++ b/wpimath/src/main/java/org/wpilib/math/system/LinearSystem.java
@@ -5,31 +5,9 @@
 package org.wpilib.math.system;
 
 import java.util.Arrays;
-import java.util.Collections;
-import java.util.List;
-import java.util.stream.Collectors;
 import org.ejml.simple.SimpleMatrix;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
-import org.wpilib.math.numbers.N10;
-import org.wpilib.math.numbers.N11;
-import org.wpilib.math.numbers.N12;
-import org.wpilib.math.numbers.N13;
-import org.wpilib.math.numbers.N14;
-import org.wpilib.math.numbers.N15;
-import org.wpilib.math.numbers.N16;
-import org.wpilib.math.numbers.N17;
-import org.wpilib.math.numbers.N18;
-import org.wpilib.math.numbers.N19;
-import org.wpilib.math.numbers.N2;
-import org.wpilib.math.numbers.N20;
-import org.wpilib.math.numbers.N3;
-import org.wpilib.math.numbers.N4;
-import org.wpilib.math.numbers.N5;
-import org.wpilib.math.numbers.N6;
-import org.wpilib.math.numbers.N7;
-import org.wpilib.math.numbers.N8;
-import org.wpilib.math.numbers.N9;
 import org.wpilib.math.system.proto.LinearSystemProto;
 import org.wpilib.math.system.struct.LinearSystemStruct;
 import org.wpilib.math.util.Nat;
@@ -254,113 +232,23 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
               + "errors.");
     }
 
-    List<Integer> outputIndicesList =
-        Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList());
-    Collections.sort(outputIndicesList);
+    int[] outputIndicesList = Arrays.stream(outputIndices).distinct().sorted().toArray();
 
-    if (outputIndices.length != outputIndicesList.size()) {
+    if (outputIndices.length != outputIndicesList.length) {
       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());
+    SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.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);
+      new_C_Storage.setRow(row, 0, m_C.extractRowVector(index).getData());
+      new_D_Storage.setRow(row, 0, m_D.extractRowVector(index).getData());
       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);
-    }
+    return new LinearSystem<>(m_A, m_B, new Matrix<>(new_C_Storage), new Matrix<>(new_D_Storage));
   }
 
   @Override