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

[wpimath] Fix precondition violation messages in LQR and Kalman filters (#6731)

Browse Source
This commit is contained in:
Tyler Veness
2024-06-21 20:55:01 -07:00
committed by GitHub
parent d2b1aa1869
commit b8c2571638
9 changed files with 20 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -47,7 +47,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
* @throws IllegalArgumentException If the system is uncontrollable.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
LinearSystem<States, Inputs, Outputs> plant,
@@ -74,7 +74,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
* @throws IllegalArgumentException If the system is uncontrollable.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
Matrix<States, States> A,
@@ -98,7 +98,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dtSeconds Discretization timestep.
* @throws IllegalArgumentException If the system is uncontrollable.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
Matrix<States, States> A,
@@ -112,7 +112,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
if (!StateSpaceUtil.isStabilizable(discA, discB)) {
var msg =
"The system passed to the LQR is uncontrollable!\n\nA =\n"
"The system passed to the LQR is unstabilizable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nB =\n"
+ discB.getStorage().toString()
@@ -147,7 +147,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dtSeconds Discretization timestep.
* @throws IllegalArgumentException If the system is uncontrollable.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
Matrix<States, States> A,

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

@@ -59,7 +59,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @throws IllegalArgumentException If the system is unobservable.
* @throws IllegalArgumentException If the system is undetectable.
*/
public KalmanFilter(
Nat<States> states,
@@ -87,7 +87,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is unobservable!\n\nA =\n"
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()

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

@@ -60,7 +60,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @throws IllegalArgumentException If the system is unobservable.
* @throws IllegalArgumentException If the system is undetectable.
*/
public SteadyStateKalmanFilter(
Nat<States> states,
@@ -86,7 +86,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
if (!StateSpaceUtil.isDetectable(discA, C)) {
var msg =
"The system passed to the Kalman filter is unobservable!\n\nA =\n"
"The system passed to the Kalman filter is undetectable!\n\nA =\n"
+ discA.getStorage().toString()
+ "\nC =\n"
+ C.getStorage().toString()