Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use triangular solves to compute UKF Kalman gain (#9009)

Browse Source
This commit is contained in:
Tyler Veness
2026-06-21 15:09:34 -07:00
committed by GitHub
parent 7298d9eafb
commit d4f89c11c3
2 changed files with 6 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java
View File

@@ -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
//

9
wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
View File

@@ -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