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

[wpimath] DARE: Use wpi::expected instead of exceptions (#7312)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-31 20:37:57 -07:00
committed by GitHub
parent 21980c7447
commit 9f6f267f5c
19 changed files with 717 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,7 +5,6 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.StateSpaceUtil;
@@ -110,17 +109,6 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var msg =
"The system passed to the LQR is unstabilizable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nB =\n"
+ discB.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
var S = DARE.dare(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA

12
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -85,17 +84,6 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
m_initP = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
reset();

12
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.DARE;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -84,17 +83,6 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
var C = plant.getC();
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()
+ '\n';
MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(msg);
}
var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR));
// S = CPCᵀ + R