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

[wpimath] Add RKF45 integration (#3047)

Browse Source 
This is more stable than Runge-Kutta for systems with large elements in their A or B matrices.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-06 21:40:25 -08:00
committed by GitHub
parent 278e0f126e
commit 85a0bd43c2
25 changed files with 560 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,8 +12,8 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "units/time.h"
namespace frc {
@@ -149,7 +149,7 @@ class ExtendedKalmanFilter {
Eigen::Matrix<double, States, States> discQ;
DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RungeKutta(m_f, m_xHat, u, dt);
m_xHat = RK4(m_f, m_xHat, u, dt);
m_P = discA * m_P * discA.transpose() + discQ;
}

4
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -13,8 +13,8 @@
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/RungeKutta.h"
#include "units/time.h"
namespace frc {
@@ -216,7 +216,7 @@ class UnscentedKalmanFilter {
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
Eigen::Matrix<double, States, 1> x =
sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto ret = UnscentedTransform<States, States>(