Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Clean up notation in DARE precondition docs (#5595)

Browse Source
This commit is contained in:
Tyler Veness
2023-08-31 11:56:41 -07:00
committed by GitHub
parent b2dd59450b
commit 10d4f5b5df
2 changed files with 45 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -182,6 +182,14 @@ Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This is equivalent to solving the original DARE:
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
where A₂ and Q₂ are a change of variables:
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
@@ -212,10 +220,10 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
This internal function skips expensive precondition checks for increased
performance. The solver may hang if any of the following occur:
<ul>
<li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite</li>
<li>Q₂ isn't symmetric positive semidefinite</li>
<li>R isn't symmetric positive definite</li>
<li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable</li>
<li>The (A, C) pair where Q = CᵀC isn't detectable</li>
<li>The (A₂, B) pair isn't stabilizable</li>
<li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
</ul>
Only use this function if you're sure the preconditions are met.
@@ -300,6 +308,14 @@ Riccati equation:
AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
This is equivalent to solving the original DARE:
A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
where A₂ and Q₂ are a change of variables:
A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
This overload of the DARE is useful for finding the control law uₖ that
minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
@@ -334,11 +350,11 @@ J = Σ [uₖ] [0 R][uₖ] ΔT
@param Q The state cost matrix.
@param R The input cost matrix.
@param N The state-input cross cost matrix.
@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive
semidefinite.
@throws std::invalid_argument if Q₂ isn't symmetric positive semidefinite.
@throws std::invalid_argument if R isn't symmetric positive definite.
@throws std::invalid_argument if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable.
@throws std::invalid_argument if the (A₂, B) pair isn't stabilizable.
@throws std::invalid_argument if the (A₂, C) pair where Q₂ = CᵀC isn't
detectable.
*/
template <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(