diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java index af2a0217af..ad07d056e7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/DARE.java +++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java @@ -58,6 +58,14 @@ public final class DARE { * *
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ₖ. * @@ -88,10 +96,10 @@ public final class DARE { * solver may hang if any of the following occur: * *
Only use this function if you're sure the preconditions are met. @@ -163,6 +171,14 @@ public final class DARE { * *
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ₖ.
*
@@ -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
-
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