Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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

24
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -87,9 +87,9 @@ public final class StateSpaceUtil {
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
* where n is number of states.
* where n is the number of states.
*
* @param <States> Num representing the size of A.
* @param <Inputs> Num representing the columns of B.
@@ -103,6 +103,26 @@ public final class StateSpaceUtil {
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
}
/**
* Returns true if (A, C) is a detectable pair.
*
* <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
* values less than one, where an eigenvalue is unobservable if rank(λI - A; C) %3C n where n is
* the number of states.
*
* @param <States> Num representing the size of A.
* @param <Outputs> Num representing the rows of C.
* @param A System matrix.
* @param C Output matrix.
* @return If the system is detectable.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Outputs extends Num> boolean isDetectable(
Matrix<States, States> A, Matrix<Outputs, States> C) {
return WPIMathJNI.isStabilizable(
A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
}
/**
* Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
*