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

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

wpimath/src/main/native/include/frc/system/LinearSystem.h

@@ -5,9 +5,13 @@
 #pragma once
 
 #include <algorithm>
+#include <concepts>
 #include <functional>
 #include <stdexcept>
 
+#include <wpi/Algorithm.h>
+#include <wpi/SmallVector.h>
+
 #include "frc/EigenCore.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
@@ -164,6 +168,56 @@ class LinearSystem {
     return m_C * x + m_D * clampedU;
   }
 
+  /**
+   * Returns the LinearSystem with the outputs listed in outputIndices.
+   *
+   * <p>This is used by state observers such as the Kalman Filter.
+   *
+   * @param outputIndices the list of output indices to include in the sliced
+   * system.
+   * @return the sliced LinearSystem with outputs set to row vectors of
+   * LinearSystem.
+   * @throws std::domain_error if any outputIndices are outside the range of
+   * system outputs.
+   * @throws std::domain_error if number of outputIndices exceeds the system
+   * outputs.
+   * @throws std::domain_error if duplication exists in outputIndices.
+   */
+  template <std::same_as<int>... OutputIndices>
+  LinearSystem<States, Inputs, sizeof...(OutputIndices)> Slice(
+      OutputIndices... outputIndices) {
+    static_assert(sizeof...(OutputIndices) <= Outputs,
+                  "More outputs requested than available. This is usually due "
+                  "to model implementation errors.");
+
+    wpi::for_each(
+        [](size_t i, const auto& elem) {
+          if (elem < 0 || elem >= Outputs) {
+            throw std::domain_error(
+                "Slice indices out of range. This is usually due to model "
+                "implementation errors.");
+          }
+        },
+        outputIndices...);
+
+    // Sort and deduplicate output indices
+    wpi::SmallVector<int> outputIndicesArray{outputIndices...};
+    std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
+    auto last =
+        std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
+    outputIndicesArray.erase(last, outputIndicesArray.end());
+
+    if (outputIndicesArray.size() != sizeof...(outputIndices)) {
+      throw std::domain_error(
+          "Duplicate indices exist. This is usually due to model "
+          "implementation errors.");
+    }
+
+    return LinearSystem<States, Inputs, sizeof...(OutputIndices)>{
+        m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all),
+        m_D(outputIndicesArray, Eigen::placeholders::all)};
+  }
+
  private:
   /**
    * Continuous system matrix.

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -44,7 +44,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param gearing Gear ratio from motor to carriage.
    * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
    */
-  static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
+  static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
                                               units::kilogram_t mass,
                                               units::meter_t radius,
                                               double gearing);
@@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param gearing Gear ratio from motor to arm.
    * @throws std::domain_error if J <= 0 or gearing <= 0.
    */
-  static LinearSystem<2, 1, 1> SingleJointedArmSystem(
+  static LinearSystem<2, 1, 2> SingleJointedArmSystem(
       DCMotor motor, units::kilogram_square_meter_t J, double gearing);
 
   /**