From 10d4f5b5dfbcb8f40009b1dd9250be8a49fcbc17 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 31 Aug 2023 11:56:41 -0700
Subject: [PATCH] [wpimath] Clean up notation in DARE precondition docs (#5595)

---
 .../main/java/edu/wpi/first/math/DARE.java    | 28 +++++++++++++----
 wpimath/src/main/native/include/frc/DARE.h    | 30 ++++++++++++++-----
 2 files changed, 45 insertions(+), 13 deletions(-)

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 {
    *
    * <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,
diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h
index 4ac18a5773..512941982a 100644
--- a/wpimath/src/main/native/include/frc/DARE.h
+++ b/wpimath/src/main/native/include/frc/DARE.h
@@ -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(