mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpimath] Add core State-space classes (#2614)
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
@@ -0,0 +1,215 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
import java.util.function.BiFunction;
|
||||
import java.util.function.Function;
|
||||
|
||||
import edu.wpi.first.wpilibj.system.NumericalJacobian;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.Num;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
/**
|
||||
* Constructs a control-affine plant inversion model-based feedforward from
|
||||
* given model dynamics.
|
||||
*
|
||||
* <p>If given the vector valued function as f(x, u) where x is the state
|
||||
* vector and u is the input vector, the B matrix(continuous input matrix)
|
||||
* is calculated through a {@link edu.wpi.first.wpilibj.system.NumericalJacobian}.
|
||||
* In this case f has to be control-affine (of the form f(x) + Bu).
|
||||
*
|
||||
* <p>The feedforward is calculated as
|
||||
* <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
|
||||
* <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* <p>This feedforward does not account for a dynamic B matrix, B is either
|
||||
* determined or supplied when the feedforward is created and remains constant.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
|
||||
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
|
||||
/**
|
||||
* The current reference state.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/**
|
||||
* The computed feedforward.
|
||||
*/
|
||||
private Matrix<Inputs, N1> m_uff;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, Inputs> m_B;
|
||||
|
||||
private final Nat<Inputs> m_inputs;
|
||||
|
||||
private final double m_dt;
|
||||
|
||||
/**
|
||||
* The model dynamics.
|
||||
*/
|
||||
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function
|
||||
* of state and input.
|
||||
*
|
||||
* @param states A {@link Nat} representing the number of states.
|
||||
* @param inputs A {@link Nat} representing the number of inputs.
|
||||
* @param f A vector-valued function of x, the state, and
|
||||
* u, the input, that returns the derivative of
|
||||
* the state vector. HAS to be control-affine
|
||||
* (of the form f(x) + Bu).
|
||||
* @param dtSeconds The timestep between calls of calculate().
|
||||
*/
|
||||
public ControlAffinePlantInversionFeedforward(
|
||||
Nat<States> states,
|
||||
Nat<Inputs> inputs,
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
double dtSeconds) {
|
||||
this.m_dt = dtSeconds;
|
||||
this.m_f = f;
|
||||
this.m_inputs = inputs;
|
||||
|
||||
this.m_B = NumericalJacobian.numericalJacobianU(states, inputs,
|
||||
m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_uff = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with given model dynamics as a function of state,
|
||||
* and the plant's B(continuous input matrix) matrix.
|
||||
*
|
||||
* @param states A {@link Nat} representing the number of states.
|
||||
* @param inputs A {@link Nat} representing the number of inputs.
|
||||
* @param f A vector-valued function of x, the state,
|
||||
* that returns the derivative of the state vector.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds The timestep between calls of calculate().
|
||||
*/
|
||||
public ControlAffinePlantInversionFeedforward(
|
||||
Nat<States> states,
|
||||
Nat<Inputs> inputs,
|
||||
Function<Matrix<States, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, Inputs> B,
|
||||
double dtSeconds) {
|
||||
this.m_dt = dtSeconds;
|
||||
this.m_inputs = inputs;
|
||||
|
||||
this.m_f = (x, u) -> f.apply(x);
|
||||
this.m_B = B;
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_uff = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getUff() {
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
public double getUff(int row) {
|
||||
return m_uff.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the current reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
public void reset(Matrix<States, N1> initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* <p>If this method is used the initial state of the system is the one
|
||||
* set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
|
||||
* If the initial state is not set it defaults to a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
|
||||
return calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
|
||||
var rDot = (nextR.minus(r)).div(m_dt);
|
||||
|
||||
m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,170 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
import edu.wpi.first.wpilibj.math.Discretization;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Num;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
/**
|
||||
* Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
|
||||
*
|
||||
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
|
||||
* where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
|
||||
public class LinearPlantInversionFeedforward<States extends Num, Inputs extends Num,
|
||||
Outputs extends Num> {
|
||||
/**
|
||||
* The current reference state.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/**
|
||||
* The computed feedforward.
|
||||
*/
|
||||
private Matrix<Inputs, N1> m_uff;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, Inputs> m_B;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, States> m_A;
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
public LinearPlantInversionFeedforward(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
double dtSeconds
|
||||
) {
|
||||
this(plant.getA(), plant.getB(), dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a feedforward with the given coefficients.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearPlantInversionFeedforward(Matrix<States, States> A, Matrix<States, Inputs> B,
|
||||
double dtSeconds) {
|
||||
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
|
||||
this.m_A = discABPair.getFirst();
|
||||
this.m_B = discABPair.getSecond();
|
||||
|
||||
m_r = new Matrix<States, N1>(new SimpleMatrix(B.getNumRows(), 1));
|
||||
m_uff = new Matrix<Inputs, N1>(new SimpleMatrix(B.getNumCols(), 1));
|
||||
|
||||
reset(m_r);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the previously calculated feedforward as an input vector.
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getUff() {
|
||||
return m_uff;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the previously calculated feedforward.
|
||||
*
|
||||
* @param row Row of uff.
|
||||
*
|
||||
* @return The row of the calculated feedforward.
|
||||
*/
|
||||
public double getUff(int row) {
|
||||
return m_uff.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current reference vector r.
|
||||
*
|
||||
* @return The current reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the current reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
*
|
||||
* @return The row of the current reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a specified initial state vector.
|
||||
*
|
||||
* @param initialState The initial state vector.
|
||||
*/
|
||||
public void reset(Matrix<States, N1> initialState) {
|
||||
m_r = initialState;
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the feedforward with a zero initial state vector.
|
||||
*/
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_uff.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with only the desired
|
||||
* future reference. This uses the internally stored "current"
|
||||
* reference.
|
||||
*
|
||||
* <p>If this method is used the initial state of the system is the one
|
||||
* set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
|
||||
* If the initial state is not set it defaults to a zero vector.
|
||||
*
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
|
||||
return calculate(m_r, nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the feedforward with current and future reference vectors.
|
||||
*
|
||||
* @param r The reference state of the current timestep (k).
|
||||
* @param nextR The reference state of the future timestep (k + dt).
|
||||
*
|
||||
* @return The calculated feedforward.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
|
||||
m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
|
||||
|
||||
m_r = nextR;
|
||||
return m_uff;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,254 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.controller;
|
||||
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
import edu.wpi.first.math.Drake;
|
||||
import edu.wpi.first.wpilibj.math.Discretization;
|
||||
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpiutil.math.Matrix;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.Num;
|
||||
import edu.wpi.first.wpiutil.math.Vector;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
|
||||
/**
|
||||
* Contains the controller coefficients and logic for a linear-quadratic
|
||||
* regulator (LQR).
|
||||
* LQRs use the control law u = K(r - x).
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings("ClassTypeParameterName")
|
||||
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
|
||||
Outputs extends Num> {
|
||||
/**
|
||||
* The current reference state.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<States, N1> m_r;
|
||||
|
||||
/**
|
||||
* The computed and capped controller output.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<Inputs, N1> m_u;
|
||||
|
||||
// Controller gain.
|
||||
@SuppressWarnings("MemberName")
|
||||
private Matrix<Inputs, States> m_K;
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
public LinearQuadraticRegulator(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
Vector<States> qelms,
|
||||
Vector<Inputs> relms,
|
||||
double dtSeconds
|
||||
) {
|
||||
this(plant.getA(), plant.getB(), qelms, 1.0, relms, dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param plant The plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state excursion.
|
||||
* Greater values penalize state excursion more heavily. 1 is a good starting
|
||||
* value.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
public LinearQuadraticRegulator(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
Vector<States> qelms,
|
||||
double rho,
|
||||
Vector<Inputs> relms,
|
||||
double dtSeconds
|
||||
) {
|
||||
this(plant.getA(), plant.getB(), qelms, rho, relms, dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
|
||||
Vector<States> qelms, Vector<Inputs> relms,
|
||||
double dtSeconds
|
||||
) {
|
||||
this(A, B, qelms, 1.0, relms, dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param qelms The maximum desired error tolerance for each state.
|
||||
* @param rho A weighting factor that balances control effort and state excursion.
|
||||
* Greater
|
||||
* values penalize state excursion more heavily. 1 is a good starting value.
|
||||
* @param relms The maximum desired control effort for each input.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
|
||||
Vector<States> qelms, double rho, Vector<Inputs> relms,
|
||||
double dtSeconds
|
||||
) {
|
||||
this(A, B, StateSpaceUtil.makeCostMatrix(qelms).times(rho),
|
||||
StateSpaceUtil.makeCostMatrix(relms), dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
* @param A Continuous system matrix of the plant being controlled.
|
||||
* @param B Continuous input matrix of the plant being controlled.
|
||||
* @param Q The state cost matrix.
|
||||
* @param R The input cost matrix.
|
||||
* @param dtSeconds Discretization timestep.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
|
||||
Matrix<States, States> Q, Matrix<Inputs, Inputs> R,
|
||||
double dtSeconds
|
||||
) {
|
||||
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
|
||||
var discA = discABPair.getFirst();
|
||||
var discB = discABPair.getSecond();
|
||||
|
||||
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
|
||||
|
||||
var temp = discB.transpose().times(S).times(discB).plus(R);
|
||||
|
||||
m_K = temp.solve(discB.transpose().times(S).times(discA));
|
||||
|
||||
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
|
||||
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a controller with the given coefficients and plant.
|
||||
*
|
||||
* @param states The number of states.
|
||||
* @param inputs The number of inputs.
|
||||
* @param k The gain matrix.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public LinearQuadraticRegulator(
|
||||
Nat<States> states, Nat<Inputs> inputs,
|
||||
Matrix<Inputs, States> k
|
||||
) {
|
||||
m_K = k;
|
||||
|
||||
m_r = new Matrix<>(states, Nat.N1());
|
||||
m_u = new Matrix<>(inputs, Nat.N1());
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the control input vector u.
|
||||
*
|
||||
* @return The control input.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getU() {
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the control input vector u.
|
||||
*
|
||||
* @param row Row of u.
|
||||
*
|
||||
* @return The row of the control input vector.
|
||||
*/
|
||||
public double getU(int row) {
|
||||
return m_u.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the reference vector r.
|
||||
*
|
||||
* @return The reference vector.
|
||||
*/
|
||||
public Matrix<States, N1> getR() {
|
||||
return m_r;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the reference vector r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
*
|
||||
* @return The row of the reference vector.
|
||||
*/
|
||||
public double getR(int row) {
|
||||
return m_r.get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the controller matrix K.
|
||||
*
|
||||
* @return the controller matrix K.
|
||||
*/
|
||||
public Matrix<Inputs, States> getK() {
|
||||
return m_K;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the controller.
|
||||
*/
|
||||
public void reset() {
|
||||
m_r.fill(0.0);
|
||||
m_u.fill(0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
|
||||
m_u = m_K.times(m_r.minus(x));
|
||||
return m_u;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the controller.
|
||||
*
|
||||
* @param x The current state x.
|
||||
* @param nextR the next reference vector r.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
|
||||
m_r = nextR;
|
||||
return calculate(x);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user