Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove discretizeAQTaylor() (#5562)

Browse Source 
It gives incorrect results. Any replacement should just be an
implementation detail of discretizeAQ().

Closes #5339.
This commit is contained in:
Tyler Veness
2023-08-23 10:47:32 -07:00
committed by GitHub
parent 7c20fa1b18
commit 8f3d6a1d4b
4 changed files with 0 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View File

@@ -108,93 +108,6 @@ public final class Discretization {
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.

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

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

92
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View File

@@ -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<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() {

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

@@ -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}};