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

[wpimath] Add time-varying RKDP (#7362)

Browse Source 
This makes the ground truth for the Taylor series AQ discretization more
accurate.
This commit is contained in:
Tyler Veness
2024-11-07 23:46:52 -08:00
committed by GitHub
parent 01f85abcfe
commit 661bae568f
10 changed files with 369 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -10,7 +10,6 @@
#include "frc/EigenCore.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/RungeKuttaTimeVarying.h"
// Check that for a simple second-order system that we can easily analyze
// analytically,
@@ -62,15 +61,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
// T
// Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discQIntegrated =
frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;
@@ -94,15 +93,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) {
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discQIntegrated =
frc::RKDP<std::function<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,
frc::Matrixd<2, 2>>(
[&](units::second_t t, const frc::Matrixd<2, 2>&) {
return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ *
(contA.transpose() * t.value()).exp());
},
0_s, frc::Matrixd<2, 2>::Zero(), dt);
frc::Matrixd<2, 2> discA;
frc::Matrixd<2, 2> discQ;