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] Improve Discretization internal docs (#4400)

Browse Source
This commit is contained in:
Tyler Veness
2022-09-04 17:24:38 -07:00
committed by GitHub
parent 5149f7d894
commit f36162fddc
4 changed files with 166 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
wpimath/src/main/native/include/frc/system/Discretization.h
View File

@@ -21,6 +21,7 @@ namespace frc {
template <int States>
void DiscretizeA(const Matrixd<States, States>& contA, units::second_t dt,
Matrixd<States, States>* discA) {
// A_d = eᴬᵀ
*discA = (contA * dt.value()).exp();
}
@@ -40,16 +41,19 @@ void DiscretizeAB(const Matrixd<States, States>& contA,
const Matrixd<States, Inputs>& contB, units::second_t dt,
Matrixd<States, States>* discA,
Matrixd<States, Inputs>* discB) {
// Matrices are blocked here to minimize matrix exponentiation calculations
Matrixd<States + Inputs, States + Inputs> Mcont;
Mcont.setZero();
Mcont.template block<States, States>(0, 0) = contA * dt.value();
Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
// M = [A B]
// [0 0]
Matrixd<States + Inputs, States + Inputs> M;
M.setZero();
M.template block<States, States>(0, 0) = contA;
M.template block<States, Inputs>(0, States) = contB;
// Discretize A and B with the given timestep
Matrixd<States + Inputs, States + Inputs> Mdisc = Mcont.exp();
*discA = Mdisc.template block<States, States>(0, 0);
*discB = Mdisc.template block<States, Inputs>(0, States);
// ϕ = eᴹᵀ = [A_d B_d]
// [ 0 I ]
Matrixd<States + Inputs, States + Inputs> phi = (M * dt.value()).exp();
*discA = phi.template block<States, States>(0, 0);
*discB = phi.template block<States, Inputs>(0, States);
}
/**
@@ -70,18 +74,22 @@ void DiscretizeAQ(const Matrixd<States, States>& contA,
// Make continuous Q symmetric if it isn't already
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
// Set up the matrix M = [[-A, Q], [0, A.T]]
// M = [−A Q ]
// [ 0 Aᵀ]
Matrixd<2 * States, 2 * States> M;
M.template block<States, States>(0, 0) = -contA;
M.template block<States, States>(0, States) = Q;
M.template block<States, States>(States, 0).setZero();
M.template block<States, States>(States, States) = contA.transpose();
// ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d]
// [ 0 A_dᵀ ]
Matrixd<2 * States, 2 * States> phi = (M * dt.value()).exp();
// Phi12 = phi[0:States, States:2*States]
// Phi22 = phi[States:2*States, States:2*States]
// ϕ₁₂ = A_d⁻¹Q_d
Matrixd<States, States> phi12 = phi.block(0, States, States, States);
// ϕ₂₂ = A_dᵀ
Matrixd<States, States> phi22 = phi.block(States, States, States, States);
*discA = phi22.transpose();
@@ -99,10 +107,12 @@ void DiscretizeAQ(const Matrixd<States, States>& contA,
* (which is expensive), we take advantage of the structure of the block matrix
* of A and Q.
*
* 1) The exponential of A*t, which is only N x N, is relatively cheap.
* 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
* using a taylor series to several terms and still be substantially cheaper
* than taking the big exponential.
* <ul>
* <li>eᴬᵀ, which is only N x N, is relatively cheap.
* <li>The upper-right quarter of the 2N x 2N matrix, which we can approximate
* using a taylor series to several terms and still be substantially
* cheaper than taking the big exponential.
* </ul>
*
* @tparam States Number of states.
* @param contA Continuous system matrix.
@@ -116,6 +126,41 @@ void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
const Matrixd<States, States>& contQ,
units::second_t dt, Matrixd<States, States>* discA,
Matrixd<States, States>* discQ) {
// T
// Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ
// 0
//
// M = [−A Q ]
// [ 0 Aᵀ]
// ϕ = eᴹᵀ
// ϕ₁₂ = A_d⁻¹Q_d
//
// Taylor series of ϕ:
//
// ϕ = eᴹᵀ = I + MT + 1/2 M²T² + 1/6 M³T³ + …
// ϕ = eᴹᵀ = I + MT + 1/2 T²M² + 1/6 T³M³ + …
//
// Taylor series of ϕ expanded for ϕ₁₂:
//
// ϕ₁₂ = 0 + QT + 1/2 T² (−AQ + QAᵀ) + 1/6 T³ (−A lastTerm + Q Aᵀ²) + …
//
// ```
// lastTerm = Q
// lastCoeff = T
// ATn = Aᵀ
// ϕ₁₂ = lastTerm lastCoeff = QT
//
// for i in range(2, 6):
// // i = 2
// lastTerm = −A lastTerm + Q ATn = −AQ + QAᵀ
// lastCoeff *= T/i → lastCoeff *= T/2 = 1/2 T²
// ATn *= Aᵀ = Aᵀ²
//
// // i = 3
// lastTerm = −A lastTerm + Q ATn = −A (−AQ + QAᵀ) + QAᵀ² = …
// …
// ```
// Make continuous Q symmetric if it isn't already
Matrixd<States, States> Q = (contQ + contQ.transpose()) / 2.0;
@@ -123,18 +168,18 @@ void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
double lastCoeff = dt.value();
// Aᵀⁿ
Matrixd<States, States> Atn = contA.transpose();
Matrixd<States, States> ATn = contA.transpose();
Matrixd<States, States> phi12 = lastTerm * lastCoeff;
// i = 6 i.e. 5th order should be enough precision
for (int i = 2; i < 6; ++i) {
lastTerm = -contA * lastTerm + Q * Atn;
lastTerm = -contA * lastTerm + Q * ATn;
lastCoeff *= dt.value() / static_cast<double>(i);
phi12 += lastTerm * lastCoeff;
Atn *= contA.transpose();
ATn *= contA.transpose();
}
DiscretizeA<States>(contA, dt, discA);
@@ -155,6 +200,7 @@ void DiscretizeAQTaylor(const Matrixd<States, States>& contA,
template <int Outputs>
Matrixd<Outputs, Outputs> DiscretizeR(const Matrixd<Outputs, Outputs>& R,
units::second_t dt) {
// R_d = 1/T R
return R / dt.value();
}