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] Print uncontrollable/unobservable models in LQR and KF (#3694)

Browse Source 
IsDetectable() was added to make the code easier to read.
This commit is contained in:
Tyler Veness
2021-10-29 00:03:02 -07:00
committed by GitHub
parent d5270d113b
commit a939cd9c89
12 changed files with 169 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,6 +5,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Drake;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -90,7 +91,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param R The input cost matrix.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
@@ -101,6 +102,18 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
builder.append(discA.getStorage().toString());
builder.append("\nB =\n");
builder.append(discB.getStorage().toString());
builder.append("\n");
var msg = builder.toString();
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA