Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 1 | diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h |
| 2 | index 5d7a316f3..dc08be95e 100644 |
| 3 | --- a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h |
| 4 | +++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h |
| 5 | @@ -9,18 +9,19 @@ |
| 6 | namespace drake { |
| 7 | namespace math { |
| 8 | |
| 9 | -/// Computes the unique stabilizing solution X to the discrete-time algebraic |
| 10 | -/// Riccati equation: |
| 11 | -/// |
| 12 | -/// AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 |
| 13 | -/// |
| 14 | -/// @throws std::exception if Q is not positive semi-definite. |
| 15 | -/// @throws std::exception if R is not positive definite. |
| 16 | -/// |
| 17 | -/// Based on the Schur Vector approach outlined in this paper: |
| 18 | -/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" |
| 19 | -/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell |
| 20 | -/// |
| 21 | +/** |
| 22 | +Computes the unique stabilizing solution X to the discrete-time algebraic |
| 23 | +Riccati equation: |
| 24 | + |
| 25 | +AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 |
| 26 | + |
| 27 | +@throws std::exception if Q is not positive semi-definite. |
| 28 | +@throws std::exception if R is not positive definite. |
| 29 | + |
| 30 | +Based on the Schur Vector approach outlined in this paper: |
| 31 | +"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" |
| 32 | +by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell |
| 33 | +*/ |
| 34 | WPILIB_DLLEXPORT |
| 35 | Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| 36 | const Eigen::Ref<const Eigen::MatrixXd>& A, |
| 37 | @@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| 38 | const Eigen::Ref<const Eigen::MatrixXd>& Q, |
| 39 | const Eigen::Ref<const Eigen::MatrixXd>& R); |
| 40 | |
| 41 | -/// Computes the unique stabilizing solution X to the discrete-time algebraic |
| 42 | -/// Riccati equation: |
| 43 | -/// |
| 44 | -/// AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 |
| 45 | -/// |
| 46 | -/// This is equivalent to solving the original DARE: |
| 47 | -/// |
| 48 | -/// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 |
| 49 | -/// |
| 50 | -/// where A₂ and Q₂ are a change of variables: |
| 51 | -/// |
| 52 | -/// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ |
| 53 | -/// |
| 54 | -/// This overload of the DARE is useful for finding the control law uₖ that |
| 55 | -/// minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. |
| 56 | -/// |
| 57 | -/// @verbatim |
| 58 | -/// ∞ [xₖ]ᵀ[Q N][xₖ] |
| 59 | -/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT |
| 60 | -/// k=0 |
| 61 | -/// @endverbatim |
| 62 | -/// |
| 63 | -/// This is a more general form of the following. The linear-quadratic regulator |
| 64 | -/// is the feedback control law uₖ that minimizes the following cost function |
| 65 | -/// subject to xₖ₊₁ = Axₖ + Buₖ: |
| 66 | -/// |
| 67 | -/// @verbatim |
| 68 | -/// ∞ |
| 69 | -/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT |
| 70 | -/// k=0 |
| 71 | -/// @endverbatim |
| 72 | -/// |
| 73 | -/// This can be refactored as: |
| 74 | -/// |
| 75 | -/// @verbatim |
| 76 | -/// ∞ [xₖ]ᵀ[Q 0][xₖ] |
| 77 | -/// J = Σ [uₖ] [0 R][uₖ] ΔT |
| 78 | -/// k=0 |
| 79 | -/// @endverbatim |
| 80 | -/// |
| 81 | -/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. |
| 82 | -/// @throws std::runtime_error if R is not positive definite. |
| 83 | -/// |
| 84 | +/** |
| 85 | +Computes the unique stabilizing solution X to the discrete-time algebraic |
| 86 | +Riccati equation: |
| 87 | + |
| 88 | +AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 |
| 89 | + |
| 90 | +This is equivalent to solving the original DARE: |
| 91 | + |
| 92 | +A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 |
| 93 | + |
| 94 | +where A₂ and Q₂ are a change of variables: |
| 95 | + |
| 96 | +A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ |
| 97 | + |
| 98 | +This overload of the DARE is useful for finding the control law uₖ that |
| 99 | +minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. |
| 100 | + |
| 101 | +@verbatim |
| 102 | + ∞ [xₖ]ᵀ[Q N][xₖ] |
| 103 | +J = Σ [uₖ] [Nᵀ R][uₖ] ΔT |
| 104 | + k=0 |
| 105 | +@endverbatim |
| 106 | + |
| 107 | +This is a more general form of the following. The linear-quadratic regulator |
| 108 | +is the feedback control law uₖ that minimizes the following cost function |
| 109 | +subject to xₖ₊₁ = Axₖ + Buₖ: |
| 110 | + |
| 111 | +@verbatim |
| 112 | + ∞ |
| 113 | +J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT |
| 114 | + k=0 |
| 115 | +@endverbatim |
| 116 | + |
| 117 | +This can be refactored as: |
| 118 | + |
| 119 | +@verbatim |
| 120 | + ∞ [xₖ]ᵀ[Q 0][xₖ] |
| 121 | +J = Σ [uₖ] [0 R][uₖ] ΔT |
| 122 | + k=0 |
| 123 | +@endverbatim |
| 124 | + |
| 125 | +@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. |
| 126 | +@throws std::runtime_error if R is not positive definite. |
| 127 | +*/ |
| 128 | WPILIB_DLLEXPORT |
| 129 | Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| 130 | const Eigen::Ref<const Eigen::MatrixXd>& A, |