[586] | 1 | #define BDMLIB |
---|
[717] | 2 | #include "../mat_checks.h" |
---|
[1130] | 3 | #include "design/lq_ctrl.h" |
---|
[586] | 4 | |
---|
| 5 | using namespace bdm; |
---|
| 6 | |
---|
[737] | 7 | TEST ( LQG_test ) { |
---|
[1064] | 8 | LQG reg; |
---|
| 9 | shared_ptr<StateSpace<chmat> > stsp = new StateSpace<chmat>; |
---|
| 10 | // 2 x 1 x 1 |
---|
| 11 | stsp-> set_parameters ( eye ( 2 ), ones ( 2, 1 ), ones ( 1, 2 ), ones ( 1, 1 ), /* Q,R */ eye ( 2 ), eye ( 1 ) ); |
---|
| 12 | reg.set_system ( stsp ); // A, B, C |
---|
| 13 | reg.set_control_parameters ( eye ( 1 ), eye ( 1 ), vec_1 ( 1.0 ), 6 ); //Qy, Qu, horizon |
---|
| 14 | reg.validate(); |
---|
[586] | 15 | |
---|
[1064] | 16 | reg.redesign(); |
---|
| 17 | double reg_apply = reg.ctrlaction ( "0.5, 1.1", "0.0" ) ( 0 ); /*convert vec to double*/ |
---|
| 18 | CHECK_CLOSE ( reg_apply, -0.248528137234392, 0.0001 ); |
---|
[586] | 19 | } |
---|
| 20 | |
---|
[737] | 21 | TEST ( to_state_test ) { |
---|
[1064] | 22 | mlnorm<fsqmat> ml; |
---|
| 23 | mat A = "1.1, 2.3"; |
---|
| 24 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
---|
| 25 | RV yr = RV ( "y", 1 ); |
---|
| 26 | RV ur = RV ( "u", 1 ); |
---|
| 27 | ml.set_rv ( yr ); |
---|
| 28 | yr.t_plus ( -1 ); |
---|
| 29 | ml.set_rvc ( concat ( yr, ur ) ); |
---|
[737] | 30 | |
---|
[1064] | 31 | shared_ptr<StateCanonical > Stsp = new StateCanonical; |
---|
| 32 | Stsp->connect_mlnorm ( ml ); |
---|
[737] | 33 | |
---|
[1064] | 34 | /* results from |
---|
| 35 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
| 36 | */ |
---|
| 37 | CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1" ), 0.0001 ); |
---|
| 38 | CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "2.53" ), 0.0001 ); |
---|
| 39 | CHECK_CLOSE_EX ( Stsp->_D().get_row ( 0 ), vec ( "2.30" ), 0.0001 ); |
---|
[605] | 40 | } |
---|
| 41 | |
---|
[737] | 42 | TEST ( to_state_arx_test ) { |
---|
[1064] | 43 | mlnorm<chmat> ml; |
---|
| 44 | mat A = "1.1, 2.3, 3.4"; |
---|
| 45 | ml.set_parameters ( A, vec_1 ( 1.3 ), eye ( 1 ) ); |
---|
| 46 | RV yr = RV ( "y", 1 ); |
---|
| 47 | RV ur = RV ( "u", 1 ); |
---|
| 48 | ml.set_rv ( yr ); |
---|
| 49 | ml.set_rvc ( concat ( yr.copy_t ( -1 ), concat ( ur, ur.copy_t ( -1 ) ) ) ); |
---|
[737] | 50 | |
---|
[1064] | 51 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
| 52 | RV xrv; |
---|
| 53 | RV urv; |
---|
| 54 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
---|
[737] | 55 | |
---|
[1064] | 56 | /* results from |
---|
| 57 | [A,B,C,D]=tf2ss([2.3 0],[1 -1.1]) |
---|
| 58 | */ |
---|
| 59 | CHECK_CLOSE_EX ( Stsp->_A().get_row ( 0 ), vec ( "1.1, 3.4, 1.3" ), 0.0001 ); |
---|
| 60 | CHECK_CLOSE_EX ( Stsp->_B().get_row ( 0 ), vec ( "2.3" ), 0.0001 ); |
---|
| 61 | CHECK_CLOSE_EX ( Stsp->_C().get_row ( 0 ), vec ( "1, 0, 0" ), 0.0001 ); |
---|
[703] | 62 | } |
---|
| 63 | |
---|
[737] | 64 | TEST ( arx_LQG_test ) { |
---|
[1064] | 65 | mlnorm<chmat> ml; |
---|
| 66 | mat A = "1.81, -.81, .00468, .00438"; |
---|
| 67 | ml.set_parameters ( A, vec_1 ( 0.0 ), 0.00001*eye ( 1 ) ); |
---|
| 68 | RV yr = RV ( "y", 1 ); |
---|
| 69 | RV ur = RV ( "u", 1 ); |
---|
| 70 | RV rgr = yr.copy_t ( -1 ); |
---|
| 71 | rgr.add ( yr.copy_t ( -2 ) ); |
---|
| 72 | rgr.add ( yr.copy_t ( -2 ) ); |
---|
| 73 | rgr.add ( ur.copy_t ( -1 ) ); |
---|
| 74 | rgr.add ( ur ); |
---|
[737] | 75 | |
---|
[1064] | 76 | ml.set_rv ( yr ); |
---|
| 77 | ml.set_rvc ( rgr ); |
---|
| 78 | ml.validate(); |
---|
[737] | 79 | |
---|
[1064] | 80 | shared_ptr<StateFromARX> Stsp = new StateFromARX; |
---|
| 81 | RV xrv; |
---|
| 82 | RV urv; |
---|
| 83 | Stsp->connect_mlnorm ( ml, xrv, urv ); |
---|
[737] | 84 | |
---|
[1064] | 85 | LQG L; |
---|
| 86 | L.set_system ( Stsp ); |
---|
| 87 | L.set_control_parameters ( eye ( 1 ), sqrt ( 1.0 / 1000 ) *eye ( 1 ), vec_1 ( 0.0 ), 100 ); |
---|
| 88 | L.validate(); |
---|
[737] | 89 | |
---|
[1064] | 90 | L.redesign(); |
---|
| 91 | cout << L.to_string() << endl; |
---|
[699] | 92 | } |
---|
[1130] | 93 | |
---|
[1194] | 94 | TEST (LQGU_static_vs_Riccati_test){ |
---|
[1157] | 95 | //test of universal LQG controller |
---|
| 96 | LQG_universal lq; |
---|
| 97 | lq.rv = RV("u", 2, 0); |
---|
| 98 | lq.set_rvc(RV("x", 2, 0)); |
---|
| 99 | lq.horizon = 10; |
---|
| 100 | |
---|
| 101 | /* |
---|
| 102 | model: x = Ax + Bu time: 0..horizon |
---|
| 103 | loss: l = x'Q'Qx + u'R'Ru time: 0..horizon-1 |
---|
| 104 | final loss: l = x'S'Sx time: horizon |
---|
| 105 | |
---|
| 106 | dim: x: 2 |
---|
| 107 | u: 2 |
---|
| 108 | |
---|
| 109 | A = [ 2 -1 ] |
---|
| 110 | [ 0 0.5 ] |
---|
| 111 | |
---|
| 112 | B = [ 1 -0.1 ] |
---|
| 113 | [ -0.2 2 ] |
---|
| 114 | |
---|
| 115 | Q = [ 5 0 ] |
---|
| 116 | [ 0 1 ] |
---|
| 117 | |
---|
| 118 | R = [ 0.01 0 ] |
---|
| 119 | [ 0 0.1 ] |
---|
| 120 | |
---|
| 121 | S = Q |
---|
| 122 | */ |
---|
| 123 | |
---|
[1194] | 124 | mat mA("2 -1;0 0.5"); |
---|
| 125 | mat mB("1 -0.1;-0.2 2"); |
---|
| 126 | mat mQ("5 0;0 1"); |
---|
| 127 | mat mR("0.01 0;0 0.1"); |
---|
| 128 | mat mS = mQ; |
---|
[1157] | 129 | |
---|
| 130 | //starting configuration |
---|
[1222] | 131 | vec x0("6 3"); |
---|
[1157] | 132 | |
---|
| 133 | //model |
---|
| 134 | Array<linfnEx> model(2); |
---|
| 135 | //model Ax part |
---|
| 136 | model(0).A = mA; |
---|
| 137 | model(0).B = vec("0 0"); |
---|
| 138 | model(0).rv = RV("x", 2, 0); |
---|
| 139 | model(0).rv_ret = RV("x", 2, 1); |
---|
| 140 | //model Bu part |
---|
| 141 | model(1).A = mB; |
---|
| 142 | model(1).B = vec("0 0"); |
---|
| 143 | model(1).rv = RV("u", 2, 0); |
---|
| 144 | model(1).rv_ret = RV("x", 2, 1); |
---|
| 145 | //setup |
---|
| 146 | lq.Models = model; |
---|
| 147 | |
---|
| 148 | //loss |
---|
| 149 | Array<quadraticfn> loss(2); |
---|
| 150 | //loss x'Qx part |
---|
| 151 | loss(0).Q.setCh(mQ); |
---|
[1205] | 152 | loss(0).rv = RV("x", 2, 1); |
---|
[1157] | 153 | //loss u'Ru part |
---|
| 154 | loss(1).Q.setCh(mR); |
---|
| 155 | loss(1).rv = RV("u", 2, 0); |
---|
| 156 | //setup |
---|
| 157 | lq.Losses = loss; |
---|
| 158 | |
---|
| 159 | //finalloss setup |
---|
| 160 | lq.finalLoss.Q.setCh(mS); |
---|
| 161 | lq.finalLoss.rv = RV("x", 2, 1); |
---|
[1194] | 162 | |
---|
| 163 | lq.validate(); |
---|
[1222] | 164 | |
---|
[1157] | 165 | //produce last control matrix L |
---|
| 166 | lq.redesign(); |
---|
| 167 | |
---|
| 168 | //verification via Riccati LQG version |
---|
| 169 | mat mK = mS; |
---|
| 170 | mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
[1222] | 171 | |
---|
[1157] | 172 | //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion |
---|
| 173 | //more important is reached loss compared in the next part |
---|
| 174 | double tolerr = 1;//0.01; //0.01 OK x 0.001 NO OK |
---|
| 175 | |
---|
| 176 | //check last time L matrix |
---|
| 177 | CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr); |
---|
| 178 | |
---|
| 179 | mat oldK; |
---|
| 180 | int i; |
---|
| 181 | |
---|
| 182 | //produce next control matrix L |
---|
| 183 | for(i = 0; i < lq.horizon - 1; i++) { |
---|
| 184 | lq.redesign(); |
---|
| 185 | oldK = mK; |
---|
| 186 | mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ; |
---|
| 187 | mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 188 | |
---|
| 189 | //check other times L matrix |
---|
| 190 | CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr); |
---|
| 191 | } |
---|
| 192 | |
---|
| 193 | //check losses of LQG control - compare LQG_universal and Riccati version, no noise |
---|
| 194 | |
---|
| 195 | //loss of LQG_universal |
---|
[1222] | 196 | vec loss_uni("0"); |
---|
[1157] | 197 | |
---|
| 198 | //setup |
---|
| 199 | vec x = x0; |
---|
| 200 | vec xold = x; |
---|
| 201 | vec u; |
---|
| 202 | //vec tmploss; |
---|
| 203 | |
---|
| 204 | //iteration |
---|
| 205 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 206 | u = lq.getL().get_cols(0,1) * xold; |
---|
| 207 | x = mA * xold + mB * u; |
---|
| 208 | /*tmploss*/loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 209 | //loss_uni += tmploss.get(0); |
---|
| 210 | xold = x; |
---|
| 211 | } |
---|
[1222] | 212 | loss_uni = x.transpose() * mS * x; |
---|
| 213 | |
---|
[1157] | 214 | //loss of LQG Riccati version |
---|
[1222] | 215 | vec loss_rct("0"); |
---|
[1157] | 216 | |
---|
| 217 | //setup |
---|
| 218 | x = x0; |
---|
| 219 | xold = x; |
---|
| 220 | |
---|
| 221 | //iteration |
---|
| 222 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 223 | u = mL * xold; |
---|
| 224 | x = mA * xold + mB * u; |
---|
[1222] | 225 | loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
[1157] | 226 | xold = x; |
---|
| 227 | } |
---|
[1222] | 228 | loss_rct = x.transpose() * mS * x; |
---|
| 229 | |
---|
[1194] | 230 | CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001); |
---|
| 231 | } |
---|
| 232 | |
---|
| 233 | TEST (LQGU_random_vs_Riccati_test){ |
---|
| 234 | |
---|
| 235 | //uniform random generator |
---|
| 236 | Uniform_RNG urng; |
---|
| 237 | double maxmult = 10.0; |
---|
| 238 | int maxxsize = 5; |
---|
| 239 | int maxusize = 5; |
---|
| 240 | |
---|
| 241 | urng.setup(2.0, maxxsize); |
---|
[1206] | 242 | int matxsize = round_i(urng()); |
---|
[1194] | 243 | urng.setup(2.0, maxusize); |
---|
[1206] | 244 | int matusize = round_i(urng()); |
---|
[1194] | 245 | |
---|
| 246 | urng.setup(-maxmult, maxmult); |
---|
| 247 | |
---|
| 248 | mat tmpmat; |
---|
| 249 | |
---|
| 250 | mat mA; |
---|
| 251 | mA = urng(matxsize, matxsize); |
---|
| 252 | mat mB; |
---|
| 253 | mB = urng(matxsize, matusize); |
---|
| 254 | mat mQ; |
---|
| 255 | tmpmat = urng(matxsize, matxsize); |
---|
| 256 | mQ = tmpmat*tmpmat.transpose(); |
---|
| 257 | mat mR; |
---|
| 258 | tmpmat = urng(matusize, matusize); |
---|
| 259 | mR = tmpmat*tmpmat.transpose(); |
---|
| 260 | mat mS; |
---|
| 261 | tmpmat = urng(matxsize, matxsize); |
---|
| 262 | mS = tmpmat*tmpmat.transpose(); |
---|
| 263 | |
---|
| 264 | //starting configuration |
---|
| 265 | vec x0; |
---|
[1222] | 266 | x0 = urng(matxsize); |
---|
[1194] | 267 | |
---|
| 268 | vec zerovecx(matxsize); |
---|
| 269 | zerovecx.zeros(); |
---|
| 270 | vec zerovecu(matusize); |
---|
| 271 | zerovecu.zeros(); |
---|
| 272 | |
---|
| 273 | //test of universal LQG controller |
---|
| 274 | LQG_universal lq; |
---|
| 275 | lq.rv = RV("u", matusize, 0); |
---|
| 276 | lq.set_rvc(RV("x", matxsize, 0)); |
---|
| 277 | lq.horizon = 10; |
---|
| 278 | |
---|
| 279 | //model |
---|
| 280 | Array<linfnEx> model(2); |
---|
| 281 | //model Ax part |
---|
| 282 | model(0).A = mA; |
---|
| 283 | model(0).B = zerovecx; |
---|
| 284 | model(0).rv = RV("x", matxsize, 0); |
---|
| 285 | model(0).rv_ret = RV("x", matxsize, 1); |
---|
| 286 | //model Bu part |
---|
| 287 | model(1).A = mB; |
---|
| 288 | model(1).B = zerovecu; |
---|
| 289 | model(1).rv = RV("u", matusize, 0); |
---|
| 290 | model(1).rv_ret = RV("x", matxsize, 1); |
---|
| 291 | //setup |
---|
| 292 | lq.Models = model; |
---|
| 293 | |
---|
| 294 | //loss |
---|
| 295 | Array<quadraticfn> loss(2); |
---|
| 296 | //loss x'Qx part |
---|
| 297 | loss(0).Q.setCh(mQ); |
---|
[1205] | 298 | loss(0).rv = RV("x", matxsize, 1); |
---|
[1194] | 299 | //loss u'Ru part |
---|
| 300 | loss(1).Q.setCh(mR); |
---|
| 301 | loss(1).rv = RV("u", matusize, 0); |
---|
| 302 | //setup |
---|
| 303 | lq.Losses = loss; |
---|
| 304 | |
---|
| 305 | //finalloss setup |
---|
| 306 | lq.finalLoss.Q.setCh(mS); |
---|
| 307 | lq.finalLoss.rv = RV("x", matxsize, 1); |
---|
| 308 | |
---|
| 309 | lq.validate(); |
---|
| 310 | |
---|
| 311 | //produce last control matrix L |
---|
| 312 | lq.redesign(); |
---|
| 313 | |
---|
| 314 | //verification via Riccati LQG version |
---|
| 315 | mat mK = mS; |
---|
| 316 | mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 317 | |
---|
| 318 | //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion |
---|
| 319 | //more important is reached loss compared in the next part |
---|
[1222] | 320 | double tolerr = 2; |
---|
[1194] | 321 | |
---|
| 322 | //check last time L matrix |
---|
| 323 | CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr); |
---|
| 324 | |
---|
| 325 | mat oldK; |
---|
| 326 | int i; |
---|
| 327 | |
---|
| 328 | //produce next control matrix L |
---|
| 329 | for(i = 0; i < lq.horizon - 1; i++) { |
---|
| 330 | lq.redesign(); |
---|
| 331 | oldK = mK; |
---|
| 332 | mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ; |
---|
| 333 | mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 334 | |
---|
| 335 | //check other times L matrix |
---|
| 336 | CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr); |
---|
| 337 | } |
---|
| 338 | |
---|
| 339 | //check losses of LQG control - compare LQG_universal and Riccati version, no noise |
---|
| 340 | |
---|
| 341 | //loss of LQG_universal |
---|
| 342 | vec loss_uni("0"); |
---|
| 343 | |
---|
| 344 | //setup |
---|
| 345 | vec x = x0; |
---|
| 346 | vec xold = x; |
---|
| 347 | vec u; |
---|
| 348 | //vec tmploss; |
---|
| 349 | |
---|
| 350 | //iteration |
---|
| 351 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 352 | u = lq.getL().get_cols(0,(matxsize-1)) * xold; |
---|
| 353 | x = mA * xold + mB * u; |
---|
| 354 | loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 355 | xold = x; |
---|
| 356 | } |
---|
| 357 | |
---|
| 358 | loss_uni = x.transpose() * mS * x; |
---|
| 359 | |
---|
| 360 | //loss of LQG Riccati version |
---|
| 361 | vec loss_rct("0"); |
---|
| 362 | |
---|
| 363 | //setup |
---|
| 364 | x = x0; |
---|
| 365 | xold = x; |
---|
| 366 | |
---|
| 367 | //iteration |
---|
| 368 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 369 | u = mL * xold; |
---|
| 370 | x = mA * xold + mB * u; |
---|
| 371 | loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 372 | xold = x; |
---|
| 373 | } |
---|
| 374 | loss_rct = x.transpose() * mS * x; |
---|
| 375 | |
---|
| 376 | CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001); |
---|
| 377 | } |
---|
| 378 | |
---|
| 379 | TEST (LQGU_SiSy_test){ |
---|
| 380 | int K = 5; |
---|
| 381 | int N = 100; |
---|
| 382 | double sigma = 0.1; |
---|
| 383 | double yr = 1; |
---|
| 384 | double y0 = 0; |
---|
| 385 | double x0 = y0 - yr; |
---|
| 386 | double Eb = 1; |
---|
| 387 | double P; |
---|
| 388 | //double P = 0.01; //loss ~ 1.0??? e-2 |
---|
| 389 | //double P = 0.1; //loss ~ 1.??? e-1 |
---|
| 390 | //double P = 1; //loss ~ ???.??? e+2 |
---|
| 391 | //double P = 10; //loss ~ ?e+6 |
---|
| 392 | |
---|
| 393 | mat A("1"); |
---|
| 394 | mat B(1,1); |
---|
| 395 | B(0,0) = Eb; |
---|
| 396 | mat Q("1"); |
---|
| 397 | mat R("0"); |
---|
| 398 | |
---|
| 399 | vec u(K); |
---|
| 400 | u.zeros(); |
---|
| 401 | mat xn(K, N); |
---|
| 402 | xn.zeros(); |
---|
| 403 | vec S(K); |
---|
| 404 | S.zeros(); |
---|
| 405 | vec L(K); |
---|
| 406 | L.zeros(); |
---|
| 407 | |
---|
| 408 | vec loss(N); |
---|
| 409 | loss.zeros(); |
---|
| 410 | |
---|
| 411 | LQG_universal lq; |
---|
| 412 | lq.rv = RV("u", 1, 0); |
---|
| 413 | lq.set_rvc(RV("x", 1, 0)); |
---|
| 414 | lq.horizon = K; |
---|
| 415 | |
---|
| 416 | Array<linfnEx> model(2); |
---|
| 417 | model(0).A = A; |
---|
| 418 | model(0).B = vec("0 0"); |
---|
| 419 | model(0).rv = RV("x", 1, 0); |
---|
| 420 | model(0).rv_ret = RV("x", 1, 1); |
---|
| 421 | model(1).A = B; |
---|
| 422 | model(1).B = vec("0 0"); |
---|
| 423 | model(1).rv = RV("u", 1, 0); |
---|
| 424 | model(1).rv_ret = RV("x", 1, 1); |
---|
| 425 | lq.Models = model; |
---|
| 426 | |
---|
| 427 | Array<quadraticfn> qloss(2); |
---|
| 428 | qloss(0).Q.setCh(Q); |
---|
[1205] | 429 | qloss(0).rv = RV("x", 1, 1); |
---|
[1194] | 430 | qloss(1).Q.setCh(R); |
---|
| 431 | qloss(1).rv = RV("u", 1, 0); |
---|
| 432 | lq.Losses = qloss; |
---|
| 433 | |
---|
| 434 | lq.finalLoss.Q.setCh(Q); |
---|
| 435 | lq.finalLoss.rv = RV("x", 1, 1); |
---|
| 436 | |
---|
| 437 | lq.validate(); |
---|
| 438 | |
---|
| 439 | int n, k; |
---|
| 440 | double Br; |
---|
[1222] | 441 | |
---|
[1194] | 442 | for(k = K-1; k >= 0;k--){ |
---|
| 443 | lq.redesign(); |
---|
| 444 | L(k) = (lq.getL())(0,0); |
---|
| 445 | } |
---|
| 446 | |
---|
| 447 | for(P = 0.01; P <= 10; (P*=10)){ |
---|
[1222] | 448 | lq.resetTime(); |
---|
| 449 | |
---|
[1194] | 450 | for(n = 0; n < N; n++){ |
---|
| 451 | Br = Eb + sqrt(P)*randn(); |
---|
| 452 | |
---|
| 453 | xn(0, n) = x0 + sigma*randn(); |
---|
| 454 | for(k = 0; k < K-1; k++){ |
---|
| 455 | u(k) = L(k)*xn(k, n); |
---|
| 456 | xn(k+1, n) = (A*xn(k, n))(0,0) + Br*u(k) + sigma*randn(); |
---|
| 457 | } |
---|
| 458 | |
---|
| 459 | loss(n) = 0; |
---|
| 460 | for(k=0;k<K;k++) loss(n) += xn(k, n)*xn(k, n); |
---|
| 461 | |
---|
| 462 | } |
---|
| 463 | |
---|
| 464 | double tolerr; |
---|
| 465 | //double P = 0.01; //loss ~ 1.0??? e-2 |
---|
| 466 | //double P = 0.1; //loss ~ 1.??? e-1 |
---|
| 467 | //double P = 1; //loss ~ ???.??? e+2 |
---|
| 468 | //double P = 10; //loss ~ ?e+6 |
---|
| 469 | if(P == 0.01) tolerr = 0.2; |
---|
| 470 | else if(P == 0.1) tolerr = 2; |
---|
| 471 | else if(P == 1) tolerr = 2000; |
---|
| 472 | else if(P == 10) tolerr = 2e+7; |
---|
| 473 | else tolerr = 0; |
---|
| 474 | |
---|
| 475 | CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr); |
---|
| 476 | } |
---|
| 477 | } |
---|
| 478 | |
---|
| 479 | TEST (LQGU_SiSy_SuSt_test){ |
---|
| 480 | int K = 5; |
---|
| 481 | int N = 100; |
---|
| 482 | double sigma = 0.1; |
---|
| 483 | double yr = 1; |
---|
| 484 | double y0 = 0; |
---|
| 485 | double Eb = 1; |
---|
| 486 | double inP; |
---|
| 487 | //double inP = 0.01; //loss ~ 1.0??? e-2 |
---|
| 488 | //double inP = 0.1; //loss ~ 1.0??? e-2 |
---|
| 489 | //double inP = 1; //loss ~ 1.0??? e-2 |
---|
| 490 | //double inP = 10; //loss ~ 1.0??? e-2 |
---|
| 491 | |
---|
| 492 | vec x0(3); |
---|
| 493 | x0(0) = y0 - yr; |
---|
| 494 | x0(1) = Eb; |
---|
[1222] | 495 | |
---|
[1194] | 496 | mat A("1 0 0; 0 1 0; 0 0 1"); |
---|
| 497 | mat B("0; 0; 0"); |
---|
| 498 | mat X("1 0 0; 0 0 0; 0 0 0"); |
---|
| 499 | mat Y("0.00001"); |
---|
| 500 | mat Q(3,3); |
---|
| 501 | Q.zeros(); |
---|
| 502 | Q(1,1) = sigma*sigma; |
---|
[1222] | 503 | |
---|
[1194] | 504 | vec u(K); |
---|
| 505 | u.zeros(); |
---|
| 506 | vec Kk(K); |
---|
| 507 | Kk.zeros(); |
---|
| 508 | mat xn0(K+1, N); |
---|
| 509 | xn0.zeros(); |
---|
| 510 | mat xn1(K+1, N); |
---|
| 511 | xn1.zeros(); |
---|
| 512 | mat xn2(K+1, N); |
---|
| 513 | xn2.zeros(); |
---|
[1222] | 514 | |
---|
[1194] | 515 | mat L(1, 3); |
---|
| 516 | L.zeros(); |
---|
| 517 | vec loss(N); |
---|
| 518 | loss.zeros(); |
---|
| 519 | |
---|
| 520 | LQG_universal lq; |
---|
| 521 | lq.rv = RV("u", 1, 0); |
---|
| 522 | lq.set_rvc(RV("x", 3, 0)); |
---|
| 523 | lq.horizon = K; |
---|
| 524 | |
---|
| 525 | Array<linfnEx> model(2); |
---|
[1222] | 526 | model(0).A = A; |
---|
| 527 | model(0).B = vec("0 0"); |
---|
| 528 | model(0).rv = RV("x", 3, 0); |
---|
| 529 | model(0).rv_ret = RV("x", 3, 1); |
---|
| 530 | model(1).A = B; |
---|
| 531 | model(1).B = vec("0 0"); |
---|
| 532 | model(1).rv = RV("u", 1, 0); |
---|
| 533 | model(1).rv_ret = RV("x", 3, 1); |
---|
| 534 | lq.Models = model; |
---|
[1194] | 535 | |
---|
[1222] | 536 | Array<quadraticfn> qloss(2); |
---|
| 537 | qloss(0).Q.setCh(X); |
---|
| 538 | qloss(0).rv = RV("x", 3, 1); |
---|
| 539 | qloss(1).Q.setCh(Y); |
---|
| 540 | qloss(1).rv = RV("u", 1, 0); |
---|
| 541 | lq.Losses = qloss; |
---|
[1194] | 542 | |
---|
[1222] | 543 | lq.finalLoss.Q.setCh(X); |
---|
| 544 | lq.finalLoss.rv = RV("x", 3, 1); |
---|
[1194] | 545 | |
---|
[1222] | 546 | lq.validate(); |
---|
[1194] | 547 | |
---|
| 548 | int n, k; |
---|
[1222] | 549 | |
---|
[1194] | 550 | for(inP = 0.01; inP <= 10; (inP*=10)){ |
---|
| 551 | lq.resetTime(); |
---|
| 552 | x0(2) = inP; |
---|
| 553 | for(n = 0; n < N; n++){ |
---|
| 554 | L.zeros(); |
---|
| 555 | xn0(0, n) = x0(0); |
---|
| 556 | xn1(0, n) = x0(1) + sqrt(inP) * randn(); |
---|
| 557 | xn2(0, n) = x0(2); |
---|
| 558 | //^neznalost, sum zatim ne |
---|
| 559 | |
---|
| 560 | for(k = 0; k < K-1; k++){ |
---|
| 561 | u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n); |
---|
| 562 | Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 563 | xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn(); |
---|
| 564 | xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)); |
---|
| 565 | xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n); |
---|
| 566 | } |
---|
| 567 | |
---|
| 568 | lq.resetTime(); |
---|
| 569 | lq.redesign(); |
---|
| 570 | for(k = K-1; k > 0; k--){ |
---|
| 571 | A(0, 1) = u(k); |
---|
| 572 | A(1, 0) = -Kk(k); |
---|
| 573 | A(1, 1) = 1-Kk(k)*u(k); |
---|
| 574 | A(1, 2) = u(k)*sigma*sigma*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 575 | A(2, 2) = 1 - u(k)*u(k)*xn2(k, n)*(u(k)*u(k)*xn2(k, n) + 2*sigma*sigma) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 576 | B(0) = xn1(k, n); |
---|
| 577 | B(1) = ((xn2(k, n)*sigma*sigma-u(k)*u(k)*xn2(k, n)*xn2(k, n))*(xn0(k+1, n)-xn0(k, n)) - 2*xn1(k, n)*u(k)) / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 578 | B(2) = -2*u(k)*xn2(k, n)*xn2(k, n)*sigma*sigma / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 579 | lq.Models(0).A = A; |
---|
| 580 | lq.Models(1).A = B; |
---|
| 581 | lq.redesign(); |
---|
| 582 | } |
---|
| 583 | L = lq.getL(); |
---|
| 584 | |
---|
| 585 | //simulation |
---|
| 586 | xn0(0, n) += sigma*randn(); |
---|
| 587 | for(k = 0; k < K-1; k++){ |
---|
| 588 | u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n); |
---|
| 589 | Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
[1222] | 590 | xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn(); |
---|
| 591 | xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)); |
---|
| 592 | xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n); |
---|
[1194] | 593 | } |
---|
| 594 | |
---|
| 595 | |
---|
[1222] | 596 | loss(n) = 0; |
---|
[1194] | 597 | for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n); |
---|
| 598 | |
---|
[1222] | 599 | } |
---|
[1194] | 600 | |
---|
| 601 | double tolerr = 0.2; |
---|
[1222] | 602 | |
---|
[1194] | 603 | CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr); |
---|
| 604 | } |
---|
| 605 | } |
---|
| 606 | |
---|
[1209] | 607 | TEST (LQGU_PMSM_test){ |
---|
[1205] | 608 | |
---|
[1215] | 609 | int K = 5; |
---|
| 610 | int Kt = 200; |
---|
[1209] | 611 | int N = 15; |
---|
[1194] | 612 | |
---|
[1205] | 613 | double a = 0.9898; |
---|
| 614 | double b = 0.0072; |
---|
| 615 | double c = 0.0361; |
---|
| 616 | double d = 1; |
---|
| 617 | double e = 0.0149; |
---|
| 618 | |
---|
[1215] | 619 | double OMEGAt = 11.5; |
---|
[1205] | 620 | double DELTAt = 0.000125; |
---|
| 621 | |
---|
| 622 | double v = 0.0001; |
---|
| 623 | double w = 1; |
---|
| 624 | |
---|
| 625 | mat A(5,5); |
---|
| 626 | A.zeros(); |
---|
| 627 | A(0,0) = a; |
---|
| 628 | A(1,1) = a; |
---|
| 629 | A(2,2) = d; |
---|
| 630 | A(3,3) = 1.0; |
---|
| 631 | A(4,4) = 1.0; |
---|
| 632 | A(2,4) = d-1.0; |
---|
| 633 | A(3,2) = DELTAt; |
---|
| 634 | A(3,4) = DELTAt; |
---|
| 635 | |
---|
| 636 | mat B(5,2); |
---|
| 637 | B.zeros(); |
---|
| 638 | B(0,0) = c; |
---|
| 639 | B(1,1) = c; |
---|
| 640 | |
---|
| 641 | mat C(2,5); |
---|
| 642 | C.zeros(); |
---|
| 643 | C(0,0) = c; |
---|
| 644 | C(1,1) = c; |
---|
| 645 | |
---|
| 646 | mat X(5,5); |
---|
| 647 | X.zeros(); |
---|
| 648 | X(2,2) = w; |
---|
| 649 | |
---|
| 650 | mat Y(2,2); |
---|
| 651 | Y.zeros(); |
---|
| 652 | Y(0,0) = v; |
---|
| 653 | Y(1,1) = v; |
---|
| 654 | |
---|
| 655 | mat Q(5,5); |
---|
| 656 | Q.zeros(); |
---|
| 657 | Q(0,0) = 0.0013; |
---|
| 658 | Q(1,1) = 0.0013; |
---|
| 659 | Q(2,2) = 5e-6; |
---|
| 660 | Q(3,3) = 1e-10; |
---|
| 661 | |
---|
| 662 | mat R(2,2); |
---|
| 663 | R.zeros(); |
---|
| 664 | R(0,0) = 0.0006; |
---|
| 665 | R(0,0) = 0.0006; |
---|
| 666 | |
---|
| 667 | vec x0(5); |
---|
| 668 | x0.zeros(); |
---|
| 669 | x0(2) = 1.0-OMEGAt; |
---|
| 670 | x0(3) = itpp::pi / 2; |
---|
| 671 | x0(4) = OMEGAt; |
---|
| 672 | |
---|
| 673 | mat P(5,5); |
---|
| 674 | P.zeros(); |
---|
| 675 | P(0,0) = 0.01; |
---|
| 676 | P(1,1) = 0.01; |
---|
| 677 | P(2,2) = 0.01; |
---|
| 678 | P(3,3) = 0.01; |
---|
[1209] | 679 | |
---|
[1205] | 680 | vec u(2); |
---|
| 681 | |
---|
[1209] | 682 | mat x(5,Kt+K); |
---|
[1205] | 683 | |
---|
| 684 | mat L(2, 5); |
---|
| 685 | mat L0(5, Kt); |
---|
| 686 | mat L1(5, Kt); |
---|
| 687 | |
---|
| 688 | int i,n,k,kt; |
---|
| 689 | vec x00(5); |
---|
| 690 | vec randvec(2); |
---|
| 691 | |
---|
| 692 | LQG_universal lq; |
---|
| 693 | lq.rv = RV("u", 2, 0); |
---|
| 694 | lq.set_rvc(RV("x", 5, 0)); |
---|
| 695 | lq.horizon = K; |
---|
| 696 | |
---|
| 697 | Array<linfnEx> model(2); |
---|
| 698 | model(0).A = A; |
---|
| 699 | model(0).B = vec("0 0 0 0 0"); |
---|
| 700 | model(0).rv = RV("x", 5, 0); |
---|
| 701 | model(0).rv_ret = RV("x", 5, 1); |
---|
| 702 | model(1).A = B; |
---|
| 703 | model(1).B = vec("0 0"); |
---|
| 704 | model(1).rv = RV("u", 2, 0); |
---|
| 705 | model(1).rv_ret = RV("x", 5, 1); |
---|
| 706 | lq.Models = model; |
---|
| 707 | |
---|
| 708 | Array<quadraticfn> qloss(2); |
---|
| 709 | qloss(0).Q.setCh(X); |
---|
| 710 | qloss(0).rv = RV("x", 5, 1); |
---|
| 711 | qloss(1).Q.setCh(Y); |
---|
| 712 | qloss(1).rv = RV("u", 2, 0); |
---|
| 713 | lq.Losses = qloss; |
---|
| 714 | |
---|
| 715 | lq.finalLoss.Q.setCh(X); |
---|
| 716 | lq.finalLoss.rv = RV("x", 5, 1); |
---|
| 717 | |
---|
| 718 | lq.validate(); |
---|
| 719 | |
---|
| 720 | vec losses(N); |
---|
| 721 | losses.zeros(); |
---|
[1215] | 722 | vec omega(Kt+K); |
---|
| 723 | omega.zeros(); |
---|
[1205] | 724 | |
---|
| 725 | for(n = 0; n < N; n++) { |
---|
| 726 | L0.zeros(); |
---|
| 727 | L1.zeros(); |
---|
[1209] | 728 | |
---|
| 729 | for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn(); |
---|
[1205] | 730 | |
---|
| 731 | for(kt = 0; kt < Kt; kt++){ |
---|
| 732 | x.set_col(0, x00); |
---|
[1209] | 733 | |
---|
[1205] | 734 | for(k = 0; k < kt+K-1; k++){ |
---|
| 735 | L.set_size(2,5); |
---|
| 736 | L.set_row(0, L0.get_col(kt)); |
---|
| 737 | L.set_row(1, L1.get_col(kt)); |
---|
| 738 | u = L*x.get_col(k); |
---|
[1209] | 739 | if(u(0) > 50) u(0) = 50; |
---|
| 740 | else if(u(0) < -50) u(0) = -50; |
---|
| 741 | if(u(1) > 50) u(1) = 50; |
---|
| 742 | else if(u(1) < -50) u(1) = -50; |
---|
| 743 | |
---|
[1222] | 744 | x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0); |
---|
| 745 | x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1); |
---|
| 746 | x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k))); |
---|
| 747 | x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt; |
---|
[1209] | 748 | x(4, k+1) = x(4, k); |
---|
[1205] | 749 | } |
---|
| 750 | |
---|
| 751 | lq.resetTime(); |
---|
| 752 | lq.redesign(); |
---|
| 753 | for(k = K-1; k > 0; k--){ |
---|
| 754 | A(2, 0) = -e*sin(x(3, k+kt-1)); |
---|
| 755 | A(2, 1) = e*cos(x(3, k+kt-1)); |
---|
| 756 | A(0, 2) = b*sin(x(3, k+kt-1)); |
---|
| 757 | A(1, 2) = -b*cos(x(3, k+kt-1)); |
---|
| 758 | A(0, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*cos(x(3, k+kt-1)); |
---|
| 759 | A(1, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*sin(x(3, k+kt-1)); |
---|
| 760 | A(2, 3) = -e*(x(1, k+kt-1)*sin(x(3, k+kt-1) + x(0,k+kt-1)*cos(x(3, k+kt-1)))); |
---|
| 761 | A(0, 4) = b*sin(x(3, k+kt-1)); |
---|
| 762 | A(1, 4) = -b*cos(x(3, k+kt-1)); |
---|
| 763 | lq.Models(0).A = A; |
---|
| 764 | lq.redesign(); |
---|
| 765 | } |
---|
| 766 | L = lq.getL(); |
---|
| 767 | L0.set_col(kt, L.get_row(0).get(0,4)); |
---|
| 768 | L1.set_col(kt, L.get_row(1).get(0,4)); |
---|
| 769 | } |
---|
[1209] | 770 | |
---|
[1205] | 771 | x.set_col(0, x00); |
---|
[1209] | 772 | |
---|
| 773 | for(k = 0; k < Kt; k++){ |
---|
| 774 | |
---|
| 775 | L.set_size(2,5); |
---|
[1205] | 776 | L.set_row(0, L0.get_col(k)); |
---|
| 777 | L.set_row(1, L1.get_col(k)); |
---|
| 778 | u = L*x.get_col(k); |
---|
[1209] | 779 | if(u(0) > 50) u(0) = 50; |
---|
| 780 | else if(u(0) < -50) u(0) = -50; |
---|
| 781 | if(u(1) > 50) u(1) = 50; |
---|
| 782 | else if(u(1) < -50) u(1) = -50; |
---|
| 783 | |
---|
[1205] | 784 | x(0, k+1) = a*x(0, k) + b*(x(2, k) + x(4, k))*sin(x(3, k)) + c*u(0) + sqrt(Q(0, 0))*randn(); |
---|
| 785 | x(1, k+1) = a*x(1, k) - b*(x(2, k) + x(4, k))*cos(x(3, k)) + c*u(1) + sqrt(Q(1, 1))*randn(); |
---|
| 786 | x(2, k+1) = d*x(2, k) + (d-1)*x(4, k) + e*(x(1, k)*cos(x(3, k)) - x(0, k)*sin(x(3, k))) + sqrt(Q(2, 2))*randn(); |
---|
| 787 | x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt + sqrt(Q(3, 3))*randn(); |
---|
| 788 | x(4, k+1) = x(4, k); |
---|
[1209] | 789 | |
---|
[1222] | 790 | losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0); |
---|
[1215] | 791 | } |
---|
| 792 | |
---|
| 793 | omega += x.get_row(2); |
---|
[1209] | 794 | } |
---|
[1205] | 795 | |
---|
[1222] | 796 | cout << "Loss: " << sum(losses)/N << endl; |
---|
[1209] | 797 | } |
---|
[1205] | 798 | |
---|
[1209] | 799 | TEST (LQGU_181_zero){ |
---|
| 800 | /* v ????? .8189 |
---|
| 801 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
[1205] | 802 | |
---|
[1209] | 803 | S = y^2 + u^2/1000 |
---|
[1205] | 804 | |
---|
[1209] | 805 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 806 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 807 | */ |
---|
| 808 | |
---|
| 809 | int K = 150; |
---|
| 810 | |
---|
| 811 | LQG_universal lq; |
---|
| 812 | lq.rv = RV("ut", 1, 0); |
---|
| 813 | RV rvc; |
---|
| 814 | rvc = RV("yt", 1, -1); |
---|
| 815 | rvc.add(RV("yt", 1, -2)); |
---|
| 816 | rvc.add(RV("ut", 1, -1)); |
---|
| 817 | rvc.add(RV("1", 1, 0)); |
---|
| 818 | lq.set_rvc(rvc); |
---|
| 819 | lq.horizon = K; |
---|
| 820 | |
---|
| 821 | Array<linfnEx> model(4); |
---|
| 822 | model(0).A = mat("1.81"); |
---|
| 823 | model(0).B = vec("0"); |
---|
| 824 | model(0).rv = RV("yt", 1, -1); |
---|
| 825 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 826 | model(1).A = mat("-0.8189"); |
---|
| 827 | model(1).B = vec("0"); |
---|
| 828 | model(1).rv = RV("yt", 1, -2); |
---|
| 829 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 830 | model(2).A = mat("0.00438"); |
---|
| 831 | model(2).B = vec("0"); |
---|
| 832 | model(2).rv = RV("ut", 1, 0); |
---|
| 833 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 834 | model(3).A = mat("0.00468"); |
---|
| 835 | model(3).B = vec("0"); |
---|
| 836 | model(3).rv = RV("ut", 1, -1); |
---|
| 837 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 838 | lq.Models = model; |
---|
| 839 | |
---|
| 840 | Array<quadraticfn> qloss(2); |
---|
| 841 | qloss(0).Q.setCh(mat("1")); |
---|
| 842 | qloss(0).rv = RV("yt", 1, 0); |
---|
[1222] | 843 | qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) |
---|
[1209] | 844 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 845 | lq.Losses = qloss; |
---|
| 846 | |
---|
| 847 | lq.finalLoss.Q.setCh(mat("1")); |
---|
| 848 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 849 | |
---|
| 850 | lq.validate(); |
---|
| 851 | |
---|
| 852 | lq.resetTime(); |
---|
| 853 | lq.redesign(); |
---|
| 854 | |
---|
[1222] | 855 | for(int k = K-1; k > 0; k--){ |
---|
[1209] | 856 | lq.redesign(); |
---|
[1205] | 857 | } |
---|
| 858 | |
---|
[1222] | 859 | mat resref("-69.4086 47.2578 -0.2701 0.0"); |
---|
| 860 | CHECK_CLOSE_EX(resref, lq.getL(), 0.1); |
---|
[1194] | 861 | } |
---|
| 862 | |
---|
[1215] | 863 | TEST (LQGU_181_ten){ |
---|
| 864 | /* v ????? .8189 |
---|
| 865 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
| 866 | |
---|
| 867 | S = y^2 + u^2/1000 |
---|
| 868 | |
---|
| 869 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 870 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 871 | */ |
---|
| 872 | |
---|
| 873 | int K = 150; |
---|
[1222] | 874 | |
---|
[1215] | 875 | LQG_universal lq; |
---|
| 876 | lq.rv = RV("ut", 1, 0); |
---|
| 877 | RV rvc; |
---|
| 878 | rvc = RV("yt", 1, -1); |
---|
| 879 | rvc.add(RV("yt", 1, -2)); |
---|
| 880 | rvc.add(RV("ut", 1, -1)); |
---|
| 881 | rvc.add(RV("1", 1, 0)); |
---|
| 882 | lq.set_rvc(rvc); |
---|
| 883 | lq.horizon = K; |
---|
| 884 | |
---|
| 885 | Array<linfnEx> model(4); |
---|
| 886 | model(0).A = mat("1.81"); |
---|
| 887 | model(0).B = vec("0"); |
---|
| 888 | model(0).rv = RV("yt", 1, -1); |
---|
| 889 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 890 | model(1).A = mat("-0.8189"); |
---|
| 891 | model(1).B = vec("0"); |
---|
| 892 | model(1).rv = RV("yt", 1, -2); |
---|
| 893 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 894 | model(2).A = mat("0.00438"); |
---|
| 895 | model(2).B = vec("0"); |
---|
| 896 | model(2).rv = RV("ut", 1, 0); |
---|
| 897 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 898 | model(3).A = mat("0.00468"); |
---|
| 899 | model(3).B = vec("0"); |
---|
| 900 | model(3).rv = RV("ut", 1, -1); |
---|
| 901 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 902 | lq.Models = model; |
---|
| 903 | |
---|
| 904 | Array<quadraticfn> qloss(2); |
---|
| 905 | qloss(0).Q.setCh(mat("1 -10")); |
---|
| 906 | qloss(0).rv = RV("yt", 1, 0); |
---|
| 907 | qloss(0).rv.add(RV("1", 1, 0)); |
---|
| 908 | qloss(1).Q.setCh(mat("0.03162")); |
---|
| 909 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 910 | lq.Losses = qloss; |
---|
| 911 | |
---|
| 912 | lq.finalLoss.Q.setCh(mat("0")); |
---|
| 913 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 914 | |
---|
| 915 | lq.validate(); |
---|
| 916 | |
---|
| 917 | lq.resetTime(); |
---|
| 918 | lq.redesign(); |
---|
| 919 | |
---|
[1222] | 920 | for(int k = K-1; k > 0; k--){ |
---|
[1215] | 921 | lq.redesign(); |
---|
| 922 | } |
---|
| 923 | |
---|
[1222] | 924 | mat resref("-69.4086 47.2578 -0.2701 233.758"); |
---|
| 925 | CHECK_CLOSE_EX(resref, lq.getL(), 0.1); |
---|
[1215] | 926 | } |
---|
| 927 | |
---|
[1209] | 928 | TEST (LQGU_181_reqv){ |
---|
| 929 | /* v ????? .8189 |
---|
| 930 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
| 931 | |
---|
| 932 | S = y^2 + u^2/1000 |
---|
| 933 | |
---|
| 934 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 935 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 936 | */ |
---|
| 937 | |
---|
[1222] | 938 | int K = 200; |
---|
[1209] | 939 | |
---|
| 940 | LQG_universal lq; |
---|
| 941 | lq.rv = RV("ut", 1, 0); |
---|
| 942 | RV rvc; |
---|
| 943 | rvc = RV("yt", 1, -1); |
---|
| 944 | rvc.add(RV("yt", 1, -2)); |
---|
| 945 | rvc.add(RV("ut", 1, -1)); |
---|
| 946 | rvc.add(RV("yr", 1, 0)); |
---|
| 947 | rvc.add(RV("1", 1, 0)); |
---|
| 948 | lq.set_rvc(rvc); |
---|
| 949 | lq.horizon = K; |
---|
| 950 | |
---|
| 951 | Array<linfnEx> model(5); |
---|
| 952 | model(0).A = mat("1.81"); |
---|
| 953 | model(0).B = vec("0"); |
---|
| 954 | model(0).rv = RV("yt", 1, -1); |
---|
| 955 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 956 | model(1).A = mat("-0.8189"); |
---|
| 957 | model(1).B = vec("0"); |
---|
| 958 | model(1).rv = RV("yt", 1, -2); |
---|
| 959 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 960 | model(2).A = mat("0.00438"); |
---|
| 961 | model(2).B = vec("0"); |
---|
| 962 | model(2).rv = RV("ut", 1, 0); |
---|
| 963 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 964 | model(3).A = mat("0.00468"); |
---|
| 965 | model(3).B = vec("0"); |
---|
| 966 | model(3).rv = RV("ut", 1, -1); |
---|
| 967 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 968 | model(4).A = mat("1"); |
---|
| 969 | model(4).B = vec("0"); |
---|
| 970 | model(4).rv = RV("yr", 1, 0); |
---|
| 971 | model(4).rv_ret = RV("yr", 1, 1); |
---|
| 972 | lq.Models = model; |
---|
| 973 | |
---|
| 974 | Array<quadraticfn> qloss(2); |
---|
| 975 | qloss(0).Q.setCh(mat("1 -1")); |
---|
| 976 | qloss(0).rv = RV("yt", 1, 0); |
---|
[1217] | 977 | qloss(0).rv.add(RV("yr", 1, 0)); |
---|
[1222] | 978 | qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) |
---|
[1209] | 979 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 980 | lq.Losses = qloss; |
---|
| 981 | |
---|
| 982 | lq.finalLoss.Q.setCh(mat("1 -1")); |
---|
| 983 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 984 | lq.finalLoss.rv.add(RV("yr", 1, 0)); |
---|
| 985 | |
---|
| 986 | lq.validate(); |
---|
| 987 | |
---|
| 988 | lq.resetTime(); |
---|
| 989 | lq.redesign(); |
---|
| 990 | |
---|
[1222] | 991 | for(int k = K-1; k > 0; k--){ |
---|
[1209] | 992 | lq.redesign(); |
---|
| 993 | } |
---|
| 994 | |
---|
[1222] | 995 | mat resref("-69.4086 47.2578 -0.2701 23.3758 0.0"); |
---|
| 996 | CHECK_CLOSE_EX(resref, lq.getL(), 0.1); |
---|
[1209] | 997 | } |
---|
| 998 | |
---|
[1217] | 999 | TEST (LQGU_Filatov){ |
---|
| 1000 | /* |
---|
| 1001 | system from: |
---|
| 1002 | Adaptive Predictive Control Policy for Nonlinear Stochastic Systems |
---|
| 1003 | (N. M. Filatov, H. Unbehauen) |
---|
| 1004 | VII. Simulations |
---|
| 1005 | |
---|
| 1006 | q(k+1) = 1.654q(k) - 1.022q(k-1) + 0.2019q(k-2) |
---|
| 1007 | +0.1098u(k) + 0.0792u(k-1) - 0.0229u(k-2) |
---|
| 1008 | J(k) = (w(k+1) - q(k+1))^2 + 0.0005(u(k))^2 |
---|
| 1009 | w(k) == 5 |
---|
| 1010 | K = 50 |
---|
| 1011 | q(>=0) = 0.1; |
---|
| 1012 | |
---|
| 1013 | ! used without noise |
---|
| 1014 | */ |
---|
| 1015 | |
---|
| 1016 | int K = 50; |
---|
[1222] | 1017 | |
---|
[1217] | 1018 | LQG_universal lq; |
---|
| 1019 | lq.rv = RV("u", 1, 0); |
---|
| 1020 | RV rvc; |
---|
| 1021 | rvc = RV("q", 1, 0); |
---|
| 1022 | rvc.add(RV("q", 1, -1)); |
---|
| 1023 | rvc.add(RV("q", 1, -2)); |
---|
| 1024 | rvc.add(RV("u", 1, -1)); |
---|
| 1025 | rvc.add(RV("u", 1, -2)); |
---|
| 1026 | rvc.add(RV("w", 1, 0)); |
---|
| 1027 | rvc.add(RV("1", 1, 0)); |
---|
| 1028 | lq.set_rvc(rvc); |
---|
| 1029 | lq.horizon = K; |
---|
| 1030 | |
---|
| 1031 | Array<linfnEx> model(7); |
---|
| 1032 | model(0).A = mat("1.654"); |
---|
| 1033 | model(0).B = vec("0"); |
---|
| 1034 | model(0).rv = RV("q", 1, 0); |
---|
| 1035 | model(0).rv_ret = RV("q", 1, 1); |
---|
| 1036 | model(1).A = mat("-1.022"); |
---|
| 1037 | model(1).B = vec("0"); |
---|
| 1038 | model(1).rv = RV("q", 1, -1); |
---|
| 1039 | model(1).rv_ret = RV("q", 1, 1); |
---|
| 1040 | model(2).A = mat("0.2019"); |
---|
| 1041 | model(2).B = vec("0"); |
---|
| 1042 | model(2).rv = RV("q", 1, -2); |
---|
| 1043 | model(2).rv_ret = RV("q", 1, 1); |
---|
| 1044 | model(3).A = mat("0.1098"); |
---|
| 1045 | model(3).B = vec("0"); |
---|
| 1046 | model(3).rv = RV("u", 1, 0); |
---|
| 1047 | model(3).rv_ret = RV("q", 1, 1); |
---|
| 1048 | model(4).A = mat("0.0792"); |
---|
| 1049 | model(4).B = vec("0"); |
---|
| 1050 | model(4).rv = RV("u", 1, -1); |
---|
| 1051 | model(4).rv_ret = RV("q", 1, 1); |
---|
| 1052 | model(5).A = mat("-0.0229"); |
---|
| 1053 | model(5).B = vec("0"); |
---|
| 1054 | model(5).rv = RV("u", 1, -2); |
---|
| 1055 | model(5).rv_ret = RV("q", 1, 1); |
---|
| 1056 | model(6).A = mat("1"); |
---|
| 1057 | model(6).B = vec("0"); |
---|
| 1058 | model(6).rv = RV("w", 1, 0); |
---|
| 1059 | model(6).rv_ret = RV("w", 1, 1); |
---|
| 1060 | lq.Models = model; |
---|
| 1061 | |
---|
| 1062 | Array<quadraticfn> qloss(2); |
---|
| 1063 | qloss(0).Q.setCh(mat("1 -1")); |
---|
| 1064 | qloss(0).rv = RV("q", 1, 1); |
---|
| 1065 | qloss(0).rv.add(RV("w", 1, 1)); |
---|
| 1066 | qloss(1).Q = mat("0.0005");//automatic sqrt |
---|
| 1067 | qloss(1).rv = RV("u", 1, 0); |
---|
| 1068 | lq.Losses = qloss; |
---|
| 1069 | |
---|
| 1070 | lq.finalLoss.Q.setCh(mat("1 -1")); |
---|
| 1071 | lq.finalLoss.rv = RV("q", 1, 1); |
---|
| 1072 | lq.finalLoss.rv.add(RV("w", 1, 1)); |
---|
| 1073 | |
---|
| 1074 | lq.validate(); |
---|
| 1075 | |
---|
| 1076 | lq.resetTime(); |
---|
| 1077 | lq.redesign(); |
---|
| 1078 | |
---|
| 1079 | int k; |
---|
| 1080 | |
---|
| 1081 | for(k = K-1; k > 0; k--) lq.redesign(); |
---|
| 1082 | |
---|
[1222] | 1083 | mat resref("-12.1148 7.9949 -1.6191 -0.6123 0.1836 7.1642 0.0"); |
---|
| 1084 | CHECK_CLOSE_EX(resref, lq.getL(), 0.01); |
---|
[1217] | 1085 | } |
---|
| 1086 | |
---|
[1222] | 1087 | TEST (Matrix_Division){ |
---|
[1217] | 1088 | Uniform_RNG urng; |
---|
| 1089 | |
---|
[1222] | 1090 | mat A("1 1 1 1 1; 0 2 2 2 2; 0 0 3 3 3; 0 0 0 4 4; 0 0 0 0 5"); |
---|
| 1091 | mat B = urng(5,5); |
---|
| 1092 | mat C1, C2; |
---|
[1217] | 1093 | |
---|
[1222] | 1094 | C1 = inv(A)*B; |
---|
| 1095 | ldutg(A,B,C2); |
---|
[1217] | 1096 | |
---|
[1222] | 1097 | CHECK_CLOSE_EX(C1, C2, 0.000001); |
---|
| 1098 | } |
---|