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

[wpimath] Replace DiscretizeAQTaylor() with DiscretizeAQ() (#5344)

Browse Source 
Until #5339 is fixed, we have to use the slower, more accurate version.
This commit is contained in:
Tyler Veness
2023-05-18 07:13:20 -07:00
committed by GitHub
parent c976f40364
commit eed1e6e3cb
6 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -36,7 +36,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
@@ -71,7 +71,7 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
@@ -93,7 +93,7 @@ void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);

2
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -31,7 +31,7 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQTaylor<States>(plant.A(), contQ, dt, &discA, &discQ);
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);

2
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -76,7 +76,7 @@ void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, m_dt, &discA, &discQ);
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =