From d4f89c11c337e0418212bc2ddeecb9adba6bb757 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 21 Jun 2026 15:09:34 -0700
Subject: [PATCH] [wpimath] Use triangular solves to compute UKF Kalman gain
 (#9009)

---
 .../org/wpilib/math/estimator/UnscentedKalmanFilter.java | 9 ++-------
 .../include/wpi/math/estimator/UnscentedKalmanFilter.hpp | 9 ++++-----
 2 files changed, 6 insertions(+), 12 deletions(-)

diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java
index a502ddf988..350bb1a138 100644
--- a/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java
+++ b/wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java
@@ -564,19 +564,14 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
       Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
     }
 
-    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
-    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
-    // that.
+    // Compute the Kalman gain
     //
     //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
     //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
     //   K = (S_{y}ᵀ \ (S_{y} \ P_{xy}ᵀ))ᵀ
     //
     // equation (27)
-    Matrix<States, R> K =
-        Sy.transpose()
-            .solveFullPivHouseholderQr(Sy.solveFullPivHouseholderQr(Pxy.transpose()))
-            .transpose();
+    Matrix<States, R> K = Sy.transpose().solve(Sy.solve(Pxy.transpose())).transpose();
 
     // Compute the posterior state mean
     //
diff --git a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
index b8afbe7433..bb112c66d4 100644
--- a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
+++ b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
@@ -437,9 +437,7 @@ class UnscentedKalmanFilter {
               .transpose();
     }
 
-    // Compute the Kalman gain. We use Eigen's QR decomposition to solve. This
-    // is equivalent to MATLAB's \ operator, so we need to rearrange to use
-    // that.
+    // Compute the Kalman gain
     //
     //   K = (P_{xy} / S_{y}ᵀ) / S_{y}
     //   K = (S_{y} \ P_{xy})ᵀ / S_{y}
@@ -448,8 +446,9 @@ class UnscentedKalmanFilter {
     // equation (27)
     Matrixd<States, Rows> K =
         Sy.transpose()
-            .fullPivHouseholderQr()
-            .solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
+            .template triangularView<Eigen::Upper>()
+            .solve(Sy.template triangularView<Eigen::Lower>().solve(
+                Pxy.transpose()))
             .transpose();
 
     // Compute the posterior state mean