Class StateSpaceUtil

java.lang.Object
org.wpilib.math.util.StateSpaceUtil
public final class StateSpaceUtil extends Object
State-space utilities.

  • Method Details

    • costMatrix

      public static <Elements extends Num> Matrix<Elements,Elements> costMatrix(Matrix<Elements,N1> tolerances)
      Creates a cost matrix from the given vector for use with LQR.

      The cost matrix is constructed using Bryson's rule. The inverse square of each tolerance is placed on the cost matrix diagonal. If a tolerance is infinity, its cost matrix entry is set to zero.

      Type Parameters:
      Elements - Nat representing the number of system states or inputs.
      Parameters:
      tolerances - An array. For a Q matrix, its elements are the maximum allowed excursions of the states from the reference. For an R matrix, its elements are the maximum allowed excursions of the control inputs from no actuation.
      Returns:
      State excursion or control effort cost matrix.

    • covarianceMatrix

      public static <States extends Num> Matrix<States,States> covarianceMatrix(Nat<States> states, Matrix<States,N1> stdDevs)
      Creates a covariance matrix from the given vector for use with Kalman filters.

      Each element is squared and placed on the covariance matrix diagonal.

      Type Parameters:
      States - Num representing the states of the system.
      Parameters:
      states - A Nat representing the states of the system.
      stdDevs - For a Q matrix, its elements are the standard deviations of each state from how the model behaves. For an R matrix, its elements are the standard deviations for each output measurement.
      Returns:
      Process noise or measurement noise covariance matrix.

    • desaturateInputVector

      public static <I extends Num> Matrix<I,N1> desaturateInputVector(Matrix<I,N1> u, double maxMagnitude)
      Renormalize all inputs if any exceeds the maximum magnitude. Useful for systems such as differential drivetrains.
      Type Parameters:
      I - Number of inputs.
      Parameters:
      u - The input vector.
      maxMagnitude - The maximum magnitude any input can have.
      Returns:
      The normalizedInput