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

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

wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp

@@ -41,7 +41,7 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
   // I = V / R - omega / (Kv * R)
   // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
   // is spinning 10x faster than the output.
-  return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+  return m_gearbox.Current(units::radians_per_second_t{m_x(1)} * m_gearing,
                            units::volt_t{m_u(0)}) *
          wpi::sgn(m_u(0));
 }