diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml
index eff78b2887..feccf83370 100644
--- a/.github/workflows/gradle.yml
+++ b/.github/workflows/gradle.yml
@@ -55,7 +55,7 @@ jobs:
             artifact-name: Win64Debug
             architecture: x64
             task: "build"
-            build-options: "-PciDebugOnly"
+            build-options: "-PciDebugOnly --max-workers 1"
           - os: windows-2019
             artifact-name: Win64Release
             architecture: x64
diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
index 113758baa6..7ffe29785a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
+++ b/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();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 54445d3600..0ad89d09f4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -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);
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
index fb0628b2f2..470f560e9e 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -71,16 +71,16 @@ public class MerweScaledSigmaPoints<S extends Num> {
    * of the filter.
    *
    * @param x An array of the means.
-   * @param P Covariance of the filter.
+   * @param s Square-root covariance of the filter.
    * @return Two dimensional array of sigma points. Each column contains all of the sigmas for one
    *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
    */
   @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
+  public Matrix<S, ?> squareRootSigmaPoints(Matrix<S, N1> x, Matrix<S, S> s) {
     double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+    double eta = Math.sqrt(lambda + m_states.getNum());
 
-    var intermediate = P.times(lambda + m_states.getNum());
-    var U = intermediate.lltDecompose(true); // Lower triangular
+    Matrix<S, S> U = s.times(eta);
 
     // 2 * states + 1 by states
     Matrix<S, ?> sigmas =
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
index ffc2c15442..c65be03296 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -14,6 +14,7 @@ import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
 import java.util.function.BiFunction;
+import org.ejml.dense.row.decomposition.qr.QRDecompositionHouseholder_DDRM;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -33,6 +34,9 @@ import org.ejml.simple.SimpleMatrix;
  * <p>For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
  * theory".
+ *
+ * <p>This class implements a square-root-form unscented Kalman filter (SR-UKF). For more
+ * information about the SR-UKF, see https://www.researchgate.net/publication/3908304.
  */
 @SuppressWarnings({"MemberName", "ClassTypeParameterName"})
 public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
@@ -50,7 +54,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
 
   private Matrix<States, N1> m_xHat;
-  private Matrix<States, States> m_P;
+  private Matrix<States, States> m_S;
   private final Matrix<States, States> m_contQ;
   private final Matrix<Outputs, Outputs> m_contR;
   private Matrix<States, ?> m_sigmasF;
@@ -152,14 +156,16 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   }
 
   @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
-      Nat<S> s,
-      Nat<C> dim,
-      Matrix<C, ?> sigmas,
-      Matrix<?, N1> Wm,
-      Matrix<?, N1> Wc,
-      BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
-      BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc) {
+  static <S extends Num, C extends Num>
+      Pair<Matrix<C, N1>, Matrix<C, C>> squareRootUnscentedTransform(
+          Nat<S> s,
+          Nat<C> dim,
+          Matrix<C, ?> sigmas,
+          Matrix<?, N1> Wm,
+          Matrix<?, N1> Wc,
+          BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+          BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc,
+          Matrix<C, C> squareRootR) {
     if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
       throw new IllegalArgumentException(
           "Sigmas must be covDim by 2 * states + 1! Got "
@@ -184,28 +190,64 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     //      k=1
     Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
 
-    // New covariance is the sum of the outer product of the residuals times the
-    // weights
-    Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
-    for (int i = 0; i < 2 * s.getNum() + 1; i++) {
-      // y[:, i] = sigmas[:, i] - x
-      y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
+    Matrix<C, ?> Sbar = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + dim.getNum()));
+    for (int i = 0; i < 2 * s.getNum(); i++) {
+      Sbar.setColumn(
+          i,
+          residualFunc.apply(sigmas.extractColumnVector(1 + i), x).times(Math.sqrt(Wc.get(1, 0))));
     }
-    Matrix<C, C> P =
-        y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
-            .times(Matrix.changeBoundsUnchecked(y.transpose()));
+    Sbar.assignBlock(0, 2 * s.getNum(), squareRootR);
 
-    return new Pair<>(x, P);
+    QRDecompositionHouseholder_DDRM qr = new QRDecompositionHouseholder_DDRM();
+    var qrStorage = Sbar.transpose().getStorage();
+
+    if (!qr.decompose(qrStorage.getDDRM())) {
+      throw new RuntimeException("QR decomposition failed! Input matrix:\n" + qrStorage.toString());
+    }
+
+    Matrix<C, C> newS = new Matrix<>(new SimpleMatrix(qr.getR(null, true)));
+    newS.rankUpdate(residualFunc.apply(sigmas.extractColumnVector(0), x), Wc.get(0, 0), false);
+
+    return new Pair<>(x, newS);
   }
 
   /**
-   * Returns the error covariance matrix P.
+   * Returns the square-root error covariance matrix S.
+   *
+   * @return the square-root error covariance matrix S.
+   */
+  public Matrix<States, States> getS() {
+    return m_S;
+  }
+
+  /**
+   * Returns an element of the square-root error covariance matrix S.
+   *
+   * @param row Row of S.
+   * @param col Column of S.
+   * @return the value of the square-root error covariance matrix S at (i, j).
+   */
+  public double getS(int row, int col) {
+    return m_S.get(row, col);
+  }
+
+  /**
+   * Sets the entire square-root error covariance matrix S.
+   *
+   * @param newS The new value of S to use.
+   */
+  public void setS(Matrix<States, States> newS) {
+    m_S = newS;
+  }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
    *
    * @return the error covariance matrix P.
    */
   @Override
   public Matrix<States, States> getP() {
-    return m_P;
+    return m_S.transpose().times(m_S);
   }
 
   /**
@@ -214,10 +256,12 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    * @param row Row of P.
    * @param col Column of P.
    * @return the value of the error covariance matrix P at (i, j).
+   * @throws UnsupportedOperationException indexing into the reconstructed P matrix is not supported
    */
   @Override
   public double getP(int row, int col) {
-    return m_P.get(row, col);
+    throw new UnsupportedOperationException(
+        "indexing into the reconstructed P matrix is not supported");
   }
 
   /**
@@ -227,7 +271,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
    */
   @Override
   public void setP(Matrix<States, States> newP) {
-    m_P = newP;
+    m_S = newP.lltDecompose(false);
   }
 
   /**
@@ -277,7 +321,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
   @Override
   public void reset() {
     m_xHat = new Matrix<>(m_states, Nat.N1());
-    m_P = new Matrix<>(m_states, m_states);
+    m_S = new Matrix<>(m_states, m_states);
     m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
   }
 
@@ -294,8 +338,9 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     Matrix<States, States> contA =
         NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
     var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+    var squareRootDiscQ = discQ.lltDecompose(true);
 
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
 
     for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
       Matrix<States, N1> x = sigmas.extractColumnVector(i);
@@ -304,17 +349,18 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
     }
 
     var ret =
-        unscentedTransform(
+        squareRootUnscentedTransform(
             m_states,
             m_states,
             m_sigmasF,
             m_pts.getWm(),
             m_pts.getWc(),
             m_meanFuncX,
-            m_residualFuncX);
+            m_residualFuncX,
+            squareRootDiscQ);
 
     m_xHat = ret.getFirst();
-    m_P = ret.getSecond().plus(discQ);
+    m_S = ret.getSecond();
     m_dtSeconds = dtSeconds;
   }
 
@@ -394,10 +440,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
       BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
     final var discR = Discretization.discretizeR(R, m_dtSeconds);
+    final var squareRootDiscR = discR.lltDecompose(true);
 
     // Transform sigma points into measurement space
     Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
     for (int i = 0; i < m_pts.getNumSigmas(); i++) {
       Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
       sigmasH.setColumn(i, hRet);
@@ -405,10 +452,17 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
 
     // Mean and covariance of prediction passed through unscented transform
     var transRet =
-        unscentedTransform(
-            m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
+        squareRootUnscentedTransform(
+            m_states,
+            rows,
+            sigmasH,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            meanFuncY,
+            residualFuncY,
+            squareRootDiscR);
     var yHat = transRet.getFirst();
-    var Py = transRet.getSecond().plus(discR);
+    var Sy = transRet.getSecond();
 
     // Compute cross covariance of the state and the measurements
     Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
@@ -420,17 +474,20 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
       Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
     }
 
-    // K = P_{xy} P_y⁻¹
-    // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
-    // P_yᵀKᵀ = P_{xy}ᵀ
-    // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
-    // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
-    Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
+    // K = (P_{xy} / S_yᵀ) / S_y
+    // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+    // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
+    Matrix<States, R> K =
+        Sy.transpose()
+            .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
+            .transpose();
 
     // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
     m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
 
-    // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
-    m_P = m_P.minus(K.times(Py).times(K.transpose()));
+    Matrix<States, R> U = K.times(Sy);
+    for (int i = 0; i < rows.getNum(); i++) {
+      m_S.rankUpdate(U.extractColumnVector(i), -1, false);
+    }
   }
 }
diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index b036833fac..e7ed5b3626 100644
--- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -8,6 +8,7 @@
 
 #include <wpi/jni_util.h>
 
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
 #include "Eigen/Eigenvalues"
 #include "Eigen/QR"
@@ -307,4 +308,60 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
   }
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    rankUpdate
+ * Signature: ([DI[DDZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
+  (JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
+   jdouble sigma, jboolean lowerTriangular)
+{
+  jdouble* matBody = env->GetDoubleArrayElements(mat, nullptr);
+  jdouble* vecBody = env->GetDoubleArrayElements(vec, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      L{matBody, rows, rows};
+  Eigen::Map<Eigen::Vector<double, Eigen::Dynamic>> v{vecBody, rows};
+
+  if (lowerTriangular == JNI_TRUE) {
+    Eigen::internal::llt_inplace<double, Eigen::Lower>::rankUpdate(L, v, sigma);
+  } else {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(L, v, sigma);
+  }
+
+  env->ReleaseDoubleArrayElements(mat, matBody, 0);
+  env->ReleaseDoubleArrayElements(vec, vecBody, 0);
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    solveFullPivHouseholderQr
+ * Signature: ([DII[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
+  (JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
+   jint Brows, jint Bcols, jdoubleArray dst)
+{
+  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
+  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{nativeA, Arows, Acols};
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Bmat{nativeB, Brows, Bcols};
+
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Xmat =
+      Amat.fullPivHouseholderQr().solve(Bmat);
+
+  env->ReleaseDoubleArrayElements(A, nativeA, 0);
+  env->ReleaseDoubleArrayElements(B, nativeB, 0);
+  env->SetDoubleArrayRegion(dst, 0, Brows * Bcols, Xmat.data());
+}
+
 }  // extern "C"
diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
index 5186d441c9..e2e326b472 100644
--- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
+++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -90,11 +90,15 @@ template <int CovDim, int States>
 Vectord<CovDim> AngleMean(const Matrixd<CovDim, 2 * States + 1>& sigmas,
                           const Vectord<2 * States + 1>& Wm,
                           int angleStatesIdx) {
-  double sumSin = sigmas.row(angleStatesIdx)
-                      .unaryExpr([](auto it) { return std::sin(it); })
+  double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::sin(it);
+                  }) *
+                   Wm)
                       .sum();
-  double sumCos = sigmas.row(angleStatesIdx)
-                      .unaryExpr([](auto it) { return std::cos(it); })
+  double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
+                    return std::cos(it);
+                  }) *
+                   Wm)
                       .sum();
 
   Vectord<CovDim> ret = sigmas * Wm;
diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
index 9156efa66f..d6e41271c8 100644
--- a/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
+++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -21,14 +21,14 @@ class KalmanFilterLatencyCompensator {
  public:
   struct ObserverSnapshot {
     Vectord<States> xHat;
-    Matrixd<States, States> errorCovariances;
+    Matrixd<States, States> squareRootErrorCovariances;
     Vectord<Inputs> inputs;
     Vectord<Outputs> localMeasurements;
 
     ObserverSnapshot(const KalmanFilterType& observer, const Vectord<Inputs>& u,
                      const Vectord<Outputs>& localY)
         : xHat(observer.Xhat()),
-          errorCovariances(observer.P()),
+          squareRootErrorCovariances(observer.S()),
           inputs(u),
           localMeasurements(localY) {}
   };
@@ -135,7 +135,7 @@ class KalmanFilterLatencyCompensator {
       auto& [key, snapshot] = m_pastObserverSnapshots[i];
 
       if (i == indexOfClosestEntry) {
-        observer->SetP(snapshot.errorCovariances);
+        observer->SetS(snapshot.squareRootErrorCovariances);
         observer->SetXhat(snapshot.xHat);
       }
 
diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index 1c46834041..b2e0e458ca 100644
--- a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -6,7 +6,6 @@
 
 #include <cmath>
 
-#include "Eigen/Cholesky"
 #include "frc/EigenCore.h"
 
 namespace frc {
@@ -52,20 +51,21 @@ class MerweScaledSigmaPoints {
 
   /**
    * Computes the sigma points for an unscented Kalman filter given the mean
-   * (x) and covariance(P) of the filter.
+   * (x) and square-root covariance(S) of the filter.
    *
    * @param x An array of the means.
-   * @param P Covariance of the filter.
+   * @param S Square-root covariance of the filter.
    *
    * @return Two dimensional array of sigma points. Each column contains all of
    *         the sigmas for one dimension in the problem space. Ordered by
    *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
    *
    */
-  Matrixd<States, 2 * States + 1> SigmaPoints(
-      const Vectord<States>& x, const Matrixd<States, States>& P) {
+  Matrixd<States, 2 * States + 1> SquareRootSigmaPoints(
+      const Vectord<States>& x, const Matrixd<States, States>& S) {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
-    Matrixd<States, States> U = ((lambda + States) * P).llt().matrixL();
+    double eta = std::sqrt(lambda + States);
+    Matrixd<States, States> U = eta * S;
 
     Matrixd<States, 2 * States + 1> sigmas;
     sigmas.template block<States, 1>(0, 0) = x;
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 8a434578ef..39ce6152ca 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -35,6 +35,10 @@ namespace frc {
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
  * "Stochastic control theory".
  *
+ * <p> This class implements a square-root-form unscented Kalman filter
+ * (SR-UKF). For more information about the SR-UKF, see
+ * https://www.researchgate.net/publication/3908304.
+ *
  * @tparam States The number of states.
  * @tparam Inputs The number of inputs.
  * @tparam Outputs The number of outputs.
@@ -111,24 +115,37 @@ class UnscentedKalmanFilter {
       units::second_t dt);
 
   /**
-   * Returns the error covariance matrix P.
+   * Returns the square-root error covariance matrix S.
    */
-  const StateMatrix& P() const { return m_P; }
+  const StateMatrix& S() const { return m_S; }
 
   /**
-   * Returns an element of the error covariance matrix P.
+   * Returns an element of the square-root error covariance matrix S.
    *
-   * @param i Row of P.
-   * @param j Column of P.
+   * @param i Row of S.
+   * @param j Column of S.
    */
-  double P(int i, int j) const { return m_P(i, j); }
+  double S(int i, int j) const { return m_S(i, j); }
 
   /**
-   * Set the current error covariance matrix P.
+   * Set the current square-root error covariance matrix S.
+   *
+   * @param S The square-root error covariance matrix S.
+   */
+  void SetS(const StateMatrix& S) { m_S = S; }
+
+  /**
+   * Returns the reconstructed error covariance matrix P.
+   */
+  StateMatrix P() const { return m_S.transpose() * m_S; }
+
+  /**
+   * Set the current square-root error covariance matrix S by taking the square
+   * root of P.
    *
    * @param P The error covariance matrix P.
    */
-  void SetP(const StateMatrix& P) { m_P = P; }
+  void SetP(const StateMatrix& P) { m_S = P.llt().matrixU(); }
 
   /**
    * Returns the state estimate x-hat.
@@ -162,7 +179,7 @@ class UnscentedKalmanFilter {
    */
   void Reset() {
     m_xHat.setZero();
-    m_P.setZero();
+    m_S.setZero();
     m_sigmasF.setZero();
   }
 
@@ -254,7 +271,7 @@ class UnscentedKalmanFilter {
       m_residualFuncY;
   std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
   StateVector m_xHat;
-  StateMatrix m_P;
+  StateMatrix m_S;
   StateMatrix m_contQ;
   Matrixd<Outputs, Outputs> m_contR;
   Matrixd<States, 2 * States + 1> m_sigmasF;
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
index b2ba677cf2..a6744bf61b 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
@@ -76,21 +76,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
       NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
   StateMatrix discA;
   StateMatrix discQ;
-  DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+  DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
 
-  Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
+  Matrixd<States, 2 * States + 1> sigmas =
+      m_pts.SquareRootSigmaPoints(m_xHat, m_S);
 
   for (int i = 0; i < m_pts.NumSigmas(); ++i) {
     StateVector x = sigmas.template block<States, 1>(0, i);
     m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
   }
 
-  auto ret = UnscentedTransform<States, States>(
-      m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
-  m_xHat = std::get<0>(ret);
-  m_P = std::get<1>(ret);
-
-  m_P += discQ;
+  auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
+      m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
+      discQ.template triangularView<Eigen::Lower>());
+  m_xHat = xHat;
+  m_S = S;
 }
 
 template <int States, int Inputs, int Outputs>
@@ -123,20 +124,22 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
         residualFuncX,
     std::function<StateVector(const StateVector&, const StateVector&)>
         addFuncX) {
-  const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+  Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
+  Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
 
   // Transform sigma points into measurement space
   Matrixd<Rows, 2 * States + 1> sigmasH;
-  Matrixd<States, 2 * States + 1> sigmas = m_pts.SigmaPoints(m_xHat, m_P);
+  Matrixd<States, 2 * States + 1> sigmas =
+      m_pts.SquareRootSigmaPoints(m_xHat, m_S);
   for (int i = 0; i < m_pts.NumSigmas(); ++i) {
     sigmasH.template block<Rows, 1>(0, i) =
         h(sigmas.template block<States, 1>(0, i), u);
   }
 
   // Mean and covariance of prediction passed through UT
-  auto [yHat, Py] = UnscentedTransform<Rows, States>(
-      sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
-  Py += discR;
+  auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
+      sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
+      discR.template triangularView<Eigen::Lower>());
 
   // Compute cross covariance of the state and the measurements
   Matrixd<States, Rows> Pxy;
@@ -149,19 +152,23 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
                .transpose();
   }
 
-  // K = P_{xy} P_y⁻¹
-  // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
-  // P_yᵀKᵀ = P_{xy}ᵀ
-  // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
-  // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
+  // K = (P_{xy} / S_yᵀ) / S_y
+  // K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
+  // K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
   Matrixd<States, Rows> K =
-      Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
+      Sy.transpose()
+          .fullPivHouseholderQr()
+          .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
+          .transpose();
 
   // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
   m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
 
-  // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
-  m_P -= K * Py * K.transpose();
+  Matrixd<States, Rows> U = K * Sy;
+  for (int i = 0; i < Rows; i++) {
+    Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+        m_S, U.template block<States, 1>(0, i), -1);
+  }
 }
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index 1859316909..e28f0941b7 100644
--- a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -6,15 +6,17 @@
 
 #include <tuple>
 
+#include "Eigen/QR"
 #include "frc/EigenCore.h"
 
 namespace frc {
 
 /**
  * Computes unscented transform of a set of sigma points and weights. CovDim
- * returns the mean and covariance in a tuple.
+ * returns the mean and square-root covariance of the sigma points in a tuple.
  *
- * This works in conjunction with the UnscentedKalmanFilter class.
+ * This works in conjunction with the UnscentedKalmanFilter class. For use with
+ * square-root form UKFs.
  *
  * @tparam CovDim      Dimension of covariance of sigma points after passing
  *                     through the transform.
@@ -26,12 +28,14 @@ namespace frc {
  *                     vectors using a given set of weights.
  * @param residualFunc A function that computes the residual of two state
  *                     vectors (i.e. it subtracts them.)
+ * @param squareRootR  Square-root of the noise covaraince of the sigma points.
  *
- * @return Tuple of x, mean of sigma points; P, covariance of sigma points after
- *         passing through the transform.
+ * @return Tuple of x, mean of sigma points; S, square-root covariance of
+ * sigmas.
  */
 template <int CovDim, int States>
-std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
+std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
+SquareRootUnscentedTransform(
     const Matrixd<CovDim, 2 * States + 1>& sigmas,
     const Vectord<2 * States + 1>& Wm, const Vectord<2 * States + 1>& Wc,
     std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
@@ -39,25 +43,33 @@ std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>> UnscentedTransform(
         meanFunc,
     std::function<Vectord<CovDim>(const Vectord<CovDim>&,
                                   const Vectord<CovDim>&)>
-        residualFunc) {
+        residualFunc,
+    const Matrixd<CovDim, CovDim>& squareRootR) {
   // New mean is usually just the sum of the sigmas * weight:
   //       n
   // dot = Σ W[k] Xᵢ[k]
   //      k=1
   Vectord<CovDim> x = meanFunc(sigmas, Wm);
 
-  // New covariance is the sum of the outer product of the residuals times the
-  // weights
-  Matrixd<CovDim, 2 * States + 1> y;
-  for (int i = 0; i < 2 * States + 1; ++i) {
-    // y[:, i] = sigmas[:, i] - x
-    y.template block<CovDim, 1>(0, i) =
-        residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
+  Matrixd<CovDim, States * 2 + CovDim> Sbar;
+  for (int i = 0; i < States * 2; i++) {
+    Sbar.template block<CovDim, 1>(0, i) =
+        std::sqrt(Wc[1]) *
+        residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
   }
-  Matrixd<CovDim, CovDim> P =
-      y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
+  Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
 
-  return std::make_tuple(x, P);
+  // Merwe defines the QR decomposition as Aᵀ = QR
+  Matrixd<CovDim, CovDim> S = Sbar.transpose()
+                                  .householderQr()
+                                  .matrixQR()
+                                  .template block<CovDim, CovDim>(0, 0)
+                                  .template triangularView<Eigen::Upper>();
+
+  Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
+      S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
+
+  return std::make_tuple(x, S);
 }
 
 }  // namespace frc
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index 78908e48bb..381dc891f1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -110,7 +110,7 @@ class DifferentialDrivePoseEstimatorTest {
       t += dt;
     }
 
-    assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.055, "Incorrect max error");
+    assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.1, "Incorrect max error");
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index d8756f0888..be7c28a8df 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -111,7 +111,7 @@ class MecanumDrivePoseEstimatorTest {
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.1, "Incorrect max error");
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
index 3c51d6c96a..2131c35d5d 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -16,7 +16,7 @@ class MerweScaledSigmaPointsTest {
   void testZeroMeanPoints() {
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
-        merweScaledSigmaPoints.sigmaPoints(
+        merweScaledSigmaPoints.squareRootSigmaPoints(
             VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
 
     assertTrue(
@@ -31,8 +31,8 @@ class MerweScaledSigmaPointsTest {
   void testNonzeroMeanPoints() {
     var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
     var points =
-        merweScaledSigmaPoints.sigmaPoints(
-            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
+        merweScaledSigmaPoints.squareRootSigmaPoints(
+            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
 
     assertTrue(
         points.isEqual(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index c6126c6f58..d1fba25798 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -110,7 +110,7 @@ class SwerveDrivePoseEstimatorTest {
     }
 
     assertEquals(
-        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
-    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.1, "Incorrect max error");
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
index 25def496a0..466a26c168 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -16,8 +16,8 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
-import edu.wpi.first.math.numbers.N4;
-import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.system.Discretization;
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.NumericalJacobian;
@@ -31,91 +31,111 @@ import org.junit.jupiter.api.Test;
 
 class UnscentedKalmanFilterTest {
   @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  private static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     var motors = DCMotor.getCIM(2);
 
-    var gHigh = 7.08;
-    var rb = 0.8382 / 2.0;
-    var r = 0.0746125;
-    var m = 63.503;
-    var J = 5.6;
+    // var gLow = 15.32;    // Low gear ratio
+    var gHigh = 7.08; // High gear ratio
+    var rb = 0.8382 / 2.0; // Robot radius
+    var r = 0.0746125; // Wheel radius
+    var m = 63.503; // Robot mass
+    var J = 5.6; // Robot moment of inertia
 
     var C1 =
         -Math.pow(gHigh, 2)
             * motors.KtNMPerAmp
             / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
     var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
+    var k1 = 1.0 / m + Math.pow(rb, 2) / J;
+    var k2 = 1.0 / m - Math.pow(rb, 2) / J;
 
-    var c = x.get(2, 0);
-    var s = x.get(3, 0);
-    var vl = x.get(4, 0);
-    var vr = x.get(5, 0);
-
+    var vl = x.get(3, 0);
+    var vr = x.get(4, 0);
     var Vl = u.get(0, 0);
     var Vr = u.get(1, 0);
 
-    var k1 = 1.0 / m + rb * rb / J;
-    var k2 = 1.0 / m - rb * rb / J;
-
-    var xvel = (vl + vr) / 2;
-    var w = (vr - vl) / (2.0 * rb);
-
+    var v = 0.5 * (vl + vr);
     return VecBuilder.fill(
-        xvel * c,
-        xvel * s,
-        -s * w,
-        c * w,
-        k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
-        k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+        v * Math.cos(x.get(2, 0)),
+        v * Math.sin(x.get(2, 0)),
+        (vr - vl) / (2.0 * rb),
+        k1 * (C1 * vl + C2 * Vl) + k2 * (C1 * vr + C2 * Vr),
+        k2 * (C1 * vl + C2 * Vl) + k1 * (C1 * vr + C2 * Vr));
   }
 
   @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
-  private static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
-    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
+  private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
   }
 
   @SuppressWarnings({"PMD.UnusedFormalParameter", "ParameterName"})
-  private static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+  private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
     return x.copy();
   }
 
   @Test
   @SuppressWarnings("LocalVariableName")
   void testInit() {
+    var dtSeconds = 0.005;
     assertDoesNotThrow(
         () -> {
-          UnscentedKalmanFilter<N6, N2, N4> observer =
+          UnscentedKalmanFilter<N5, N2, N3> observer =
               new UnscentedKalmanFilter<>(
-                  Nat.N6(),
-                  Nat.N4(),
+                  Nat.N5(),
+                  Nat.N3(),
                   UnscentedKalmanFilterTest::getDynamics,
                   UnscentedKalmanFilterTest::getLocalMeasurementModel,
-                  VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-                  VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
-                  0.00505);
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  AngleStatistics.angleMean(2),
+                  AngleStatistics.angleMean(0),
+                  AngleStatistics.angleResidual(2),
+                  AngleStatistics.angleResidual(0),
+                  AngleStatistics.angleAdd(2),
+                  dtSeconds);
 
           var u = VecBuilder.fill(12.0, 12.0);
-          observer.predict(u, 0.00505);
+          observer.predict(u, dtSeconds);
 
           var localY = getLocalMeasurementModel(observer.getXhat(), u);
           observer.correct(u, localY);
+
+          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var R =
+              StateSpaceUtil.makeCovarianceMatrix(
+                  Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
+          observer.correct(
+              Nat.N5(),
+              u,
+              globalY,
+              UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+              R,
+              AngleStatistics.angleMean(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleResidual(2),
+              AngleStatistics.angleAdd(2));
         });
   }
 
   @SuppressWarnings("LocalVariableName")
   @Test
   void testConvergence() {
-    double dtSeconds = 0.00505;
+    double dtSeconds = 0.005;
     double rbMeters = 0.8382 / 2.0; // Robot radius
 
-    UnscentedKalmanFilter<N6, N2, N4> observer =
+    UnscentedKalmanFilter<N5, N2, N3> observer =
         new UnscentedKalmanFilter<>(
-            Nat.N6(),
-            Nat.N4(),
+            Nat.N5(),
+            Nat.N3(),
             UnscentedKalmanFilterTest::getDynamics,
             UnscentedKalmanFilterTest::getLocalMeasurementModel,
-            VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-            VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.0001, 0.5, 0.5),
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
             dtSeconds);
 
     List<Pose2d> waypoints =
@@ -125,56 +145,52 @@ class UnscentedKalmanFilterTest {
     var trajectory =
         TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
 
-    Matrix<N6, N1> nextR;
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
     Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
 
     var B =
         NumericalJacobian.numericalJacobianU(
-            Nat.N6(),
+            Nat.N5(),
             Nat.N2(),
             UnscentedKalmanFilterTest::getDynamics,
-            new Matrix<>(Nat.N6(), Nat.N1()),
-            u);
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            new Matrix<>(Nat.N2(), Nat.N1()));
 
-    observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
-
-    var ref = trajectory.sample(0.0);
-
-    Matrix<N6, N1> r =
+    observer.setXhat(
         VecBuilder.fill(
-            ref.poseMeters.getTranslation().getX(),
-            ref.poseMeters.getTranslation().getY(),
-            ref.poseMeters.getRotation().getCos(),
-            ref.poseMeters.getRotation().getSin(),
-            ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
-            ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
-    nextR = r.copy();
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
 
     var trueXhat = observer.getXhat();
 
     double totalTime = trajectory.getTotalTimeSeconds();
     for (int i = 0; i < (totalTime / dtSeconds); i++) {
-      ref = trajectory.sample(dtSeconds * i);
+      var ref = trajectory.sample(dtSeconds * i);
       double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
       double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
 
-      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
-      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
-      nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
-      nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
-      nextR.set(4, 0, vl);
-      nextR.set(5, 0, vr);
+      var nextR =
+          VecBuilder.fill(
+              ref.poseMeters.getTranslation().getX(),
+              ref.poseMeters.getTranslation().getY(),
+              ref.poseMeters.getRotation().getRadians(),
+              vl,
+              vr);
 
-      Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
-      var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
+      Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
 
       observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
 
       var rdot = nextR.minus(r).div(dtSeconds);
       u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
 
-      r = nextR;
       observer.predict(u, dtSeconds);
+
+      r = nextR;
       trueXhat =
           NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
     }
@@ -183,25 +199,28 @@ class UnscentedKalmanFilterTest {
     observer.correct(u, localY);
 
     var globalY = getGlobalMeasurementModel(trueXhat, u);
-    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
+    var R =
+        StateSpaceUtil.makeCovarianceMatrix(
+            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
     observer.correct(
-        Nat.N6(),
+        Nat.N5(),
         u,
         globalY,
         UnscentedKalmanFilterTest::getGlobalMeasurementModel,
         R,
-        (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
-        Matrix::minus,
-        Matrix::minus,
-        Matrix::plus);
+        AngleStatistics.angleMean(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleResidual(2),
+        AngleStatistics.angleAdd(2));
 
     final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
 
-    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
-    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
-    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
-    assertEquals(0.0, observer.getXhat(3), 1.0);
-    assertEquals(0.0, observer.getXhat(4), 1.0);
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15);
+    assertEquals(
+        finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005);
+    assertEquals(0.0, observer.getXhat(3), 0.1);
+    assertEquals(0.0, observer.getXhat(4), 0.1);
   }
 
   @Test
@@ -235,95 +254,24 @@ class UnscentedKalmanFilterTest {
     assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
   }
 
+  @SuppressWarnings("LocalVariableName")
   @Test
-  void testUnscentedTransform() {
-    // From FilterPy
-    var ret =
-        UnscentedKalmanFilter.unscentedTransform(
-            Nat.N4(),
-            Nat.N4(),
-            Matrix.mat(Nat.N4(), Nat.N9())
-                .fill(
-                    -0.9,
-                    -0.822540333075852,
-                    -0.8922540333075852,
-                    -0.9,
-                    -0.9,
-                    -0.9774596669241481,
-                    -0.9077459666924148,
-                    -0.9,
-                    -0.9,
-                    1.0,
-                    1.0,
-                    1.077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    0.9225403330758519,
-                    1.0,
-                    1.0,
-                    -0.9,
-                    -0.9,
-                    -0.9,
-                    -0.822540333075852,
-                    -0.8922540333075852,
-                    -0.9,
-                    -0.9,
-                    -0.9774596669241481,
-                    -0.9077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    1.0,
-                    1.077459666924148,
-                    1.0,
-                    1.0,
-                    1.0,
-                    0.9225403330758519),
-            VecBuilder.fill(
-                -132.33333333,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667),
-            VecBuilder.fill(
-                -129.34333333,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667,
-                16.66666667),
-            (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
-            Matrix::minus);
+  void testRoundTripP() {
+    var dtSeconds = 0.005;
 
-    assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
+    var observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N2(),
+            Nat.N2(),
+            (x, u) -> x,
+            (x, u) -> x,
+            VecBuilder.fill(0.0, 0.0),
+            VecBuilder.fill(0.0, 0.0),
+            dtSeconds);
 
-    assertTrue(
-        Matrix.mat(Nat.N4(), Nat.N4())
-            .fill(
-                2.02000002e-01,
-                2.00000500e-02,
-                -2.69044710e-29,
-                -4.59511477e-29,
-                2.00000500e-02,
-                2.00001000e-01,
-                -2.98781068e-29,
-                -5.12759588e-29,
-                -2.73372625e-29,
-                -3.09882635e-29,
-                2.02000002e-01,
-                2.00000500e-02,
-                -4.67065917e-29,
-                -5.10705197e-29,
-                2.00000500e-02,
-                2.00001000e-01)
-            .isEqual(ret.getSecond(), 1E-5));
+    var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
+    observer.setP(P);
+
+    assertTrue(observer.getP().isEqual(P, 1e-9));
   }
 }
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index 4a854fd138..bc787ff669 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -93,6 +93,6 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
   }
 
   EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
-              0.2);
-  EXPECT_NEAR(0.0, maxError, 0.4);
+              0.05);
+  EXPECT_NEAR(0.0, maxError, 0.1);
 }
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index 881d4e8fbb..2a9601224f 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -84,6 +84,6 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
     t += dt;
   }
 
-  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
-  EXPECT_LT(maxError, 0.4);
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
+  EXPECT_LT(maxError, 0.1);
 }
diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index 9fd92b6677..19a734b581 100644
--- a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -10,7 +10,7 @@ namespace drake::math {
 namespace {
 TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points = sigmaPoints.SigmaPoints(
+  auto points = sigmaPoints.SquareRootSigmaPoints(
       frc::Vectord<2>{0.0, 0.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
@@ -21,8 +21,9 @@ TEST(MerweScaledSigmaPointsTest, ZeroMean) {
 
 TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points = sigmaPoints.SigmaPoints(
-      frc::Vectord<2>{1.0, 2.0}, frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 10.0}});
+  auto points = sigmaPoints.SquareRootSigmaPoints(
+      frc::Vectord<2>{1.0, 2.0},
+      frc::Matrixd<2, 2>{{1.0, 0.0}, {0.0, std::sqrt(10.0)}});
 
   EXPECT_TRUE(
       (points - frc::Matrixd<2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index ee01f6fd1a..eec4112d88 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -84,6 +84,6 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
     t += dt;
   }
 
-  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
-  EXPECT_LT(maxError, 0.4);
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
+  EXPECT_LT(maxError, 0.1);
 }
diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index b5b36d65e3..68f9c40152 100644
--- a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -23,7 +23,7 @@ namespace {
 frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
-  // constexpr double Glow = 15.32;       // Low gear ratio
+  // constexpr double Glow = 15.32;    // Low gear ratio
   constexpr double Ghigh = 7.08;       // High gear ratio
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
   constexpr auto r = 0.0746125_m;      // Wheel radius
@@ -71,6 +71,11 @@ TEST(UnscentedKalmanFilterTest, Init) {
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.01, 0.01},
+                                               frc::AngleMean<5, 5>(2),
+                                               frc::AngleMean<3, 5>(0),
+                                               frc::AngleResidual<5>(2),
+                                               frc::AngleResidual<3>(0),
+                                               frc::AngleAdd<5>(2),
                                                dt};
   frc::Vectord<2> u{12.0, 12.0};
   observer.Predict(u, dt);
@@ -93,6 +98,11 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.5, 0.5},
+                                               frc::AngleMean<5, 5>(2),
+                                               frc::AngleMean<3, 5>(0),
+                                               frc::AngleResidual<5>(2),
+                                               frc::AngleResidual<3>(0),
+                                               frc::AngleAdd<5>(2),
                                                dt};
 
   auto waypoints =
@@ -150,12 +160,28 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
   );
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());
-  ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
-              1.0);
-  ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
-              1.0);
-  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
-              1.0);
-  ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
-  ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
+  EXPECT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+              0.055);
+  EXPECT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+              0.15);
+  EXPECT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+              0.000005);
+  EXPECT_NEAR(0.0, observer.Xhat(3), 0.1);
+  EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
+}
+
+TEST(UnscentedKalmanFilterTest, RoundTripP) {
+  constexpr auto dt = 5_ms;
+
+  frc::UnscentedKalmanFilter<2, 2, 2> observer{
+      [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+      [](const frc::Vectord<2>& x, const frc::Vectord<2>& u) { return x; },
+      {0.0, 0.0},
+      {0.0, 0.0},
+      dt};
+
+  frc::Matrixd<2, 2> P({{2, 1}, {1, 2}});
+  observer.SetP(P);
+
+  ASSERT_TRUE(observer.P().isApprox(P));
 }