Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove redundant transposes on symmetric matrices (#8131)

Browse Source 
This likely won't have a performance impact since it only affects matrix traversal order, but it does simplify the code.
This commit is contained in:
Tyler Veness
2025-07-31 21:04:55 -07:00
committed by GitHub
parent 0478176e47
commit feee88f40d
7 changed files with 35 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -128,7 +128,11 @@ Eigen::Matrix<double, States, States> DARE(
//
// V₂ᵀ = W.solve(Gₖᵀ)
// V₂ = W.solve(Gₖᵀ)ᵀ
StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
//
// Since W, Gₖ, and Hₖ are symmetric, drop the transposes on Gₖ and V₂.
//
// V₂ = W.solve(Gₖ)
StateMatrix V_2 = W_solver.solve(G_k);
// Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
// Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ

7
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -397,8 +397,11 @@ class ExtendedKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Rows> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));

7
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -235,8 +235,11 @@ class KalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
Matrixd<States, Outputs> K = S.ldlt().solve(C * m_P).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));

6
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -114,7 +114,11 @@ class SteadyStateKalmanFilter {
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.value().transpose()).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
m_K = S.ldlt().solve(C * P.value()).transpose();
} else if (P.error() == DAREError::QNotSymmetric ||
P.error() == DAREError::QNotPositiveSemidefinite) {
std::string msg =