Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

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

@@ -25,6 +25,7 @@ public final class Discretization {
*/
public static <States extends Num> Matrix<States, States> discretizeA(
Matrix<States, States> contA, double dtSeconds) {
// A_d = eᴬᵀ
return contA.times(dtSeconds).exp();
}
@@ -42,20 +43,23 @@ public final class Discretization {
public static <States extends Num, Inputs extends Num>
Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
Matrix<States, States> contA, Matrix<States, Inputs> 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<States, States>(new SimpleMatrix(states, states));
var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
discA.extractFrom(0, 0, phi);
var discB = new Matrix<States, Inputs>(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<States, States> 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<States, States> phi12 = phi.block(states, states, 0, states);
// ϕ₂₂ = A_dᵀ
final Matrix<States, States> phi22 = phi.block(states, states, states, states);
final var discA = phi22.transpose();
@@ -109,9 +117,12 @@ public final class Discretization {
* <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.
*
* <p>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>
*
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
@@ -123,6 +134,41 @@ public final class Discretization {
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);
@@ -130,17 +176,18 @@ public final class Discretization {
double lastCoeff = dtSeconds;
// Aᵀⁿ
Matrix<States, States> Atn = contA.transpose();
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));
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 <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
// R_d = 1/T R
return R.div(dtSeconds);
}
}

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();
}

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

@@ -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<N2, N2> 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<N2, N2> 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<N2, N2> 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<N2, N2> x) ->

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

@@ -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<frc::Matrixd<2, 2>(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<frc::Matrixd<2, 2>(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<frc::Matrixd<2, 2>(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<frc::Matrixd<2, 2>(units::second_t,
const frc::Matrixd<2, 2>&)>,