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& A, @@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( const Eigen::Ref& Q, const Eigen::Ref& 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& A,