Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -372,7 +372,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
final Matrix<States, Rows> K = S.solve(C.times(m_P)).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));

6
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -230,7 +230,11 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
final Matrix<States, Outputs> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
final Matrix<States, Outputs> K = S.solve(C.times(m_P)).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));

8
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -100,9 +100,11 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K =
new Matrix<>(
S.transpose().getStorage().solve(C.times(P.transpose()).getStorage()).transpose());
//
// Drop the transposes on symmetric matrices S and P.
//
// K = (S.solve(CP))ᵀ
m_K = new Matrix<>(S.getStorage().solve(C.times(P).getStorage()).transpose());
reset();
}