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

28
wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -58,6 +58,14 @@ public final class DARE {
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This is equivalent to solving the original DARE:
*
* <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
*
* <p>where A₂ and Q₂ are a change of variables:
*
* <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
@@ -88,10 +96,10 @@ public final class DARE {
* solver may hang if any of the following occur:
*
* <ul>
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
* <li>Q₂ isn't symmetric positive semidefinite
* <li>R isn't symmetric positive definite
* <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
* <li>The (A, C) pair where Q = CᵀC isn't detectable
* <li>The (A₂, B) pair isn't stabilizable
* <li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable
* </ul>
*
* <p>Only use this function if you're sure the preconditions are met.
@@ -163,6 +171,14 @@ public final class DARE {
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This is equivalent to solving the original DARE:
*
* <p>A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
*
* <p>where A₂ and Q₂ are a change of variables:
*
* <p>A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
@@ -197,10 +213,10 @@ public final class DARE {
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if Q₂ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
* @throws IllegalArgumentException if the (A₂, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A₂, C) pair where Q₂ = CᵀC isn't detectable.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
Matrix<States, States> A,

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(