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] Use SDA algorithm instead of SSCA for DARE solver (#5526)

Browse Source 
Both seem to work, but the SDA algorithm is specifically recommended for
solving DAREs as opposed to P-DAREs.

The QR decomposition was replaced with a partial pivoting LU
decomposition at the recommendation of section 2.4 of the paper.

More tests and a separate JNI function for each DARE solver variant were
added.
This commit is contained in:
Tyler Veness
2023-08-12 19:45:45 -07:00
committed by GitHub
parent a4b7fde767
commit 394cfeadbd
7 changed files with 549 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -53,10 +53,40 @@ public final class WPIMathJNI {
* @param states Number of states in A matrix.
* @param inputs Number of inputs in B matrix.
* @param S Array storage for DARE solution.
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dare(
public static native void dareABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
* @param Q Array containing elements of Q in row-major order.
* @param R Array containing elements of R in row-major order.
* @param N Array containing elements of N in row-major order.
* @param states Number of states in A matrix.
* @param inputs Number of inputs in B matrix.
* @param S Array storage for DARE solution.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dareABQRN(
double[] A,
double[] B,
double[] Q,
double[] R,
double[] N,
int states,
int inputs,
double[] S);
/**
* Computes the matrix exp.
*