| 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 |
| index 5d7a316f3..dc08be95e 100644 |
| --- 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 |
| @@ -9,18 +9,19 @@ |
| namespace drake { |
| namespace math { |
| |
| -/// Computes the unique stabilizing solution X to the discrete-time algebraic |
| -/// Riccati equation: |
| -/// |
| -/// AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 |
| -/// |
| -/// @throws std::exception if Q is not positive semi-definite. |
| -/// @throws std::exception if R is not positive definite. |
| -/// |
| -/// Based on the Schur Vector approach outlined in this paper: |
| -/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" |
| -/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell |
| -/// |
| +/** |
| +Computes the unique stabilizing solution X to the discrete-time algebraic |
| +Riccati equation: |
| + |
| +AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 |
| + |
| +@throws std::exception if Q is not positive semi-definite. |
| +@throws std::exception if R is not positive definite. |
| + |
| +Based on the Schur Vector approach outlined in this paper: |
| +"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" |
| +by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell |
| +*/ |
| WPILIB_DLLEXPORT |
| Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| const Eigen::Ref<const Eigen::MatrixXd>& A, |
| @@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| const Eigen::Ref<const Eigen::MatrixXd>& Q, |
| const Eigen::Ref<const Eigen::MatrixXd>& R); |
| |
| -/// Computes the unique stabilizing solution X to the discrete-time algebraic |
| -/// 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ₖ. |
| -/// |
| -/// @verbatim |
| -/// ∞ [xₖ]ᵀ[Q N][xₖ] |
| -/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT |
| -/// k=0 |
| -/// @endverbatim |
| -/// |
| -/// This is a more general form of the following. The linear-quadratic regulator |
| -/// is the feedback control law uₖ that minimizes the following cost function |
| -/// subject to xₖ₊₁ = Axₖ + Buₖ: |
| -/// |
| -/// @verbatim |
| -/// ∞ |
| -/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT |
| -/// k=0 |
| -/// @endverbatim |
| -/// |
| -/// This can be refactored as: |
| -/// |
| -/// @verbatim |
| -/// ∞ [xₖ]ᵀ[Q 0][xₖ] |
| -/// J = Σ [uₖ] [0 R][uₖ] ΔT |
| -/// k=0 |
| -/// @endverbatim |
| -/// |
| -/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. |
| -/// @throws std::runtime_error if R is not positive definite. |
| -/// |
| +/** |
| +Computes the unique stabilizing solution X to the discrete-time algebraic |
| +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ₖ. |
| + |
| +@verbatim |
| + ∞ [xₖ]ᵀ[Q N][xₖ] |
| +J = Σ [uₖ] [Nᵀ R][uₖ] ΔT |
| + k=0 |
| +@endverbatim |
| + |
| +This is a more general form of the following. The linear-quadratic regulator |
| +is the feedback control law uₖ that minimizes the following cost function |
| +subject to xₖ₊₁ = Axₖ + Buₖ: |
| + |
| +@verbatim |
| + ∞ |
| +J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT |
| + k=0 |
| +@endverbatim |
| + |
| +This can be refactored as: |
| + |
| +@verbatim |
| + ∞ [xₖ]ᵀ[Q 0][xₖ] |
| +J = Σ [uₖ] [0 R][uₖ] ΔT |
| + k=0 |
| +@endverbatim |
| + |
| +@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. |
| +@throws std::runtime_error if R is not positive definite. |
| +*/ |
| WPILIB_DLLEXPORT |
| Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( |
| const Eigen::Ref<const Eigen::MatrixXd>& A, |