-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathlqg.hpp
More file actions
382 lines (334 loc) · 10.3 KB
/
lqg.hpp
File metadata and controls
382 lines (334 loc) · 10.3 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
/**
* @file lqg.hpp
* @brief Linear Quadratic Gaussian (LQG) Controller
*
* LQG combines:
* - LQR (Linear Quadratic Regulator) for optimal state feedback
* - Kalman Filter for optimal state estimation
*
* This creates a separation principle: design observer and controller
* independently.
*
* @author CppPlot Control Systems Library
* @date January 2026
*
* References:
* [1] Anderson & Moore - "Optimal Control: Linear Quadratic Methods"
* [2] Doyle (1978) - "Guaranteed Margins for LQG Regulators"
*
* @note LQG does NOT guarantee robustness margins!
* The famous Doyle paper showed LQG can have arbitrarily small margins.
* For robust control, consider LQG/LTR or H∞ methods.
*/
#ifndef CPPPLOT_CONTROL_LQG_HPP
#define CPPPLOT_CONTROL_LQG_HPP
#include "controller_design.hpp"
#include "kalman.hpp"
#include "state_space.hpp"
namespace cppplot {
namespace control {
/**
* @brief LQG Controller result structure
*/
struct LQGResult {
Matrix K; // State feedback gains (1×n row matrix)
Matrix L; // Kalman filter gains
Matrix P; // Riccati solution (controller)
Matrix S; // Riccati solution (estimator)
// Closed-loop system matrices
Matrix Acl; // Closed-loop A matrix
StateSpace closedLoop; // Full closed-loop system
};
/**
* @brief Design LQG controller
*
* The LQG problem:
*
* Plant:
* ẋ = Ax + Bu + Gw (w = process noise, w ~ N(0, W))
* y = Cx + v (v = measurement noise, v ~ N(0, V))
*
* Cost:
* J = E[ ∫(x'Qx + u'Ru)dt ]
*
* Solution (Separation Principle):
* 1. Design LQR: K = R^(-1)B'P where P solves ARE
* 2. Design Kalman: L = SC'V^(-1) where S solves dual ARE
* 3. Controller: u = -K*x̂ where x̂ is Kalman estimate
*
* @param A System matrix (n x n)
* @param B Input matrix (n x m)
* @param C Output matrix (p x n)
* @param Q State weighting (n x n)
* @param R Input weighting (m x m)
* @param W Process noise covariance (n x n) or intensity matrix G*Qn*G'
* @param V Measurement noise covariance (p x p)
* @return LQG controller gains and closed-loop system
*
* @example
* ```cpp
* // Double integrator with position measurement
* Matrix A = {{0, 1}, {0, 0}};
* Matrix B = {{0}, {1}};
* Matrix C = {{1, 0}};
*
* // LQR weights
* Matrix Q = Matrix::eye(2);
* Matrix R = Matrix::eye(1) * 0.1;
*
* // Noise covariances
* Matrix W = Matrix::eye(2) * 0.01; // Process noise
* Matrix V = Matrix::eye(1) * 0.1; // Measurement noise
*
* auto lqg = designLQG(A, B, C, Q, R, W, V);
* std::cout << "LQR gains: K = " << lqg.K << std::endl;
* std::cout << "Kalman gains: L = " << lqg.L << std::endl;
* ```
*/
inline LQGResult designLQG(const Matrix &A, const Matrix &B, const Matrix &C,
const Matrix &Q, const Matrix &R, const Matrix &W,
const Matrix &V) {
LQGResult result;
size_t n = A.rows;
size_t m = B.cols;
size_t p = C.rows;
// Step 1: Design LQR controller
// Solve: A'P + PA - PBR^(-1)B'P + Q = 0
result.P = care(A, B, Q, R);
result.K = lqr(A, B, Q, R);
// Step 2: Design Kalman filter (dual problem)
// Solve: AS + SA' - SC'V^(-1)CS + W = 0
// This is equivalent to: A'S + SA - SC'V^(-1)CS + W = 0 for A' -> A
// Actually the dual Riccati: AS + SA' - SC'V^(-1)CS + W = 0
// We can solve it using care with transposed system:
// care(A', C', W, V) gives the solution
Matrix AT = A.T();
Matrix CT = C.T();
result.S = care(AT, CT, W, V);
// Kalman gain: L = S*C'*V^(-1)
Matrix V_inv = V.inv();
result.L = result.S * CT * V_inv;
// Step 3: Form closed-loop system
// State: [x; x̂] where x̂ = estimated state
//
// [ẋ ] [A -BK ] [x ] [I] w [0] v
// [x̂̇] = [LC A-BK-LC] [x̂] + [0] + [L]
//
// Or in terms of error e = x - x̂:
// [ẋ] [A -BK ] [x] [I] w [0] v
// [ė] = [0 A-LC] [e] + [I] + [-L]
//
// Closed-loop eigenvalues: eig(A-BK) ∪ eig(A-LC)
// Build K matrix for state feedback (result.K is already a 1×n Matrix)
Matrix BK = B * result.K;
// Closed-loop A matrix (2n x 2n)
result.Acl = Matrix::zeros(2 * n, 2 * n);
// Top-left: A
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
result.Acl(i, j) = A(i, j);
}
}
// Top-right: -B*K
// BK already computed above
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
result.Acl(i, n + j) = -BK(i, j);
}
}
// Bottom-left: L*C
Matrix LC = result.L * C;
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
result.Acl(n + i, j) = LC(i, j);
}
}
// Bottom-right: A - B*K - L*C
Matrix ABKLC = A - BK - LC;
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
result.Acl(n + i, n + j) = ABKLC(i, j);
}
}
return result;
}
/**
* @brief LQG Controller class for simulation
*
* Implements the LQG controller as a dynamic system that can be simulated.
*/
class LQGController {
private:
Matrix A_, B_, C_;
Matrix K_;
KalmanFilter *kf_;
size_t n_, m_, p_;
public:
/**
* @brief Construct LQG controller
*/
LQGController(const Matrix &A, const Matrix &B, const Matrix &C,
const Matrix &Q, const Matrix &R, const Matrix &W,
const Matrix &V)
: A_(A), B_(B), C_(C) {
n_ = A.rows;
m_ = B.cols;
p_ = C.rows;
// Design LQR
K_ = lqr(A, B, Q, R);
// Create Kalman filter
kf_ = new KalmanFilter(A, B, C, W, V);
}
~LQGController() { delete kf_; }
/**
* @brief Compute control input given measurement
*
* @param y Measurement
* @param u_prev Previous control input (for Kalman prediction)
* @return Control input u = -K*x̂
*/
Matrix computeControl(const Matrix &y, const Matrix &u_prev) {
// Update Kalman filter
auto est = kf_->update(y, u_prev);
// Compute control: u = -K*x̂
Matrix u = K_ * est.x_hat;
for (size_t i = 0; i < u.rows; ++i) {
u(i, 0) = -u(i, 0);
}
return u;
}
/**
* @brief Get state estimate
*/
Matrix getStateEstimate() const { return kf_->getState(); }
/**
* @brief Set initial state estimate
*/
void setInitialEstimate(const Matrix &x0, const Matrix &P0) {
kf_->setInitialState(x0, P0);
}
/**
* @brief Get LQR gains
*/
Matrix getGains() const { return K_; }
/**
* @brief Reset controller
*/
void reset() { kf_->reset(); }
};
/**
* @brief LQG with Loop Transfer Recovery (LQG/LTR)
*
* LQG/LTR recovers the robustness properties of LQR by:
* 1. At plant input: increase process noise (make Kalman faster)
* 2. At plant output: increase state weight (make LQR faster)
*
* As q → ∞ (or ρ → 0), the loop transfer function approaches
* the target loop transfer (LQR at input, Kalman at output).
*
* @param A System matrix
* @param B Input matrix
* @param C Output matrix
* @param Q Base state weight
* @param R Input weight
* @param W Base process noise
* @param V Measurement noise
* @param rho Recovery parameter (smaller = better recovery, but faster
* observer)
* @return LQG result with recovered robustness
*
* @note For input LTR: W_new = W + (1/ρ)*B*B'
* @note For output LTR: Q_new = Q + (1/ρ)*C'*C
*/
inline LQGResult designLQG_LTR(const Matrix &A, const Matrix &B,
const Matrix &C, const Matrix &Q,
const Matrix &R, const Matrix &W,
const Matrix &V, double rho = 0.01,
bool input_recovery = true) {
size_t n = A.rows;
if (input_recovery) {
// Input LTR: Increase process noise to speed up observer
// This recovers LQR robustness at plant input
Matrix BT = B.T();
Matrix BBT = B * BT;
Matrix W_ltr = W;
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
W_ltr(i, j) += BBT(i, j) / rho;
}
}
return designLQG(A, B, C, Q, R, W_ltr, V);
} else {
// Output LTR: Increase state weight to speed up controller
// This recovers Kalman robustness at plant output
Matrix CT = C.T();
Matrix CTC = CT * C;
Matrix Q_ltr = Q;
for (size_t i = 0; i < n; ++i) {
for (size_t j = 0; j < n; ++j) {
Q_ltr(i, j) += CTC(i, j) / rho;
}
}
return designLQG(A, B, C, Q_ltr, R, W, V);
}
}
/**
* @brief Compute loop transfer function for LQG analysis
*
* For robustness analysis, we need to examine:
* - Loop transfer at plant input: L_u = K(sI-A+BK+LC)^(-1)L C (sI-A)^(-1) B
* - Loop transfer at plant output: L_y = C(sI-A)^(-1) B K(sI-A+BK+LC)^(-1)L
*
* @param A System matrix
* @param B Input matrix
* @param C Output matrix
* @param K LQR gain
* @param L Kalman gain
* @return StateSpace representation of compensator K(sI-A+BK+LC)^(-1)L
*/
inline StateSpace lqgCompensator(const Matrix &A, const Matrix &B,
const Matrix &C, const Matrix &K,
const Matrix &L) {
size_t n = A.rows;
size_t m = B.cols;
size_t p = C.rows;
// Compensator: K(sI - A + BK + LC)^(-1) L
// State equation: x̂̇ = (A - BK - LC)x̂ + Ly
// Output equation: u = -K*x̂
Matrix Ac = A - B * K - L * C;
Matrix Bc = L;
// Output is -K*x̂
Matrix Cc(m, n);
for (size_t i = 0; i < m; ++i) {
for (size_t j = 0; j < n; ++j) {
Cc(i, j) = -K(0, j); // Only works for SISO
}
}
Matrix Dc = Matrix::zeros(m, p);
return StateSpace(Ac, Bc, Cc, Dc);
}
// ============================================================
// DISCRETE-TIME LQG
// ============================================================
/**
* @brief Design discrete-time LQG controller
*
* For discrete-time system:
* x(k+1) = A*x(k) + B*u(k) + w(k)
* y(k) = C*x(k) + v(k)
*
* Cost: J = E[ Σ(x'Qx + u'Ru) ]
*/
inline LQGResult designLQG_discrete(const Matrix &A, const Matrix &B,
const Matrix &C, const Matrix &Q,
const Matrix &R, const Matrix &W,
const Matrix &V) {
// For discrete-time, we solve DARE instead of CARE
// The implementation is similar but uses discrete Riccati equations
// For now, use continuous-time approximation
// TODO: Implement proper discrete Riccati solver
return designLQG(A, B, C, Q, R, W, V);
}
} // namespace control
} // namespace cppplot
#endif // CPPPLOT_CONTROL_LQG_HPP