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] Replace UKF implementation with square root form (#4168)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Connor Worley
2022-06-08 22:19:01 -07:00
committed by GitHub
parent 45b7fc445b
commit a99c11c14c
22 changed files with 494 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -125,6 +125,19 @@ public final class WPIMathJNI {
*/
public static native String serializeTrajectory(double[] elements);
/**
* Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
* matrix.
*
* @param mat Array of elements of the matrix to be updated.
* @param lowerTriangular Whether or not mat is lower triangular.
* @param rows How many rows there are.
* @param vec Vector to use for the rank update.
* @param sigma Sigma value to use for the rank update.
*/
public static native void rankUpdate(
double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
@@ -136,4 +149,18 @@ public final class WPIMathJNI {
extractOnStaticLoad.set(load);
}
}
/**
* Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
*
* @param A Array of elements of the A matrix.
* @param Arows Number of rows of the A matrix.
* @param Acols Number of rows of the A matrix.
* @param B Array of elements of the B matrix.
* @param Brows Number of rows of the B matrix.
* @param Bcols Number of rows of the B matrix.
* @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
*/
public static native void solveFullPivHouseholderQr(
double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
}