Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads

Browse Source 
This is useful for implementing implicit model following.
This commit is contained in:
Tyler Veness
2021-02-15 18:17:55 -08:00
committed by Peter Johnson
parent b2c3b2dd8e
commit edd2f0232c
6 changed files with 216 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -113,6 +113,40 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
reset();
}
/**
* 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 N The state-input cross-term 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,
Matrix<States, Inputs> N,
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, N);
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
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.
*