From f36162fddcbffbc957e77fb4889b2a19d2b6e403 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 4 Sep 2022 17:24:38 -0700 Subject: [PATCH] [wpimath] Improve Discretization internal docs (#4400) --- .../wpi/first/math/system/Discretization.java | 82 ++++++++++++++---- .../include/frc/system/Discretization.h | 84 ++++++++++++++----- .../first/math/system/DiscretizationTest.java | 24 ++++-- .../native/cpp/system/DiscretizationTest.cpp | 24 ++++-- 4 files changed, 166 insertions(+), 48 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java index ffbd99e218..48705fbb69 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java @@ -25,6 +25,7 @@ public final class Discretization { */ public static Matrix discretizeA( Matrix contA, double dtSeconds) { + // A_d = eᴬᵀ return contA.times(dtSeconds).exp(); } @@ -42,20 +43,23 @@ public final class Discretization { public static Pair, Matrix> discretizeAB( Matrix contA, Matrix contB, double dtSeconds) { - var scaledA = contA.times(dtSeconds); - var scaledB = contB.times(dtSeconds); - int states = contA.getNumRows(); int inputs = contB.getNumCols(); + + // M = [A B] + // [0 0] var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs)); - M.assignBlock(0, 0, scaledA); - M.assignBlock(0, scaledA.getNumCols(), scaledB); - var phi = M.exp(); + M.assignBlock(0, 0, contA); + M.assignBlock(0, contA.getNumCols(), contB); + + // ϕ = eᴹᵀ = [A_d B_d] + // [ 0 I ] + var phi = M.times(dtSeconds).exp(); var discA = new Matrix(new SimpleMatrix(states, states)); - var discB = new Matrix(new SimpleMatrix(states, inputs)); - discA.extractFrom(0, 0, phi); + + var discB = new Matrix(new SimpleMatrix(states, inputs)); discB.extractFrom(0, contB.getNumRows(), phi); return new Pair<>(discA, discB); @@ -79,18 +83,22 @@ public final class Discretization { // Make continuous Q symmetric if it isn't already Matrix Q = contQ.plus(contQ.transpose()).div(2.0); - // Set up the matrix M = [[-A, Q], [0, A.T]] + // M = [−A Q ] + // [ 0 Aᵀ] final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states)); M.assignBlock(0, 0, contA.times(-1.0)); M.assignBlock(0, states, Q); M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states))); M.assignBlock(states, states, contA.transpose()); + // ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d] + // [ 0 A_dᵀ ] final var phi = M.times(dtSeconds).exp(); - // Phi12 = phi[0:States, States:2*States] - // Phi22 = phi[States:2*States, States:2*States] + // ϕ₁₂ = A_d⁻¹Q_d final Matrix phi12 = phi.block(states, states, 0, states); + + // ϕ₂₂ = A_dᵀ final Matrix phi22 = phi.block(states, states, states, states); final var discA = phi22.transpose(); @@ -109,9 +117,12 @@ public final class Discretization { *

Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive), * we take advantage of the structure of the block matrix of A and Q. * - *

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. + *

    + *
  • eᴬᵀ, which is only N x N, is relatively cheap. + *
  • 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. + *
