blob: 8a48333b31a61f8081d9ef307edfe62c1c3c5f00 [file] [log] [blame]
Austin Schuh812d0d12021-11-04 20:16:48 -07001diff --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
2index 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,