Skip to content

Commit

Permalink
[wpimath] Remove discretizeAQTaylor()
Browse files Browse the repository at this point in the history 
It gives incorrect results. Any replacement should just be an
implementation detail of discretizeAQ().

Closes #5339.
  • Loading branch information
@calcmogul
calcmogul committed Aug 23, 2023
1 parent 96f7fa6 commit f7eb5f5
Show file tree
Hide file tree
Showing 4 changed files with 0 additions and 364 deletions.
87 changes: 0 additions & 87 deletions wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,93 +108,6 @@ Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
return new Pair<>(discA, discQ);
}

/**
* Discretizes the given continuous A and Q matrices.
*
* <p>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.
*
* <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>
*
* @param <States> 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 <States extends Num>
Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
Matrix<States, States> contA, Matrix<States, States> 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<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);

Matrix<States, States> lastTerm = Q.copy();
double lastCoeff = dtSeconds;

// Aᵀⁿ
Matrix<States, States> ATn = contA.transpose();

Matrix<States, States> 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.
Expand Down
89 changes: 0 additions & 89 deletions wpimath/src/main/native/include/frc/system/Discretization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,95 +100,6 @@ void DiscretizeAQ(const Matrixd<States, States>& 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.
*
* <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.
* @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 <int States>
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;

Matrixd<States, States> lastTerm = Q;
double lastCoeff = dt.value();

// Aᵀⁿ
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;
lastCoeff *= dt.value() / static_cast<double>(i);

phi12 += lastTerm * lastCoeff;

ATn *= contA.transpose();
}

DiscretizeA<States>(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.
Expand Down
92 changes: 0 additions & 92 deletions wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,98 +122,6 @@ void testDiscretizeFastModelAQ() {
+ 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<N2, N2> 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<N2, N2> 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() {
Expand Down
96 changes: 0 additions & 96 deletions wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::MatrixXd> 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<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::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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<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::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<Eigen::MatrixXd> 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}};
Expand Down

0 comments on commit f7eb5f5

Please sign in to comment.