* * @param Nat representing the number of states. * @param contA Continuous system matrix. @@ -123,6 +134,41 @@ public final class Discretization { public static Pair, Matrix> discretizeAQTaylor( Matrix contA, Matrix contQ, double dtSeconds) { + // 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 Matrix Q = contQ.plus(contQ.transpose()).div(2.0); @@ -130,17 +176,18 @@ public final class Discretization { double lastCoeff = dtSeconds; // Aᵀⁿ - Matrix Atn = contA.transpose(); + Matrix ATn = contA.transpose(); + Matrix phi12 = lastTerm.times(lastCoeff); // i = 6 i.e. 5th order should be enough precision for (int i = 2; i < 6; ++i) { - lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn)); + lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(ATn)); lastCoeff *= dtSeconds / ((double) i); phi12 = phi12.plus(lastTerm.times(lastCoeff)); - Atn = Atn.times(contA.transpose()); + ATn = ATn.times(contA.transpose()); } var discA = discretizeA(contA, dtSeconds); @@ -162,6 +209,7 @@ public final class Discretization { * @return Discretized version of the provided continuous measurement noise covariance matrix. */ public static Matrix discretizeR(Matrix R, double dtSeconds) { + // R_d = 1/T R return R.div(dtSeconds); } } diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h index 8ae03f4848..957b875433 100644 --- a/wpimath/src/main/native/include/frc/system/Discretization.h +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -21,6 +21,7 @@ namespace frc { template void DiscretizeA(const Matrixd& contA, units::second_t dt, Matrixd* discA) { + // A_d = eᴬᵀ *discA = (contA * dt.value()).exp(); } @@ -40,16 +41,19 @@ void DiscretizeAB(const Matrixd& contA, const Matrixd& contB, units::second_t dt, Matrixd* discA, Matrixd* discB) { - // Matrices are blocked here to minimize matrix exponentiation calculations - Matrixd Mcont; - Mcont.setZero(); - Mcont.template block(0, 0) = contA * dt.value(); - Mcont.template block(0, States) = contB * dt.value(); + // M = [A B] + // [0 0] + Matrixd M; + M.setZero(); + M.template block(0, 0) = contA; + M.template block(0, States) = contB; - // Discretize A and B with the given timestep - Matrixd Mdisc = Mcont.exp(); - *discA = Mdisc.template block(0, 0); - *discB = Mdisc.template block(0, States); + // ϕ = eᴹᵀ = [A_d B_d] + // [ 0 I ] + Matrixd phi = (M * dt.value()).exp(); + + *discA = phi.template block(0, 0); + *discB = phi.template block(0, States); } /** @@ -70,18 +74,22 @@ void DiscretizeAQ(const Matrixd& contA, // Make continuous Q symmetric if it isn't already Matrixd 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(0, 0) = -contA; M.template block(0, States) = Q; M.template block(States, 0).setZero(); M.template block(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 phi12 = phi.block(0, States, States, States); + + // ϕ₂₂ = A_dᵀ Matrixd phi22 = phi.block(States, States, States, States); *discA = phi22.transpose(); @@ -99,10 +107,12 @@ void DiscretizeAQ(const Matrixd& 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. + *
    + *
  • eᴬᵀ, which is only N x N, is relatively cheap. + *
  • 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. + *
* * @tparam States Number of states. * @param contA Continuous system matrix. @@ -116,6 +126,41 @@ void DiscretizeAQTaylor(const Matrixd& contA, const Matrixd& contQ, units::second_t dt, Matrixd* discA, Matrixd* 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 Q = (contQ + contQ.transpose()) / 2.0; @@ -123,18 +168,18 @@ void DiscretizeAQTaylor(const Matrixd& contA, double lastCoeff = dt.value(); // Aᵀⁿ - Matrixd Atn = contA.transpose(); + Matrixd ATn = contA.transpose(); Matrixd 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(i); phi12 += lastTerm * lastCoeff; - Atn *= contA.transpose(); + ATn *= contA.transpose(); } DiscretizeA(contA, dt, discA); @@ -155,6 +200,7 @@ void DiscretizeAQTaylor(const Matrixd& contA, template Matrixd DiscretizeR(const Matrixd& R, units::second_t dt) { + // R_d = 1/T R return R / dt.value(); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java index 3dff90b9be..2182674f7d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java @@ -58,9 +58,9 @@ class DiscretizationTest { assertEquals(x1Truth, x1Discrete); } - // dt - // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ - // 0 + // T + // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 @Test void testDiscretizeSlowModelAQ() { final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); @@ -68,6 +68,9 @@ class DiscretizationTest { final double dt = 1.0; + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 final var discQIntegrated = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> @@ -87,9 +90,9 @@ class DiscretizationTest { + discQIntegrated); } - // dt - // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ - // 0 + // T + // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 @Test void testDiscretizeFastModelAQ() { final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29); @@ -97,6 +100,9 @@ class DiscretizationTest { final var dt = 0.005; + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 final var discQIntegrated = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> @@ -130,6 +136,9 @@ class DiscretizationTest { assertTrue(esCont.getEigenvalue(i).real >= 0); } + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 final var discQIntegrated = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> @@ -173,6 +182,9 @@ class DiscretizationTest { assertTrue(esCont.getEigenvalue(i).real >= 0); } + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 final var discQIntegrated = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index 9a25c58adb..d73533803a 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -50,15 +50,18 @@ TEST(DiscretizationTest, DiscretizeAB) { EXPECT_EQ(x1Truth, x1Discrete); } -// dt -// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ -// 0 +// T +// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ +// 0 TEST(DiscretizationTest, DiscretizeSlowModelAQ) { frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}}; constexpr auto dt = 1_s; + // T + // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< std::function(units::second_t, const frc::Matrixd<2, 2>&)>, @@ -79,15 +82,18 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { << discQIntegrated; } -// dt -// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ -// 0 +// T +// Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ +// 0 TEST(DiscretizationTest, DiscretizeFastModelAQ) { frc::Matrixd<2, 2> contA{{0, 1}, {0, -1406.29}}; frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}}; constexpr auto dt = 5_ms; + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< std::function(units::second_t, const frc::Matrixd<2, 2>&)>, @@ -125,6 +131,9 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { EXPECT_GE(esCont.eigenvalues()[i], 0); } + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< std::function(units::second_t, const frc::Matrixd<2, 2>&)>, @@ -168,6 +177,9 @@ TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { EXPECT_GE(esCont.eigenvalues()[i], 0); } + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< std::function(units::second_t, const frc::Matrixd<2, 2>&)>,