Class LinearQuadraticRegulator<States extends Num,​Inputs extends Num,​Outputs extends Num>

java.lang.Object
edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator<States,​Inputs,​Outputs>

public class LinearQuadraticRegulator<States extends Num,​Inputs extends Num,​Outputs extends Num>
extends Object
Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use the control law u = K(r - x).

For more on the underlying math, read https://file.tavsys.net/control/controls-engineering-in-frc.pdf.

  • Constructor Details

    • LinearQuadraticRegulator

      public LinearQuadraticRegulator​(LinearSystem<States,​Inputs,​Outputs> plant, Vector<States> qelms, Vector<Inputs> relms, double dtSeconds)
      Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
      Parameters:
      plant - The plant being controlled.
      qelms - The maximum desired error tolerance for each state.
      relms - The maximum desired control effort for each input.
      dtSeconds - Discretization timestep.
    • LinearQuadraticRegulator

      public LinearQuadraticRegulator​(Matrix<States,​States> A, Matrix<States,​Inputs> B, Vector<States> qelms, Vector<Inputs> relms, double dtSeconds)
      Constructs a controller with the given coefficients and plant.
      Parameters:
      A - Continuous system matrix of the plant being controlled.
      B - Continuous input matrix of the plant being controlled.
      qelms - The maximum desired error tolerance for each state.
      relms - The maximum desired control effort for each input.
      dtSeconds - Discretization timestep.
    • LinearQuadraticRegulator

      public LinearQuadraticRegulator​(Matrix<States,​States> A, Matrix<States,​Inputs> B, Matrix<States,​States> Q, Matrix<Inputs,​Inputs> R, double dtSeconds)
      Constructs a controller with the given coefficients and plant.
      Parameters:
      A - Continuous system matrix of the plant being controlled.
      B - Continuous input matrix of the plant being controlled.
      Q - The state cost matrix.
      R - The input cost matrix.
      dtSeconds - Discretization timestep.
    • LinearQuadraticRegulator

      public LinearQuadraticRegulator​(Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs,​States> k)
      Constructs a controller with the given coefficients and plant.
      Parameters:
      states - The number of states.
      inputs - The number of inputs.
      k - The gain matrix.
  • Method Details

    • getU

      public Matrix<Inputs,​N1> getU()
      Returns the control input vector u.
      Returns:
      The control input.
    • getU

      public double getU​(int row)
      Returns an element of the control input vector u.
      Parameters:
      row - Row of u.
      Returns:
      The row of the control input vector.
    • getR

      public Matrix<States,​N1> getR()
      Returns the reference vector r.
      Returns:
      The reference vector.
    • getR

      public double getR​(int row)
      Returns an element of the reference vector r.
      Parameters:
      row - Row of r.
      Returns:
      The row of the reference vector.
    • getK

      public Matrix<Inputs,​States> getK()
      Returns the controller matrix K.
      Returns:
      the controller matrix K.
    • reset

      public void reset()
      Resets the controller.
    • calculate

      public Matrix<Inputs,​N1> calculate​(Matrix<States,​N1> x)
      Returns the next output of the controller.
      Parameters:
      x - The current state x.
    • calculate

      public Matrix<Inputs,​N1> calculate​(Matrix<States,​N1> x, Matrix<States,​N1> nextR)
      Returns the next output of the controller.
      Parameters:
      x - The current state x.
      nextR - the next reference vector r.
    • latencyCompensate

      public void latencyCompensate​(LinearSystem<States,​Inputs,​Outputs> plant, double dtSeconds, double inputDelaySeconds)
      Adjusts LQR controller gain to compensate for a pure time delay in the input.

      Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we can compute the control based on where the system will be after the time delay.

      See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a derivation.

      Parameters:
      plant - The plant being controlled.
      dtSeconds - Discretization timestep in seconds.
      inputDelaySeconds - Input time delay in seconds.