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 1871803772..b2fe9c8c8e 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 @@ -108,93 +108,6 @@ public final class Discretization { return new Pair<>(discA, discQ); } - /** - * Discretizes the given continuous A and Q matrices. - * - *

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

- * - * @param Nat representing the number of states. - * @param contA Continuous system matrix. - * @param contQ Continuous process noise covariance matrix. - * @param dtSeconds Discretization timestep. - * @return a pair representing the discrete system matrix and process noise covariance matrix. - */ - 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); - - Matrix lastTerm = Q.copy(); - double lastCoeff = dtSeconds; - - // Aᵀⁿ - 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)); - lastCoeff *= dtSeconds / ((double) i); - - phi12 = phi12.plus(lastTerm.times(lastCoeff)); - - ATn = ATn.times(contA.transpose()); - } - - var discA = discretizeA(contA, dtSeconds); - Q = discA.times(phi12); - - // Make Q symmetric if it isn't already - var discQ = Q.plus(Q.transpose()).div(2.0); - - return new Pair<>(discA, discQ); - } - /** * Returns a discretized version of the provided continuous measurement noise covariance matrix. * Note that dt=0.0 divides R by zero. diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h index 16fb45a20e..5c56f9e07a 100644 --- a/wpimath/src/main/native/include/frc/system/Discretization.h +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -100,95 +100,6 @@ void DiscretizeAQ(const Matrixd& contA, *discQ = (Q + Q.transpose()) / 2.0; } -/** - * Discretizes the given continuous A and Q matrices. - * - * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ() - * (which is expensive), we take advantage of the structure of the block matrix - * of A and Q. - * - *
    - *
  • 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. - * @param contQ Continuous process noise covariance matrix. - * @param dt Discretization timestep. - * @param discA Storage for discrete system matrix. - * @param discQ Storage for discrete process noise covariance matrix. - */ -template -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; - - Matrixd lastTerm = Q; - double lastCoeff = dt.value(); - - // Aᵀⁿ - 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; - lastCoeff *= dt.value() / static_cast(i); - - phi12 += lastTerm * lastCoeff; - - ATn *= contA.transpose(); - } - - DiscretizeA(contA, dt, discA); - Q = *discA * phi12; - - // Make discrete Q symmetric if it isn't already - *discQ = (Q + Q.transpose()) / 2.0; -} - /** * Returns a discretized version of the provided continuous measurement noise * covariance matrix. 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 2182674f7d..7f62933102 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 @@ -122,98 +122,6 @@ class DiscretizationTest { + discQIntegrated); } - // Test that the Taylor series discretization produces nearly identical results. - @Test - void testDiscretizeSlowModelAQTaylor() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); - - final var dt = 1.0; - - // Continuous Q should be positive semidefinite - final var esCont = contQ.getStorage().eig(); - for (int i = 0; i < contQ.getNumRows(); ++i) { - 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) -> - contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()), - 0.0, - new Matrix<>(Nat.N2(), Nat.N2()), - dt); - - var discA = Discretization.discretizeA(contA, dt); - - var discAQPair = Discretization.discretizeAQ(contA, contQ, dt); - var discATaylor = discAQPair.getFirst(); - var discQTaylor = discAQPair.getSecond(); - - assertTrue( - discQIntegrated.minus(discQTaylor).normF() < 1e-10, - "Expected these to be nearly equal:\ndiscQTaylor:\n" - + discQTaylor - + "\ndiscQIntegrated:\n" - + discQIntegrated); - assertTrue(discA.minus(discATaylor).normF() < 1e-10); - - // Discrete Q should be positive semidefinite - final var esDisc = discQTaylor.getStorage().eig(); - for (int i = 0; i < discQTaylor.getNumRows(); ++i) { - assertTrue(esDisc.getEigenvalue(i).real >= 0); - } - } - - // Test that the Taylor series discretization produces nearly identical results. - @Test - void testDiscretizeFastModelAQTaylor() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1); - - final var dt = 0.005; - - // Continuous Q should be positive semidefinite - final var esCont = contQ.getStorage().eig(); - for (int i = 0; i < contQ.getNumRows(); ++i) { - 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) -> - contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()), - 0.0, - new Matrix<>(Nat.N2(), Nat.N2()), - dt); - - var discA = Discretization.discretizeA(contA, dt); - - var discAQPair = Discretization.discretizeAQ(contA, contQ, dt); - var discATaylor = discAQPair.getFirst(); - var discQTaylor = discAQPair.getSecond(); - - assertTrue( - discQIntegrated.minus(discQTaylor).normF() < 1e-3, - "Expected these to be nearly equal:\ndiscQTaylor:\n" - + discQTaylor - + "\ndiscQIntegrated:\n" - + discQIntegrated); - assertTrue(discA.minus(discATaylor).normF() < 1e-10); - - // Discrete Q should be positive semidefinite - final var esDisc = discQTaylor.getStorage().eig(); - for (int i = 0; i < discQTaylor.getNumRows(); ++i) { - assertTrue(esDisc.getEigenvalue(i).real >= 0); - } - } - // Test that DiscretizeR() works @Test void testDiscretizeR() { diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index c42f57c007..38ecb261f2 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -114,102 +114,6 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { << discQIntegrated; } -// Test that the Taylor series discretization produces nearly identical results. -TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { - frc::Matrixd<2, 2> contA{{0, 1}, {0, 0}}; - frc::Matrixd<2, 2> contQ{{1, 0}, {0, 1}}; - - constexpr auto dt = 1_s; - - frc::Matrixd<2, 2> discQTaylor; - frc::Matrixd<2, 2> discA; - frc::Matrixd<2, 2> discATaylor; - - // Continuous Q should be positive semidefinite - Eigen::SelfAdjointEigenSolver esCont{contQ, - Eigen::EigenvaluesOnly}; - for (int i = 0; i < contQ.rows(); ++i) { - 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>&)>, - 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::DiscretizeA<2>(contA, dt, &discA); - frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); - - EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10) - << "Expected these to be nearly equal:\ndiscQTaylor:\n" - << discQTaylor << "\ndiscQIntegrated:\n" - << discQIntegrated; - EXPECT_LT((discA - discATaylor).norm(), 1e-10); - - // Discrete Q should be positive semidefinite - Eigen::SelfAdjointEigenSolver esDisc{discQTaylor, - Eigen::EigenvaluesOnly}; - for (int i = 0; i < discQTaylor.rows(); ++i) { - EXPECT_GE(esDisc.eigenvalues()[i], 0); - } -} - -// Test that the Taylor series discretization produces nearly identical results. -TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { - frc::Matrixd<2, 2> contA{{0, 1}, {0, -1500}}; - frc::Matrixd<2, 2> contQ{{0.0025, 0}, {0, 1}}; - - constexpr auto dt = 5_ms; - - frc::Matrixd<2, 2> discQTaylor; - frc::Matrixd<2, 2> discA; - frc::Matrixd<2, 2> discATaylor; - - // Continuous Q should be positive semidefinite - Eigen::SelfAdjointEigenSolver esCont{contQ, - Eigen::EigenvaluesOnly}; - for (int i = 0; i < contQ.rows(); ++i) { - 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>&)>, - 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::DiscretizeA<2>(contA, dt, &discA); - frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); - - EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3) - << "Expected these to be nearly equal:\ndiscQTaylor:\n" - << discQTaylor << "\ndiscQIntegrated:\n" - << discQIntegrated; - EXPECT_LT((discA - discATaylor).norm(), 1e-10); - - // Discrete Q should be positive semidefinite - Eigen::SelfAdjointEigenSolver esDisc{discQTaylor, - Eigen::EigenvaluesOnly}; - for (int i = 0; i < discQTaylor.rows(); ++i) { - EXPECT_GE(esDisc.eigenvalues()[i], 0); - } -} - // Test that DiscretizeR() works TEST(DiscretizationTest, DiscretizeR) { frc::Matrixd<2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};