[wpimath] Replace UKF implementation with square root form (#4168)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/Matrix.java

@@ -323,6 +323,10 @@ public class Matrix<R extends Num, C extends Num> {
    * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
    * is used if A is not square.
    *
+   * <p>Note that this method does not support solving using a QR decomposition with full-pivoting,
+   * as only column-pivoting is supported. For full-pivoting, use {@link
+   * #solveFullPivHouseholderQr}.
+   *
    * @param <C2> Columns in b.
    * @param b The right-hand side of the equation to solve.
    * @return The solution to the linear system.
@@ -332,6 +336,29 @@ public class Matrix<R extends Num, C extends Num> {
     return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
   }
 
+  /**
+   * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting, where this
+   * matrix is A.
+   *
+   * @param <R2> Number of rows in B.
+   * @param <C2> Number of columns in B.
+   * @param other The B matrix.
+   * @return The solution matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
+      Matrix<R2, C2> other) {
+    Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
+    WPIMathJNI.solveFullPivHouseholderQr(
+        this.getData(),
+        this.getNumRows(),
+        this.getNumCols(),
+        other.getData(),
+        other.getNumRows(),
+        other.getNumCols(),
+        solution.getData());
+    return solution;
+  }
+
   /**
    * Computes the matrix exponential using Eigen's solver. This method only works for square
    * matrices, and will otherwise throw an {@link MatrixDimensionException}.
@@ -677,6 +704,20 @@ public class Matrix<R extends Num, C extends Num> {
         this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
   }
 
+  /**
+   * Performs an inplace Cholesky rank update (or downdate).
+   *
+   * <p>If this matrix contains L where A = LL<sup>&top;</sup> before the update, it will contain L
+   * where LL<sup>&top;</sup> = A + &sigma;vv<sup>&top;</sup> after the update.
+   *
+   * @param v Vector to use for the update.
+   * @param sigma Sigma to use for the update.
+   * @param lowerTriangular Whether or not this matrix is lower triangular.
+   */
+  public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
+    WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
+  }
+
   @Override
   public String toString() {
     return m_storage.toString();