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


  • 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 Detail

      • 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.
    • Method Detail

      • getU

        public Matrix<Inputs,​N1getU()
        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,​N1getR()
        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,​StatesgetK()
        Returns the controller matrix K.
        Returns:
        the controller matrix K.
      • reset

        public void reset()
        Resets the controller.
      • calculate

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

        public Matrix<Inputs,​N1calculate​(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.
        Returns:
        The next controller output.
      • 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.