Class Discretization

java.lang.Object
edu.wpi.first.math.system.Discretization

public final class Discretization
extends Object
  • Method Details

    • discretizeA

      public static <States extends Num> Matrix<States,​States> discretizeA​(Matrix<States,​States> contA, double dtSeconds)
      Discretizes the given continuous A matrix.
      Type Parameters:
      States - Num representing the number of states.
      Parameters:
      contA - Continuous system matrix.
      dtSeconds - Discretization timestep.
      Returns:
      the discrete matrix system.
    • discretizeAB

      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 dtSeconds)
      Discretizes the given continuous A and B matrices.
      Type Parameters:
      States - Nat representing the states of the system.
      Inputs - Nat representing the inputs to the system.
      Parameters:
      contA - Continuous system matrix.
      contB - Continuous input matrix.
      dtSeconds - Discretization timestep.
      Returns:
      a Pair representing discA and diskB.
    • discretizeAQ

      public static <States extends Num> Pair<Matrix<States,​States>,​Matrix<States,​States>> discretizeAQ​(Matrix<States,​States> contA, Matrix<States,​States> contQ, double dtSeconds)
      Discretizes the given continuous A and Q matrices.
      Type Parameters:
      States - Nat representing the number of states.
      Parameters:
      contA - Continuous system matrix.
      contQ - Continuous process noise covariance matrix.
      dtSeconds - Discretization timestep.
      Returns:
      a pair representing the discrete system matrix and process noise covariance matrix.
    • discretizeAQTaylor

      public static <States extends Num> Pair<Matrix<States,​States>,​Matrix<States,​States>> discretizeAQTaylor​(Matrix<States,​States> contA, Matrix<States,​States> contQ, double dtSeconds)
      Discretizes the given continuous A and Q matrices.

      Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive), we take advantage of the structure of the block matrix of A and Q.

      • eᴬᵀ, which is only N x N, is relatively cheap.
      • The upper-right quarter of the 2N x 2N matrix, which we can approximate using a taylor series to several terms and still be substantially cheaper than taking the big exponential.
      Type Parameters:
      States - Nat representing the number of states.
      Parameters:
      contA - Continuous system matrix.
      contQ - Continuous process noise covariance matrix.
      dtSeconds - Discretization timestep.
      Returns:
      a pair representing the discrete system matrix and process noise covariance matrix.
    • discretizeR

      public static <O extends Num> Matrix<O,​O> discretizeR​(Matrix<O,​O> contR, double dtSeconds)
      Returns a discretized version of the provided continuous measurement noise covariance matrix. Note that dt=0.0 divides R by zero.
      Type Parameters:
      O - Nat representing the number of outputs.
      Parameters:
      contR - Continuous measurement noise covariance matrix.
      dtSeconds - Discretization timestep.
      Returns:
      Discretized version of the provided continuous measurement noise covariance matrix.