WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
DARE.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <string_view>
8
9#include <Eigen/Cholesky>
10#include <Eigen/Core>
11#include <Eigen/LU>
12#include <wpi/expected>
13
14#include "frc/StateSpaceUtil.h"
15
16namespace frc {
17
18/**
19 * Errors the DARE solver can encounter.
20 */
21enum class DAREError {
22 /// Q was not symmetric.
24 /// Q was not positive semidefinite.
26 /// R was not symmetric.
28 /// R was not positive definite.
30 /// (A, B) pair was not stabilizable.
32 /// (A, C) pair where Q = CᵀC was not detectable.
34};
35
36/**
37 * Converts the given DAREError enum to a string.
38 */
39constexpr std::string_view to_string(const DAREError& error) {
40 switch (error) {
42 return "Q was not symmetric.";
44 return "Q was not positive semidefinite.";
46 return "R was not symmetric.";
48 return "R was not positive definite.";
50 return "(A, B) pair was not stabilizable.";
52 return "(A, C) pair where Q = CᵀC was not detectable.";
53 }
54
55 return "";
56}
57
58namespace detail {
59
60/**
61 * Computes the unique stabilizing solution X to the discrete-time algebraic
62 * Riccati equation:
63 *
64 * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
65 *
66 * This internal function skips expensive precondition checks for increased
67 * performance. The solver may hang if any of the following occur:
68 * <ul>
69 * <li>Q isn't symmetric positive semidefinite</li>
70 * <li>R isn't symmetric positive definite</li>
71 * <li>The (A, B) pair isn't stabilizable</li>
72 * <li>The (A, C) pair where Q = CᵀC isn't detectable</li>
73 * </ul>
74 * Only use this function if you're sure the preconditions are met.
75 *
76 * @tparam States Number of states.
77 * @tparam Inputs Number of inputs.
78 * @param A The system matrix.
79 * @param B The input matrix.
80 * @param Q The state cost matrix.
81 * @param R_llt The LLT decomposition of the input cost matrix.
82 * @return Solution to the DARE.
83 */
84template <int States, int Inputs>
85Eigen::Matrix<double, States, States> DARE(
86 const Eigen::Matrix<double, States, States>& A,
87 const Eigen::Matrix<double, States, Inputs>& B,
88 const Eigen::Matrix<double, States, States>& Q,
89 const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt) {
90 using StateMatrix = Eigen::Matrix<double, States, States>;
91
92 // Implements SDA algorithm on p. 5 of [1] (initial A, G, H are from (4)).
93 //
94 // [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving
95 // Algorithms for Periodic Discrete-Time Algebraic Riccati Equations",
96 // International Journal of Control, 77:8, 767-788, 2004.
97 // DOI: 10.1080/00207170410001714988
98
99 // A₀ = A
100 // G₀ = BR⁻¹Bᵀ
101 // H₀ = Q
102 StateMatrix A_k = A;
103 StateMatrix G_k = B * R_llt.solve(B.transpose());
104 StateMatrix H_k;
105 StateMatrix H_k1 = Q;
106
107 do {
108 H_k = H_k1;
109
110 // W = I + GₖHₖ
111 StateMatrix W = StateMatrix::Identity(H_k.rows(), H_k.cols()) + G_k * H_k;
112
113 auto W_solver = W.lu();
114
115 // Solve WV₁ = Aₖ for V₁
116 StateMatrix V_1 = W_solver.solve(A_k);
117
118 // Solve V₂Wᵀ = Gₖ for V₂
119 //
120 // We want to put V₂Wᵀ = Gₖ into Ax = b form so we can solve it more
121 // efficiently.
122 //
123 // V₂Wᵀ = Gₖ
124 // (V₂Wᵀ)ᵀ = Gₖᵀ
125 // WV₂ᵀ = Gₖᵀ
126 //
127 // The solution of Ax = b can be found via x = A.solve(b).
128 //
129 // V₂ᵀ = W.solve(Gₖᵀ)
130 // V₂ = W.solve(Gₖᵀ)ᵀ
131 StateMatrix V_2 = W_solver.solve(G_k.transpose()).transpose();
132
133 // Gₖ₊₁ = Gₖ + AₖV₂Aₖᵀ
134 // Hₖ₊₁ = Hₖ + V₁ᵀHₖAₖ
135 // Aₖ₊₁ = AₖV₁
136 G_k += A_k * V_2 * A_k.transpose();
137 H_k1 = H_k + V_1.transpose() * H_k * A_k;
138 A_k *= V_1;
139
140 // while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁|
141 } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm());
142
143 return H_k1;
144}
145
146} // namespace detail
147
148/**
149 * Computes the unique stabilizing solution X to the discrete-time algebraic
150 * Riccati equation:
151 *
152 * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
153 *
154 * @tparam States Number of states.
155 * @tparam Inputs Number of inputs.
156 * @param A The system matrix.
157 * @param B The input matrix.
158 * @param Q The state cost matrix.
159 * @param R The input cost matrix.
160 * @param checkPreconditions Whether to check preconditions (30% less time if
161 * user is sure precondtions are already met).
162 * @return Solution to the DARE on success, or DAREError on failure.
163 */
164template <int States, int Inputs>
166 const Eigen::Matrix<double, States, States>& A,
167 const Eigen::Matrix<double, States, Inputs>& B,
168 const Eigen::Matrix<double, States, States>& Q,
169 const Eigen::Matrix<double, Inputs, Inputs>& R,
170 bool checkPreconditions = true) {
171 if (checkPreconditions) {
172 // Require R be symmetric
173 if ((R - R.transpose()).norm() > 1e-10) {
175 }
176 }
177
178 // Require R be positive definite
179 auto R_llt = R.llt();
180 if (R_llt.info() != Eigen::Success) {
182 }
183
184 if (checkPreconditions) {
185 // Require Q be symmetric
186 if ((Q - Q.transpose()).norm() > 1e-10) {
188 }
189
190 // Require Q be positive semidefinite
191 //
192 // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
193 // positive, negative, and zero diagonal entries in D equals the number of
194 // positive, negative, and zero eigenvalues respectively in Q (see
195 // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
196 //
197 // Therefore, D having no negative diagonal entries is sufficient to prove Q
198 // is positive semidefinite.
199 auto Q_ldlt = Q.ldlt();
200 if (Q_ldlt.info() != Eigen::Success ||
201 (Q_ldlt.vectorD().array() < 0.0).any()) {
203 }
204
205 // Require (A, B) pair be stabilizable
208 }
209
210 // Require (A, C) pair be detectable where Q = CᵀC
211 //
212 // Q = CᵀC = PᵀLDLᵀP
213 // C = √(D)LᵀP
214 Eigen::Matrix<double, States, States> C =
215 Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
216 Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
217 Q_ldlt.transpositionsP();
218
219 if (!IsDetectable<States, States>(A, C)) {
221 }
222 }
223
224 return detail::DARE<States, Inputs>(A, B, Q, R_llt);
225}
226
227/**
228Computes the unique stabilizing solution X to the discrete-time algebraic
229Riccati equation:
230
231 AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
232
233This is equivalent to solving the original DARE:
234
235 A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
236
237where A₂ and Q₂ are a change of variables:
238
239 A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
240
241This overload of the DARE is useful for finding the control law uₖ that
242minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
243
244@verbatim
245 ∞ [xₖ]ᵀ[Q N][xₖ]
246J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
247 k=0
248@endverbatim
249
250This is a more general form of the following. The linear-quadratic regulator
251is the feedback control law uₖ that minimizes the following cost function
252subject to xₖ₊₁ = Axₖ + Buₖ:
253
254@verbatim
255
256J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
257 k=0
258@endverbatim
259
260This can be refactored as:
261
262@verbatim
263 ∞ [xₖ]ᵀ[Q 0][xₖ]
264J = Σ [uₖ] [0 R][uₖ] ΔT
265 k=0
266@endverbatim
267
268@tparam States Number of states.
269@tparam Inputs Number of inputs.
270@param A The system matrix.
271@param B The input matrix.
272@param Q The state cost matrix.
273@param R The input cost matrix.
274@param N The state-input cross cost matrix.
275@param checkPreconditions Whether to check preconditions (30% less time if user
276 is sure precondtions are already met).
277@return Solution to the DARE on success, or DAREError on failure.
278*/
279template <int States, int Inputs>
281 const Eigen::Matrix<double, States, States>& A,
282 const Eigen::Matrix<double, States, Inputs>& B,
283 const Eigen::Matrix<double, States, States>& Q,
284 const Eigen::Matrix<double, Inputs, Inputs>& R,
285 const Eigen::Matrix<double, States, Inputs>& N,
286 bool checkPreconditions = true) {
287 if (checkPreconditions) {
288 // Require R be symmetric
289 if ((R - R.transpose()).norm() > 1e-10) {
291 }
292 }
293
294 // Require R be positive definite
295 auto R_llt = R.llt();
296 if (R_llt.info() != Eigen::Success) {
298 }
299
300 // This is a change of variables to make the DARE that includes Q, R, and N
301 // cost matrices fit the form of the DARE that includes only Q and R cost
302 // matrices.
303 //
304 // This is equivalent to solving the original DARE:
305 //
306 // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
307 //
308 // where A₂ and Q₂ are a change of variables:
309 //
310 // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
311 Eigen::Matrix<double, States, States> A_2 =
312 A - B * R_llt.solve(N.transpose());
313 Eigen::Matrix<double, States, States> Q_2 =
314 Q - N * R_llt.solve(N.transpose());
315
316 if (checkPreconditions) {
317 // Require Q be symmetric
318 if ((Q_2 - Q_2.transpose()).norm() > 1e-10) {
320 }
321
322 // Require Q be positive semidefinite
323 //
324 // If Q is a symmetric matrix with a decomposition LDLᵀ, the number of
325 // positive, negative, and zero diagonal entries in D equals the number of
326 // positive, negative, and zero eigenvalues respectively in Q (see
327 // https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
328 //
329 // Therefore, D having no negative diagonal entries is sufficient to prove Q
330 // is positive semidefinite.
331 auto Q_ldlt = Q_2.ldlt();
332 if (Q_ldlt.info() != Eigen::Success ||
333 (Q_ldlt.vectorD().array() < 0.0).any()) {
335 }
336
337 // Require (A, B) pair be stabilizable
338 if (!IsStabilizable<States, Inputs>(A_2, B)) {
340 }
341
342 // Require (A, C) pair be detectable where Q = CᵀC
343 //
344 // Q = CᵀC = PᵀLDLᵀP
345 // C = √(D)LᵀP
346 Eigen::Matrix<double, States, States> C =
347 Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
348 Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()} *
349 Q_ldlt.transpositionsP();
350
351 if (!IsDetectable<States, States>(A_2, C)) {
353 }
354 }
355
356 return detail::DARE<States, Inputs>(A_2, B, Q_2, R_llt);
357}
358
359} // namespace frc
An expected<T, E> object is an object that contains the storage for another object and manages the li...
Definition expected:1250
Definition expected:142
detail namespace with internal helper functions
Definition input_adapters.h:32
Eigen::Matrix< double, States, States > DARE(const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::LLT< Eigen::Matrix< double, Inputs, Inputs > > &R_llt)
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
Definition DARE.h:85
Definition CAN.h:11
bool IsStabilizable(const Matrixd< States, States > &A, const Matrixd< States, Inputs > &B)
Returns true if (A, B) is a stabilizable pair.
Definition StateSpaceUtil.h:250
wpi::expected< Eigen::Matrix< double, States, States >, DAREError > DARE(const Eigen::Matrix< double, States, States > &A, const Eigen::Matrix< double, States, Inputs > &B, const Eigen::Matrix< double, States, States > &Q, const Eigen::Matrix< double, Inputs, Inputs > &R, bool checkPreconditions=true)
Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation:
Definition DARE.h:165
constexpr std::string_view to_string(const DAREError &error)
Converts the given DAREError enum to a string.
Definition DARE.h:39
DAREError
Errors the DARE solver can encounter.
Definition DARE.h:21
@ QNotPositiveSemidefinite
Q was not positive semidefinite.
@ RNotSymmetric
R was not symmetric.
@ QNotSymmetric
Q was not symmetric.
@ ACNotDetectable
(A, C) pair where Q = CᵀC was not detectable.
@ RNotPositiveDefinite
R was not positive definite.
@ ABNotStabilizable
(A, B) pair was not stabilizable.
bool IsDetectable(const Matrixd< States, States > &A, const Matrixd< Outputs, States > &C)
Returns true if (A, C) is a detectable pair.
Definition StateSpaceUtil.h:309