[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 | |
---|
[1205] | 94 | //TEST (quadratic_test){ |
---|
[1157] | 95 | /*quadraticfn qf; |
---|
[1130] | 96 | qf.Q = chmat(2); |
---|
| 97 | qf.Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0"); |
---|
| 98 | CHECK_CLOSE_EX(qf.eval(vec("1 2")), vec_1(1.0), 0.0000000000000001); |
---|
| 99 | |
---|
| 100 | LQG_universal lq; |
---|
| 101 | lq.Losses = Array<quadraticfn>(1); |
---|
| 102 | lq.Losses(0) = quadraticfn(); |
---|
| 103 | lq.Losses(0).Q._Ch() = mat("1 -1 0; 0 0 0; 0 0 0"); |
---|
| 104 | lq.Losses(0).rv = RV("{u up }"); |
---|
| 105 | |
---|
[1157] | 106 | lq.Models = Array<linfnEx>(1); |
---|
| 107 | lq.Models(0) = linfnEx(mat("1"),vec("1")); |
---|
[1130] | 108 | lq.Models(0).rv = RV("{x }"); |
---|
| 109 | |
---|
| 110 | lq.rv = RV("u",1); |
---|
| 111 | |
---|
| 112 | lq.redesign(); |
---|
[1157] | 113 | CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001);*/ |
---|
[1205] | 114 | //} |
---|
[1157] | 115 | |
---|
[1194] | 116 | TEST (LQGU_static_vs_Riccati_test){ |
---|
[1157] | 117 | //test of universal LQG controller |
---|
| 118 | LQG_universal lq; |
---|
| 119 | lq.rv = RV("u", 2, 0); |
---|
| 120 | lq.set_rvc(RV("x", 2, 0)); |
---|
| 121 | lq.horizon = 10; |
---|
| 122 | |
---|
| 123 | /* |
---|
| 124 | model: x = Ax + Bu time: 0..horizon |
---|
| 125 | loss: l = x'Q'Qx + u'R'Ru time: 0..horizon-1 |
---|
| 126 | final loss: l = x'S'Sx time: horizon |
---|
| 127 | |
---|
| 128 | dim: x: 2 |
---|
| 129 | u: 2 |
---|
| 130 | |
---|
| 131 | A = [ 2 -1 ] |
---|
| 132 | [ 0 0.5 ] |
---|
| 133 | |
---|
| 134 | B = [ 1 -0.1 ] |
---|
| 135 | [ -0.2 2 ] |
---|
| 136 | |
---|
| 137 | Q = [ 5 0 ] |
---|
| 138 | [ 0 1 ] |
---|
| 139 | |
---|
| 140 | R = [ 0.01 0 ] |
---|
| 141 | [ 0 0.1 ] |
---|
| 142 | |
---|
| 143 | S = Q |
---|
| 144 | */ |
---|
| 145 | |
---|
[1194] | 146 | mat mA("2 -1;0 0.5"); |
---|
| 147 | mat mB("1 -0.1;-0.2 2"); |
---|
| 148 | mat mQ("5 0;0 1"); |
---|
| 149 | mat mR("0.01 0;0 0.1"); |
---|
| 150 | mat mS = mQ; |
---|
[1157] | 151 | |
---|
| 152 | //starting configuration |
---|
[1194] | 153 | vec x0("6 3"); |
---|
[1157] | 154 | |
---|
| 155 | /*cout << "configuration:" << endl |
---|
| 156 | << "mA:" << endl << mA << endl |
---|
| 157 | << "mB:" << endl << mB << endl |
---|
| 158 | << "mQ:" << endl << mQ << endl |
---|
| 159 | << "mR:" << endl << mR << endl |
---|
| 160 | << "mS:" << endl << mS << endl |
---|
| 161 | << "x0:" << endl << x0 << endl;*/ |
---|
| 162 | |
---|
| 163 | //model |
---|
| 164 | Array<linfnEx> model(2); |
---|
| 165 | //model Ax part |
---|
| 166 | model(0).A = mA; |
---|
| 167 | model(0).B = vec("0 0"); |
---|
| 168 | model(0).rv = RV("x", 2, 0); |
---|
| 169 | model(0).rv_ret = RV("x", 2, 1); |
---|
| 170 | //model Bu part |
---|
| 171 | model(1).A = mB; |
---|
| 172 | model(1).B = vec("0 0"); |
---|
| 173 | model(1).rv = RV("u", 2, 0); |
---|
| 174 | model(1).rv_ret = RV("x", 2, 1); |
---|
| 175 | //setup |
---|
| 176 | lq.Models = model; |
---|
| 177 | |
---|
| 178 | //loss |
---|
| 179 | Array<quadraticfn> loss(2); |
---|
| 180 | //loss x'Qx part |
---|
| 181 | loss(0).Q.setCh(mQ); |
---|
[1205] | 182 | loss(0).rv = RV("x", 2, 1); |
---|
[1157] | 183 | //loss u'Ru part |
---|
| 184 | loss(1).Q.setCh(mR); |
---|
| 185 | loss(1).rv = RV("u", 2, 0); |
---|
| 186 | //setup |
---|
| 187 | lq.Losses = loss; |
---|
| 188 | |
---|
| 189 | //finalloss setup |
---|
| 190 | lq.finalLoss.Q.setCh(mS); |
---|
| 191 | lq.finalLoss.rv = RV("x", 2, 1); |
---|
[1194] | 192 | |
---|
| 193 | lq.validate(); |
---|
[1157] | 194 | |
---|
| 195 | //default L |
---|
| 196 | //cout << "default L matrix:" << endl << lq.getL() << endl; |
---|
| 197 | |
---|
| 198 | //produce last control matrix L |
---|
| 199 | lq.redesign(); |
---|
| 200 | |
---|
| 201 | //verification via Riccati LQG version |
---|
| 202 | mat mK = mS; |
---|
| 203 | mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 204 | |
---|
| 205 | //cout << "L matrix LQG_universal:" << endl << lq.getL() << endl << |
---|
| 206 | //"L matrix LQG Riccati:" << endl << mL << endl; |
---|
| 207 | |
---|
| 208 | //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion |
---|
| 209 | //more important is reached loss compared in the next part |
---|
| 210 | double tolerr = 1;//0.01; //0.01 OK x 0.001 NO OK |
---|
| 211 | |
---|
| 212 | //check last time L matrix |
---|
| 213 | CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr); |
---|
| 214 | |
---|
| 215 | mat oldK; |
---|
| 216 | int i; |
---|
| 217 | |
---|
| 218 | //produce next control matrix L |
---|
| 219 | for(i = 0; i < lq.horizon - 1; i++) { |
---|
| 220 | lq.redesign(); |
---|
| 221 | oldK = mK; |
---|
| 222 | mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ; |
---|
| 223 | mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 224 | |
---|
| 225 | //cout << "L matrix LQG_universal:" << endl << lq.getL() << endl << |
---|
| 226 | //"L matrix LQG Riccati:" << endl << mL << endl; |
---|
| 227 | |
---|
| 228 | //check other times L matrix |
---|
| 229 | CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr); |
---|
| 230 | } |
---|
| 231 | |
---|
| 232 | //check losses of LQG control - compare LQG_universal and Riccati version, no noise |
---|
| 233 | |
---|
| 234 | //loss of LQG_universal |
---|
| 235 | /*double*/vec loss_uni("0"); |
---|
| 236 | |
---|
| 237 | //setup |
---|
| 238 | vec x = x0; |
---|
| 239 | vec xold = x; |
---|
| 240 | vec u; |
---|
| 241 | //vec tmploss; |
---|
| 242 | |
---|
| 243 | //iteration |
---|
| 244 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 245 | u = lq.getL().get_cols(0,1) * xold; |
---|
| 246 | x = mA * xold + mB * u; |
---|
| 247 | /*tmploss*/loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 248 | //loss_uni += tmploss.get(0); |
---|
| 249 | xold = x; |
---|
| 250 | } |
---|
| 251 | /*tmploss*/ loss_uni = x.transpose() * mS * x; |
---|
| 252 | //loss_uni += tmploss.get(0); |
---|
| 253 | |
---|
| 254 | //loss of LQG Riccati version |
---|
| 255 | /*double*/ vec loss_rct("0"); |
---|
| 256 | |
---|
| 257 | //setup |
---|
| 258 | x = x0; |
---|
| 259 | xold = x; |
---|
| 260 | |
---|
| 261 | //iteration |
---|
| 262 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 263 | u = mL * xold; |
---|
| 264 | x = mA * xold + mB * u; |
---|
| 265 | /*tmploss*/loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 266 | //loss_rct += tmploss.get(0); |
---|
| 267 | xold = x; |
---|
| 268 | } |
---|
| 269 | /*tmploss*/loss_rct = x.transpose() * mS * x; |
---|
| 270 | //loss_rct += tmploss.get(0); |
---|
| 271 | |
---|
| 272 | //cout << "Loss LQG_universal: " << loss_uni << " vs Loss LQG Riccati: " << loss_rct << endl; |
---|
[1194] | 273 | CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001); |
---|
| 274 | } |
---|
| 275 | |
---|
| 276 | TEST (LQGU_random_vs_Riccati_test){ |
---|
| 277 | |
---|
| 278 | //uniform random generator |
---|
| 279 | Uniform_RNG urng; |
---|
| 280 | double maxmult = 10.0; |
---|
| 281 | int maxxsize = 5; |
---|
| 282 | int maxusize = 5; |
---|
| 283 | |
---|
| 284 | urng.setup(2.0, maxxsize); |
---|
[1206] | 285 | int matxsize = round_i(urng()); |
---|
[1194] | 286 | urng.setup(2.0, maxusize); |
---|
[1206] | 287 | int matusize = round_i(urng()); |
---|
[1194] | 288 | |
---|
| 289 | urng.setup(-maxmult, maxmult); |
---|
| 290 | |
---|
| 291 | mat tmpmat; |
---|
| 292 | |
---|
| 293 | mat mA; |
---|
| 294 | mA = urng(matxsize, matxsize); |
---|
| 295 | mat mB; |
---|
| 296 | mB = urng(matxsize, matusize); |
---|
| 297 | mat mQ; |
---|
| 298 | tmpmat = urng(matxsize, matxsize); |
---|
| 299 | mQ = tmpmat*tmpmat.transpose(); |
---|
| 300 | mat mR; |
---|
| 301 | tmpmat = urng(matusize, matusize); |
---|
| 302 | mR = tmpmat*tmpmat.transpose(); |
---|
| 303 | mat mS; |
---|
| 304 | tmpmat = urng(matxsize, matxsize); |
---|
| 305 | mS = tmpmat*tmpmat.transpose(); |
---|
| 306 | |
---|
| 307 | //starting configuration |
---|
| 308 | vec x0; |
---|
| 309 | x0 = urng(matxsize); |
---|
| 310 | |
---|
| 311 | /*cout << "configuration:" << endl |
---|
| 312 | << "x size " << matxsize << " u size " << matusize << endl |
---|
| 313 | << "mA:" << endl << mA << endl |
---|
| 314 | << "mB:" << endl << mB << endl |
---|
| 315 | << "mQ:" << endl << mQ << endl |
---|
| 316 | << "mR:" << endl << mR << endl |
---|
| 317 | << "mS:" << endl << mS << endl |
---|
| 318 | << "x0:" << endl << x0 << endl;*/ |
---|
| 319 | |
---|
| 320 | vec zerovecx(matxsize); |
---|
| 321 | zerovecx.zeros(); |
---|
| 322 | vec zerovecu(matusize); |
---|
| 323 | zerovecu.zeros(); |
---|
| 324 | |
---|
| 325 | //test of universal LQG controller |
---|
| 326 | LQG_universal lq; |
---|
| 327 | lq.rv = RV("u", matusize, 0); |
---|
| 328 | lq.set_rvc(RV("x", matxsize, 0)); |
---|
| 329 | lq.horizon = 10; |
---|
| 330 | |
---|
| 331 | //model |
---|
| 332 | Array<linfnEx> model(2); |
---|
| 333 | //model Ax part |
---|
| 334 | model(0).A = mA; |
---|
| 335 | model(0).B = zerovecx; |
---|
| 336 | model(0).rv = RV("x", matxsize, 0); |
---|
| 337 | model(0).rv_ret = RV("x", matxsize, 1); |
---|
| 338 | //model Bu part |
---|
| 339 | model(1).A = mB; |
---|
| 340 | model(1).B = zerovecu; |
---|
| 341 | model(1).rv = RV("u", matusize, 0); |
---|
| 342 | model(1).rv_ret = RV("x", matxsize, 1); |
---|
| 343 | //setup |
---|
| 344 | lq.Models = model; |
---|
| 345 | |
---|
| 346 | //loss |
---|
| 347 | Array<quadraticfn> loss(2); |
---|
| 348 | //loss x'Qx part |
---|
| 349 | loss(0).Q.setCh(mQ); |
---|
[1205] | 350 | loss(0).rv = RV("x", matxsize, 1); |
---|
[1194] | 351 | //loss u'Ru part |
---|
| 352 | loss(1).Q.setCh(mR); |
---|
| 353 | loss(1).rv = RV("u", matusize, 0); |
---|
| 354 | //setup |
---|
| 355 | lq.Losses = loss; |
---|
| 356 | |
---|
| 357 | //finalloss setup |
---|
| 358 | lq.finalLoss.Q.setCh(mS); |
---|
| 359 | lq.finalLoss.rv = RV("x", matxsize, 1); |
---|
| 360 | |
---|
| 361 | lq.validate(); |
---|
| 362 | |
---|
| 363 | //produce last control matrix L |
---|
| 364 | lq.redesign(); |
---|
| 365 | |
---|
| 366 | //verification via Riccati LQG version |
---|
| 367 | mat mK = mS; |
---|
| 368 | mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 369 | |
---|
| 370 | //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion |
---|
| 371 | //more important is reached loss compared in the next part |
---|
| 372 | double tolerr = 2;//1;//0.01; //0.01 OK x 0.001 NO OK |
---|
| 373 | |
---|
| 374 | //check last time L matrix |
---|
| 375 | CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr); |
---|
| 376 | |
---|
| 377 | mat oldK; |
---|
| 378 | int i; |
---|
| 379 | |
---|
| 380 | //produce next control matrix L |
---|
| 381 | for(i = 0; i < lq.horizon - 1; i++) { |
---|
| 382 | lq.redesign(); |
---|
| 383 | oldK = mK; |
---|
| 384 | mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ; |
---|
| 385 | mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; |
---|
| 386 | |
---|
| 387 | //check other times L matrix |
---|
| 388 | CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr); |
---|
| 389 | } |
---|
| 390 | |
---|
| 391 | //check losses of LQG control - compare LQG_universal and Riccati version, no noise |
---|
| 392 | |
---|
| 393 | //loss of LQG_universal |
---|
| 394 | vec loss_uni("0"); |
---|
| 395 | |
---|
| 396 | //setup |
---|
| 397 | vec x = x0; |
---|
| 398 | vec xold = x; |
---|
| 399 | vec u; |
---|
| 400 | //vec tmploss; |
---|
| 401 | |
---|
| 402 | //iteration |
---|
| 403 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 404 | u = lq.getL().get_cols(0,(matxsize-1)) * xold; |
---|
| 405 | x = mA * xold + mB * u; |
---|
| 406 | loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 407 | xold = x; |
---|
| 408 | } |
---|
| 409 | |
---|
| 410 | loss_uni = x.transpose() * mS * x; |
---|
| 411 | |
---|
| 412 | //loss of LQG Riccati version |
---|
| 413 | vec loss_rct("0"); |
---|
| 414 | |
---|
| 415 | //setup |
---|
| 416 | x = x0; |
---|
| 417 | xold = x; |
---|
| 418 | |
---|
| 419 | //iteration |
---|
| 420 | for(i = 0; i < lq.horizon - 1; i++){ |
---|
| 421 | u = mL * xold; |
---|
| 422 | x = mA * xold + mB * u; |
---|
| 423 | loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u; |
---|
| 424 | xold = x; |
---|
| 425 | } |
---|
| 426 | loss_rct = x.transpose() * mS * x; |
---|
| 427 | |
---|
| 428 | CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001); |
---|
| 429 | } |
---|
| 430 | |
---|
| 431 | TEST (LQGU_SiSy_test){ |
---|
| 432 | int K = 5; |
---|
| 433 | int N = 100; |
---|
| 434 | double sigma = 0.1; |
---|
| 435 | double yr = 1; |
---|
| 436 | double y0 = 0; |
---|
| 437 | double x0 = y0 - yr; |
---|
| 438 | double Eb = 1; |
---|
| 439 | double P; |
---|
| 440 | //double P = 0.01; //loss ~ 1.0??? e-2 |
---|
| 441 | //double P = 0.1; //loss ~ 1.??? e-1 |
---|
| 442 | //double P = 1; //loss ~ ???.??? e+2 |
---|
| 443 | //double P = 10; //loss ~ ?e+6 |
---|
| 444 | |
---|
| 445 | mat A("1"); |
---|
| 446 | mat B(1,1); |
---|
| 447 | B(0,0) = Eb; |
---|
| 448 | mat Q("1"); |
---|
| 449 | mat R("0"); |
---|
| 450 | |
---|
| 451 | vec u(K); |
---|
| 452 | u.zeros(); |
---|
| 453 | mat xn(K, N); |
---|
| 454 | xn.zeros(); |
---|
| 455 | vec S(K); |
---|
| 456 | S.zeros(); |
---|
| 457 | vec L(K); |
---|
| 458 | L.zeros(); |
---|
| 459 | |
---|
| 460 | vec loss(N); |
---|
| 461 | loss.zeros(); |
---|
| 462 | |
---|
| 463 | LQG_universal lq; |
---|
| 464 | lq.rv = RV("u", 1, 0); |
---|
| 465 | lq.set_rvc(RV("x", 1, 0)); |
---|
| 466 | lq.horizon = K; |
---|
| 467 | |
---|
| 468 | Array<linfnEx> model(2); |
---|
| 469 | model(0).A = A; |
---|
| 470 | model(0).B = vec("0 0"); |
---|
| 471 | model(0).rv = RV("x", 1, 0); |
---|
| 472 | model(0).rv_ret = RV("x", 1, 1); |
---|
| 473 | model(1).A = B; |
---|
| 474 | model(1).B = vec("0 0"); |
---|
| 475 | model(1).rv = RV("u", 1, 0); |
---|
| 476 | model(1).rv_ret = RV("x", 1, 1); |
---|
| 477 | lq.Models = model; |
---|
| 478 | |
---|
| 479 | Array<quadraticfn> qloss(2); |
---|
| 480 | qloss(0).Q.setCh(Q); |
---|
[1205] | 481 | qloss(0).rv = RV("x", 1, 1); |
---|
[1194] | 482 | qloss(1).Q.setCh(R); |
---|
| 483 | qloss(1).rv = RV("u", 1, 0); |
---|
| 484 | lq.Losses = qloss; |
---|
| 485 | |
---|
| 486 | lq.finalLoss.Q.setCh(Q); |
---|
| 487 | lq.finalLoss.rv = RV("x", 1, 1); |
---|
| 488 | |
---|
| 489 | lq.validate(); |
---|
| 490 | |
---|
| 491 | int n, k; |
---|
| 492 | double Br; |
---|
| 493 | //double x00; |
---|
| 494 | |
---|
| 495 | for(k = K-1; k >= 0;k--){ |
---|
| 496 | lq.redesign(); |
---|
| 497 | L(k) = (lq.getL())(0,0); |
---|
| 498 | } |
---|
| 499 | |
---|
| 500 | for(P = 0.01; P <= 10; (P*=10)){ |
---|
| 501 | lq.resetTime(); |
---|
| 502 | |
---|
| 503 | //cout << "Ls " << L << endl; |
---|
| 504 | for(n = 0; n < N; n++){ |
---|
| 505 | Br = Eb + sqrt(P)*randn(); |
---|
| 506 | |
---|
| 507 | xn(0, n) = x0 + sigma*randn(); |
---|
| 508 | for(k = 0; k < K-1; k++){ |
---|
| 509 | u(k) = L(k)*xn(k, n); |
---|
| 510 | xn(k+1, n) = (A*xn(k, n))(0,0) + Br*u(k) + sigma*randn(); |
---|
| 511 | } |
---|
| 512 | |
---|
| 513 | loss(n) = 0; |
---|
| 514 | for(k=0;k<K;k++) loss(n) += xn(k, n)*xn(k, n); |
---|
| 515 | |
---|
| 516 | } |
---|
| 517 | |
---|
| 518 | /*vec mtraj(K); |
---|
| 519 | for(k=0;k<K;k++){ |
---|
| 520 | mtraj(k) = 0; |
---|
| 521 | for(n = 0; n < N; n++) mtraj(k) += xn(k, n); |
---|
| 522 | mtraj(k) /= N; |
---|
| 523 | mtraj(k) += yr; |
---|
| 524 | } |
---|
| 525 | cout << "prum trajek " << mtraj << endl;*/ |
---|
| 526 | //cout << "prum ztrata " << sum(loss)/N << endl; |
---|
| 527 | //rfloss = mean(loss); |
---|
| 528 | //rftraj = (mean((xn(1:K, :)), 2) + yr*ones(K, 1))'; |
---|
| 529 | |
---|
| 530 | double tolerr; |
---|
| 531 | //double P = 0.01; //loss ~ 1.0??? e-2 |
---|
| 532 | //double P = 0.1; //loss ~ 1.??? e-1 |
---|
| 533 | //double P = 1; //loss ~ ???.??? e+2 |
---|
| 534 | //double P = 10; //loss ~ ?e+6 |
---|
| 535 | if(P == 0.01) tolerr = 0.2; |
---|
| 536 | else if(P == 0.1) tolerr = 2; |
---|
| 537 | else if(P == 1) tolerr = 2000; |
---|
| 538 | else if(P == 10) tolerr = 2e+7; |
---|
| 539 | else tolerr = 0; |
---|
| 540 | |
---|
| 541 | CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr); |
---|
| 542 | } |
---|
| 543 | } |
---|
| 544 | |
---|
| 545 | TEST (LQGU_SiSy_SuSt_test){ |
---|
| 546 | int K = 5; |
---|
| 547 | int N = 100; |
---|
| 548 | double sigma = 0.1; |
---|
| 549 | double yr = 1; |
---|
| 550 | double y0 = 0; |
---|
| 551 | double Eb = 1; |
---|
| 552 | double inP; |
---|
| 553 | //double inP = 0.01; //loss ~ 1.0??? e-2 |
---|
| 554 | //double inP = 0.1; //loss ~ 1.0??? e-2 |
---|
| 555 | //double inP = 1; //loss ~ 1.0??? e-2 |
---|
| 556 | //double inP = 10; //loss ~ 1.0??? e-2 |
---|
| 557 | |
---|
| 558 | vec x0(3); |
---|
| 559 | x0(0) = y0 - yr; |
---|
| 560 | x0(1) = Eb; |
---|
| 561 | //x0(2) = inP; |
---|
| 562 | |
---|
| 563 | mat A("1 0 0; 0 1 0; 0 0 1"); |
---|
| 564 | mat B("0; 0; 0"); |
---|
| 565 | mat X("1 0 0; 0 0 0; 0 0 0"); |
---|
| 566 | mat Y("0.00001"); |
---|
| 567 | mat Q(3,3); |
---|
| 568 | Q.zeros(); |
---|
| 569 | Q(1,1) = sigma*sigma; |
---|
| 570 | //mat R("0"); |
---|
| 571 | //mat P(3,3); |
---|
| 572 | // P.zeros(); |
---|
| 573 | |
---|
| 574 | vec u(K); |
---|
| 575 | u.zeros(); |
---|
| 576 | vec Kk(K); |
---|
| 577 | Kk.zeros(); |
---|
| 578 | mat xn0(K+1, N); |
---|
| 579 | xn0.zeros(); |
---|
| 580 | mat xn1(K+1, N); |
---|
| 581 | xn1.zeros(); |
---|
| 582 | mat xn2(K+1, N); |
---|
| 583 | xn2.zeros(); |
---|
| 584 | //vec S(K); |
---|
| 585 | // S.zeros(); |
---|
| 586 | //vec L(K); |
---|
| 587 | // L.zeros(); |
---|
| 588 | mat L(1, 3); |
---|
| 589 | L.zeros(); |
---|
| 590 | vec loss(N); |
---|
| 591 | loss.zeros(); |
---|
| 592 | |
---|
| 593 | LQG_universal lq; |
---|
| 594 | lq.rv = RV("u", 1, 0); |
---|
| 595 | lq.set_rvc(RV("x", 3, 0)); |
---|
| 596 | lq.horizon = K; |
---|
| 597 | |
---|
| 598 | Array<linfnEx> model(2); |
---|
| 599 | model(0).A = A; |
---|
| 600 | model(0).B = vec("0 0"); |
---|
| 601 | model(0).rv = RV("x", 3, 0); |
---|
| 602 | model(0).rv_ret = RV("x", 3, 1); |
---|
| 603 | model(1).A = B; |
---|
| 604 | model(1).B = vec("0 0"); |
---|
| 605 | model(1).rv = RV("u", 1, 0); |
---|
| 606 | model(1).rv_ret = RV("x", 3, 1); |
---|
| 607 | lq.Models = model; |
---|
| 608 | |
---|
| 609 | Array<quadraticfn> qloss(2); |
---|
| 610 | qloss(0).Q.setCh(X); |
---|
[1205] | 611 | qloss(0).rv = RV("x", 3, 1); |
---|
[1194] | 612 | qloss(1).Q.setCh(Y); |
---|
| 613 | qloss(1).rv = RV("u", 1, 0); |
---|
| 614 | lq.Losses = qloss; |
---|
| 615 | |
---|
| 616 | lq.finalLoss.Q.setCh(X); |
---|
| 617 | lq.finalLoss.rv = RV("x", 3, 1); |
---|
| 618 | |
---|
| 619 | lq.validate(); |
---|
| 620 | |
---|
| 621 | int n, k; |
---|
| 622 | //double Br; |
---|
| 623 | //double x00; |
---|
| 624 | |
---|
| 625 | /*for(k = K-1; k >= 0;k--){ |
---|
| 626 | lq.redesign(); |
---|
| 627 | L(k) = (lq.getL())(0,0); |
---|
| 628 | }*/ |
---|
| 629 | //cout << "Ls " << L << endl; |
---|
| 630 | for(inP = 0.01; inP <= 10; (inP*=10)){ |
---|
| 631 | lq.resetTime(); |
---|
| 632 | x0(2) = inP; |
---|
| 633 | for(n = 0; n < N; n++){ |
---|
| 634 | L.zeros(); |
---|
| 635 | xn0(0, n) = x0(0); |
---|
| 636 | xn1(0, n) = x0(1) + sqrt(inP) * randn(); |
---|
| 637 | xn2(0, n) = x0(2); |
---|
| 638 | //^neznalost, sum zatim ne |
---|
| 639 | |
---|
| 640 | for(k = 0; k < K-1; k++){ |
---|
| 641 | u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n); |
---|
| 642 | Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 643 | xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn(); |
---|
| 644 | xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)); |
---|
| 645 | xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n); |
---|
| 646 | } |
---|
| 647 | |
---|
| 648 | lq.resetTime(); |
---|
| 649 | lq.redesign(); |
---|
| 650 | for(k = K-1; k > 0; k--){ |
---|
| 651 | A(0, 1) = u(k); |
---|
| 652 | A(1, 0) = -Kk(k); |
---|
| 653 | A(1, 1) = 1-Kk(k)*u(k); |
---|
| 654 | 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); |
---|
| 655 | 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); |
---|
| 656 | B(0) = xn1(k, n); |
---|
| 657 | 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); |
---|
| 658 | B(2) = -2*u(k)*xn2(k, n)*xn2(k, n)*sigma*sigma / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 659 | lq.Models(0).A = A; |
---|
| 660 | lq.Models(1).A = B; |
---|
| 661 | lq.redesign(); |
---|
| 662 | } |
---|
| 663 | L = lq.getL(); |
---|
| 664 | |
---|
| 665 | //simulation |
---|
| 666 | xn0(0, n) += sigma*randn(); |
---|
| 667 | for(k = 0; k < K-1; k++){ |
---|
| 668 | u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n); |
---|
| 669 | Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma); |
---|
| 670 | xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn(); |
---|
| 671 | xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k)); |
---|
| 672 | xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n); |
---|
| 673 | } |
---|
| 674 | |
---|
| 675 | |
---|
| 676 | loss(n) = 0; |
---|
| 677 | for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n); |
---|
| 678 | |
---|
| 679 | } |
---|
| 680 | |
---|
| 681 | /*vec mtraj(K); |
---|
| 682 | for(k=0;k<K;k++){ |
---|
| 683 | mtraj(k) = 0; |
---|
| 684 | for(n = 0; n < N; n++) mtraj(k) += xn0(k, n); |
---|
| 685 | mtraj(k) /= N; |
---|
| 686 | mtraj(k) += yr; |
---|
| 687 | } |
---|
| 688 | cout << "prum trajek " << mtraj << endl;*/ |
---|
| 689 | //cout << "prum ztrata " << sum(loss)/N << endl; |
---|
| 690 | |
---|
| 691 | double tolerr = 0.2; |
---|
| 692 | /*if(inP == 0.01) tolerr = 0.2; |
---|
| 693 | else if(inP == 0.1) tolerr = 2; |
---|
| 694 | else if(inP == 1) tolerr = 2000; |
---|
| 695 | else if(inP == 10) tolerr = 2e+7; |
---|
| 696 | else tolerr = 0;*/ |
---|
| 697 | |
---|
| 698 | CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr); |
---|
| 699 | } |
---|
| 700 | } |
---|
| 701 | |
---|
[1209] | 702 | TEST (LQGU_PMSM_test){ |
---|
[1205] | 703 | |
---|
[1215] | 704 | int K = 5; |
---|
| 705 | int Kt = 200; |
---|
[1209] | 706 | int N = 15; |
---|
[1194] | 707 | |
---|
[1205] | 708 | double a = 0.9898; |
---|
| 709 | double b = 0.0072; |
---|
| 710 | double c = 0.0361; |
---|
| 711 | double d = 1; |
---|
| 712 | double e = 0.0149; |
---|
| 713 | |
---|
[1215] | 714 | double OMEGAt = 11.5; |
---|
[1205] | 715 | double DELTAt = 0.000125; |
---|
| 716 | |
---|
| 717 | double v = 0.0001; |
---|
| 718 | double w = 1; |
---|
| 719 | |
---|
| 720 | mat A(5,5); |
---|
| 721 | A.zeros(); |
---|
| 722 | A(0,0) = a; |
---|
| 723 | A(1,1) = a; |
---|
| 724 | A(2,2) = d; |
---|
| 725 | A(3,3) = 1.0; |
---|
| 726 | A(4,4) = 1.0; |
---|
| 727 | A(2,4) = d-1.0; |
---|
| 728 | A(3,2) = DELTAt; |
---|
| 729 | A(3,4) = DELTAt; |
---|
| 730 | |
---|
| 731 | mat B(5,2); |
---|
| 732 | B.zeros(); |
---|
| 733 | B(0,0) = c; |
---|
| 734 | B(1,1) = c; |
---|
| 735 | |
---|
| 736 | mat C(2,5); |
---|
| 737 | C.zeros(); |
---|
| 738 | C(0,0) = c; |
---|
| 739 | C(1,1) = c; |
---|
| 740 | |
---|
| 741 | mat X(5,5); |
---|
| 742 | X.zeros(); |
---|
| 743 | X(2,2) = w; |
---|
| 744 | |
---|
| 745 | mat Y(2,2); |
---|
| 746 | Y.zeros(); |
---|
| 747 | Y(0,0) = v; |
---|
| 748 | Y(1,1) = v; |
---|
| 749 | |
---|
| 750 | mat Q(5,5); |
---|
| 751 | Q.zeros(); |
---|
| 752 | Q(0,0) = 0.0013; |
---|
| 753 | Q(1,1) = 0.0013; |
---|
| 754 | Q(2,2) = 5e-6; |
---|
| 755 | Q(3,3) = 1e-10; |
---|
| 756 | |
---|
| 757 | mat R(2,2); |
---|
| 758 | R.zeros(); |
---|
| 759 | R(0,0) = 0.0006; |
---|
| 760 | R(0,0) = 0.0006; |
---|
| 761 | |
---|
| 762 | vec x0(5); |
---|
| 763 | x0.zeros(); |
---|
| 764 | x0(2) = 1.0-OMEGAt; |
---|
| 765 | x0(3) = itpp::pi / 2; |
---|
| 766 | x0(4) = OMEGAt; |
---|
| 767 | |
---|
| 768 | mat P(5,5); |
---|
| 769 | P.zeros(); |
---|
| 770 | P(0,0) = 0.01; |
---|
| 771 | P(1,1) = 0.01; |
---|
| 772 | P(2,2) = 0.01; |
---|
| 773 | P(3,3) = 0.01; |
---|
[1209] | 774 | |
---|
[1205] | 775 | vec u(2); |
---|
| 776 | |
---|
[1209] | 777 | mat x(5,Kt+K); |
---|
[1205] | 778 | |
---|
| 779 | mat L(2, 5); |
---|
| 780 | mat L0(5, Kt); |
---|
| 781 | mat L1(5, Kt); |
---|
| 782 | |
---|
| 783 | int i,n,k,kt; |
---|
| 784 | vec x00(5); |
---|
| 785 | vec randvec(2); |
---|
| 786 | |
---|
| 787 | LQG_universal lq; |
---|
| 788 | lq.rv = RV("u", 2, 0); |
---|
| 789 | lq.set_rvc(RV("x", 5, 0)); |
---|
| 790 | lq.horizon = K; |
---|
| 791 | |
---|
| 792 | Array<linfnEx> model(2); |
---|
| 793 | model(0).A = A; |
---|
| 794 | model(0).B = vec("0 0 0 0 0"); |
---|
| 795 | model(0).rv = RV("x", 5, 0); |
---|
| 796 | model(0).rv_ret = RV("x", 5, 1); |
---|
| 797 | model(1).A = B; |
---|
| 798 | model(1).B = vec("0 0"); |
---|
| 799 | model(1).rv = RV("u", 2, 0); |
---|
| 800 | model(1).rv_ret = RV("x", 5, 1); |
---|
| 801 | lq.Models = model; |
---|
| 802 | |
---|
| 803 | Array<quadraticfn> qloss(2); |
---|
| 804 | qloss(0).Q.setCh(X); |
---|
| 805 | qloss(0).rv = RV("x", 5, 1); |
---|
| 806 | qloss(1).Q.setCh(Y); |
---|
| 807 | qloss(1).rv = RV("u", 2, 0); |
---|
| 808 | lq.Losses = qloss; |
---|
| 809 | |
---|
| 810 | lq.finalLoss.Q.setCh(X); |
---|
| 811 | lq.finalLoss.rv = RV("x", 5, 1); |
---|
| 812 | |
---|
| 813 | lq.validate(); |
---|
| 814 | |
---|
| 815 | vec losses(N); |
---|
| 816 | losses.zeros(); |
---|
[1215] | 817 | vec omega(Kt+K); |
---|
| 818 | omega.zeros(); |
---|
[1205] | 819 | |
---|
| 820 | for(n = 0; n < N; n++) { |
---|
| 821 | L0.zeros(); |
---|
| 822 | L1.zeros(); |
---|
[1209] | 823 | |
---|
| 824 | for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn(); |
---|
[1205] | 825 | |
---|
| 826 | for(kt = 0; kt < Kt; kt++){ |
---|
| 827 | x.set_col(0, x00); |
---|
[1209] | 828 | |
---|
[1205] | 829 | for(k = 0; k < kt+K-1; k++){ |
---|
| 830 | L.set_size(2,5); |
---|
| 831 | L.set_row(0, L0.get_col(kt)); |
---|
| 832 | L.set_row(1, L1.get_col(kt)); |
---|
| 833 | u = L*x.get_col(k); |
---|
[1209] | 834 | if(u(0) > 50) u(0) = 50; |
---|
| 835 | else if(u(0) < -50) u(0) = -50; |
---|
| 836 | if(u(1) > 50) u(1) = 50; |
---|
| 837 | else if(u(1) < -50) u(1) = -50; |
---|
| 838 | |
---|
| 839 | 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(); |
---|
| 840 | 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(); |
---|
| 841 | 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(); |
---|
| 842 | x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt;// + sqrt(Q(3, 3))*randn(); |
---|
| 843 | x(4, k+1) = x(4, k); |
---|
[1205] | 844 | } |
---|
| 845 | |
---|
| 846 | lq.resetTime(); |
---|
| 847 | lq.redesign(); |
---|
| 848 | for(k = K-1; k > 0; k--){ |
---|
| 849 | A(2, 0) = -e*sin(x(3, k+kt-1)); |
---|
| 850 | A(2, 1) = e*cos(x(3, k+kt-1)); |
---|
| 851 | A(0, 2) = b*sin(x(3, k+kt-1)); |
---|
| 852 | A(1, 2) = -b*cos(x(3, k+kt-1)); |
---|
| 853 | A(0, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*cos(x(3, k+kt-1)); |
---|
| 854 | A(1, 3) = b*(x(2, k+kt-1) + x(4, k+kt-1))*sin(x(3, k+kt-1)); |
---|
| 855 | 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)))); |
---|
| 856 | A(0, 4) = b*sin(x(3, k+kt-1)); |
---|
| 857 | A(1, 4) = -b*cos(x(3, k+kt-1)); |
---|
| 858 | lq.Models(0).A = A; |
---|
| 859 | lq.redesign(); |
---|
| 860 | } |
---|
| 861 | L = lq.getL(); |
---|
| 862 | L0.set_col(kt, L.get_row(0).get(0,4)); |
---|
| 863 | L1.set_col(kt, L.get_row(1).get(0,4)); |
---|
| 864 | } |
---|
[1209] | 865 | |
---|
[1205] | 866 | x.set_col(0, x00); |
---|
[1209] | 867 | |
---|
| 868 | for(k = 0; k < Kt; k++){ |
---|
| 869 | |
---|
| 870 | L.set_size(2,5); |
---|
[1205] | 871 | L.set_row(0, L0.get_col(k)); |
---|
| 872 | L.set_row(1, L1.get_col(k)); |
---|
| 873 | u = L*x.get_col(k); |
---|
[1209] | 874 | if(u(0) > 50) u(0) = 50; |
---|
| 875 | else if(u(0) < -50) u(0) = -50; |
---|
| 876 | if(u(1) > 50) u(1) = 50; |
---|
| 877 | else if(u(1) < -50) u(1) = -50; |
---|
| 878 | |
---|
[1205] | 879 | 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(); |
---|
| 880 | 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(); |
---|
| 881 | 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(); |
---|
| 882 | x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt + sqrt(Q(3, 3))*randn(); |
---|
| 883 | x(4, k+1) = x(4, k); |
---|
[1209] | 884 | |
---|
| 885 | losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);// + (u.transpose() * Y * u)(0); |
---|
[1215] | 886 | } |
---|
| 887 | |
---|
| 888 | omega += x.get_row(2); |
---|
[1209] | 889 | } |
---|
[1205] | 890 | |
---|
[1215] | 891 | cout << "Ztrata: " << sum(losses)/N << endl; |
---|
| 892 | //cout << "Trajektorie:\n" << omega(0,Kt-1)/N << endl; |
---|
| 893 | |
---|
| 894 | ofstream log; |
---|
| 895 | log.open ("log.txt"); |
---|
| 896 | for(k = 0; k < Kt; k++) |
---|
| 897 | log << k << "\t" << omega(k)/N + OMEGAt << endl; |
---|
| 898 | log.close(); |
---|
| 899 | |
---|
[1209] | 900 | } |
---|
[1205] | 901 | |
---|
[1209] | 902 | TEST (LQGU_181_zero){ |
---|
| 903 | /* v ????? .8189 |
---|
| 904 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
[1205] | 905 | |
---|
[1209] | 906 | S = y^2 + u^2/1000 |
---|
[1205] | 907 | |
---|
[1209] | 908 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 909 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 910 | */ |
---|
| 911 | |
---|
| 912 | int K = 150; |
---|
| 913 | |
---|
| 914 | LQG_universal lq; |
---|
| 915 | lq.rv = RV("ut", 1, 0); |
---|
| 916 | RV rvc; |
---|
| 917 | rvc = RV("yt", 1, -1); |
---|
| 918 | rvc.add(RV("yt", 1, -2)); |
---|
| 919 | rvc.add(RV("ut", 1, -1)); |
---|
| 920 | rvc.add(RV("1", 1, 0)); |
---|
| 921 | lq.set_rvc(rvc); |
---|
| 922 | lq.horizon = K; |
---|
| 923 | |
---|
| 924 | Array<linfnEx> model(4); |
---|
| 925 | model(0).A = mat("1.81"); |
---|
| 926 | model(0).B = vec("0"); |
---|
| 927 | model(0).rv = RV("yt", 1, -1); |
---|
| 928 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 929 | model(1).A = mat("-0.8189"); |
---|
| 930 | model(1).B = vec("0"); |
---|
| 931 | model(1).rv = RV("yt", 1, -2); |
---|
| 932 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 933 | model(2).A = mat("0.00438"); |
---|
| 934 | model(2).B = vec("0"); |
---|
| 935 | model(2).rv = RV("ut", 1, 0); |
---|
| 936 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 937 | model(3).A = mat("0.00468"); |
---|
| 938 | model(3).B = vec("0"); |
---|
| 939 | model(3).rv = RV("ut", 1, -1); |
---|
| 940 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 941 | lq.Models = model; |
---|
| 942 | |
---|
| 943 | Array<quadraticfn> qloss(2); |
---|
| 944 | qloss(0).Q.setCh(mat("1")); |
---|
| 945 | qloss(0).rv = RV("yt", 1, 0); |
---|
| 946 | qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) |
---|
| 947 | //qloss(1).Q = mat("0.001");//automatic sqrt |
---|
| 948 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 949 | lq.Losses = qloss; |
---|
| 950 | |
---|
| 951 | lq.finalLoss.Q.setCh(mat("1")); |
---|
| 952 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 953 | |
---|
| 954 | lq.validate(); |
---|
| 955 | |
---|
| 956 | lq.resetTime(); |
---|
| 957 | lq.redesign(); |
---|
| 958 | |
---|
| 959 | for(int k = K-1; k > 0; k--){ |
---|
| 960 | //cout << "L: " << lq.getL() << endl; |
---|
| 961 | lq.redesign(); |
---|
[1205] | 962 | } |
---|
| 963 | |
---|
[1209] | 964 | cout << "L: " << lq.getL() << endl; |
---|
[1194] | 965 | } |
---|
| 966 | |
---|
[1215] | 967 | TEST (LQGU_181_ten){ |
---|
| 968 | /* v ????? .8189 |
---|
| 969 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
| 970 | |
---|
| 971 | S = y^2 + u^2/1000 |
---|
| 972 | |
---|
| 973 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 974 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 975 | */ |
---|
| 976 | |
---|
| 977 | int K = 150; |
---|
| 978 | //yr = 10.0; |
---|
| 979 | |
---|
| 980 | LQG_universal lq; |
---|
| 981 | lq.rv = RV("ut", 1, 0); |
---|
| 982 | RV rvc; |
---|
| 983 | rvc = RV("yt", 1, -1); |
---|
| 984 | rvc.add(RV("yt", 1, -2)); |
---|
| 985 | rvc.add(RV("ut", 1, -1)); |
---|
| 986 | rvc.add(RV("1", 1, 0)); |
---|
| 987 | lq.set_rvc(rvc); |
---|
| 988 | lq.horizon = K; |
---|
| 989 | |
---|
| 990 | Array<linfnEx> model(4); |
---|
| 991 | model(0).A = mat("1.81"); |
---|
| 992 | model(0).B = vec("0"); |
---|
| 993 | model(0).rv = RV("yt", 1, -1); |
---|
| 994 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 995 | model(1).A = mat("-0.8189"); |
---|
| 996 | model(1).B = vec("0"); |
---|
| 997 | model(1).rv = RV("yt", 1, -2); |
---|
| 998 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 999 | model(2).A = mat("0.00438"); |
---|
| 1000 | model(2).B = vec("0"); |
---|
| 1001 | model(2).rv = RV("ut", 1, 0); |
---|
| 1002 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 1003 | model(3).A = mat("0.00468"); |
---|
| 1004 | model(3).B = vec("0"); |
---|
| 1005 | model(3).rv = RV("ut", 1, -1); |
---|
| 1006 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 1007 | lq.Models = model; |
---|
| 1008 | |
---|
| 1009 | Array<quadraticfn> qloss(2); |
---|
| 1010 | qloss(0).Q.setCh(mat("1 -10")); |
---|
| 1011 | qloss(0).rv = RV("yt", 1, 0); |
---|
| 1012 | qloss(0).rv.add(RV("1", 1, 0)); |
---|
| 1013 | qloss(1).Q.setCh(mat("0.03162")); |
---|
| 1014 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 1015 | lq.Losses = qloss; |
---|
| 1016 | |
---|
| 1017 | lq.finalLoss.Q.setCh(mat("0")); |
---|
| 1018 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 1019 | |
---|
| 1020 | lq.validate(); |
---|
| 1021 | |
---|
| 1022 | lq.resetTime(); |
---|
| 1023 | lq.redesign(); |
---|
| 1024 | |
---|
| 1025 | for(int k = K-1; k > 0; k--){ |
---|
| 1026 | //cout << "L: " << lq.getL() << endl; |
---|
| 1027 | lq.redesign(); |
---|
| 1028 | } |
---|
| 1029 | |
---|
| 1030 | cout << "L: " << lq.getL() << endl; |
---|
| 1031 | } |
---|
| 1032 | |
---|
[1209] | 1033 | TEST (LQGU_181_reqv){ |
---|
| 1034 | /* v ????? .8189 |
---|
| 1035 | y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} |
---|
| 1036 | |
---|
| 1037 | S = y^2 + u^2/1000 |
---|
| 1038 | |
---|
| 1039 | y_r = 0 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} |
---|
| 1040 | y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2} -0.2509 u_{t-1} + 233.7581 |
---|
| 1041 | */ |
---|
| 1042 | |
---|
| 1043 | int K = 200; |
---|
| 1044 | |
---|
| 1045 | double yr = 10.0; |
---|
| 1046 | |
---|
| 1047 | LQG_universal lq; |
---|
| 1048 | lq.rv = RV("ut", 1, 0); |
---|
| 1049 | RV rvc; |
---|
| 1050 | rvc = RV("yt", 1, -1); |
---|
| 1051 | rvc.add(RV("yt", 1, -2)); |
---|
| 1052 | rvc.add(RV("ut", 1, -1)); |
---|
| 1053 | rvc.add(RV("yr", 1, 0)); |
---|
| 1054 | rvc.add(RV("1", 1, 0)); |
---|
| 1055 | lq.set_rvc(rvc); |
---|
| 1056 | lq.horizon = K; |
---|
| 1057 | |
---|
| 1058 | Array<linfnEx> model(5); |
---|
| 1059 | model(0).A = mat("1.81"); |
---|
| 1060 | model(0).B = vec("0"); |
---|
| 1061 | model(0).rv = RV("yt", 1, -1); |
---|
| 1062 | model(0).rv_ret = RV("yt", 1, 0); |
---|
| 1063 | model(1).A = mat("-0.8189"); |
---|
| 1064 | model(1).B = vec("0"); |
---|
| 1065 | model(1).rv = RV("yt", 1, -2); |
---|
| 1066 | model(1).rv_ret = RV("yt", 1, 0); |
---|
| 1067 | model(2).A = mat("0.00438"); |
---|
| 1068 | model(2).B = vec("0"); |
---|
| 1069 | model(2).rv = RV("ut", 1, 0); |
---|
| 1070 | model(2).rv_ret = RV("yt", 1, 0); |
---|
| 1071 | model(3).A = mat("0.00468"); |
---|
| 1072 | model(3).B = vec("0"); |
---|
| 1073 | model(3).rv = RV("ut", 1, -1); |
---|
| 1074 | model(3).rv_ret = RV("yt", 1, 0); |
---|
| 1075 | model(4).A = mat("1"); |
---|
| 1076 | model(4).B = vec("0"); |
---|
| 1077 | model(4).rv = RV("yr", 1, 0); |
---|
| 1078 | model(4).rv_ret = RV("yr", 1, 1); |
---|
| 1079 | lq.Models = model; |
---|
| 1080 | |
---|
| 1081 | Array<quadraticfn> qloss(2); |
---|
| 1082 | qloss(0).Q.setCh(mat("1 -1")); |
---|
| 1083 | qloss(0).rv = RV("yt", 1, 0); |
---|
| 1084 | qloss(0).rv.add(RV("yr", 1, 0)); |
---|
| 1085 | qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) |
---|
| 1086 | //qloss(1).Q = mat("0.001");//automatic sqrt |
---|
| 1087 | qloss(1).rv = RV("ut", 1, 0); |
---|
| 1088 | lq.Losses = qloss; |
---|
| 1089 | |
---|
| 1090 | lq.finalLoss.Q.setCh(mat("1 -1")); |
---|
| 1091 | lq.finalLoss.rv = RV("yt", 1, 0); |
---|
| 1092 | lq.finalLoss.rv.add(RV("yr", 1, 0)); |
---|
| 1093 | |
---|
| 1094 | lq.validate(); |
---|
| 1095 | |
---|
| 1096 | lq.resetTime(); |
---|
| 1097 | lq.redesign(); |
---|
| 1098 | |
---|
| 1099 | for(int k = K-1; k > 0; k--){ |
---|
| 1100 | //cout << "L: " << lq.getL() << endl; |
---|
| 1101 | lq.redesign(); |
---|
| 1102 | } |
---|
| 1103 | |
---|
| 1104 | cout << "L: " << lq.getL() << endl; |
---|
| 1105 | } |
---|
| 1106 | |
---|
[1205] | 1107 | //TEST (LQGU_large_test){ |
---|
| 1108 | /* |
---|
| 1109 | loss = /Q1*x(+1) + /Q2*y + /Q3*z + /Q4*u + /Q5*[x(+1); y; 1] + /Q6*[z; u; 1] + /Q7*[x(+1); y; z; u; 1] |
---|
| 1110 | u = rv |
---|
| 1111 | x = rvc |
---|
| 1112 | y = A1*[x; 1] + B1 |
---|
| 1113 | z = A2*[x; u; 1] + B2 |
---|
| 1114 | x(+1) = A3*[x; 1] + B3 |
---|
| 1115 | */ |
---|
| 1116 | //uniform random generator |
---|
| 1117 | /*Uniform_RNG urng; |
---|
| 1118 | int max_size = 10; |
---|
| 1119 | double maxmult = 10.0; |
---|
| 1120 | |
---|
| 1121 | urng.setup(2.0, max_size); |
---|
| 1122 | int xsize = round(urng()); |
---|
| 1123 | int usize = round(urng()); |
---|
| 1124 | int ysize = round(urng()); |
---|
| 1125 | int zsize = round(urng()); |
---|
| 1126 | cout << "CFG: " << xsize << " " << ysize << " " << zsize << " " << usize << " " << endl; |
---|
| 1127 | urng.setup(-maxmult, maxmult); |
---|
| 1128 | mat tmpmat; |
---|
| 1129 | mat A1,A2,A3,Q1,Q2,Q3,Q4,Q5,Q6,Q7; |
---|
| 1130 | vec B1,B2,B3; |
---|
| 1131 | |
---|
| 1132 | A1 = urng(ysize,xsize+1); |
---|
| 1133 | B1 = urng(ysize); |
---|
| 1134 | A2 = urng(zsize,xsize+usize+1); |
---|
| 1135 | B2 = urng(zsize); |
---|
| 1136 | A3 = urng(xsize,xsize+1); |
---|
| 1137 | B3 = urng(xsize); |
---|
| 1138 | |
---|
| 1139 | tmpmat = urng(xsize,xsize); |
---|
| 1140 | Q1 = tmpmat*tmpmat.transpose(); |
---|
| 1141 | tmpmat = urng(ysize,ysize); |
---|
| 1142 | Q2 = tmpmat*tmpmat.transpose(); |
---|
| 1143 | tmpmat = urng(zsize,zsize); |
---|
| 1144 | Q3 = tmpmat*tmpmat.transpose(); |
---|
| 1145 | tmpmat = urng(usize,usize); |
---|
| 1146 | Q4 = tmpmat*tmpmat.transpose(); |
---|
| 1147 | tmpmat = urng(xsize+ysize+1,xsize+ysize+1); |
---|
| 1148 | Q5 = tmpmat*tmpmat.transpose(); |
---|
| 1149 | tmpmat = urng(zsize+usize+1,zsize+usize+1); |
---|
| 1150 | Q6 = tmpmat*tmpmat.transpose(); |
---|
| 1151 | tmpmat = urng(xsize+ysize+zsize+usize+1,xsize+ysize+zsize+usize+1); |
---|
| 1152 | Q7 = tmpmat*tmpmat.transpose(); |
---|
| 1153 | |
---|
| 1154 | |
---|
| 1155 | //starting configuration |
---|
| 1156 | vec x0; |
---|
| 1157 | x0 = urng(xsize); |
---|
| 1158 | |
---|
| 1159 | //test of universal LQG controller |
---|
| 1160 | LQG_universal lq; |
---|
| 1161 | lq.rv = RV("u", usize, 0); |
---|
| 1162 | lq.set_rvc(RV("x", xsize, 0)); |
---|
| 1163 | lq.horizon = 100; |
---|
| 1164 | |
---|
| 1165 | RV tmprv; |
---|
| 1166 | |
---|
| 1167 | //model |
---|
| 1168 | Array<linfnEx> model(3); |
---|
| 1169 | model(0).A = A1; |
---|
| 1170 | model(0).B = B1; |
---|
| 1171 | tmprv = RV("x", xsize, 0); |
---|
| 1172 | tmprv.add(RV("1",1,0)); |
---|
| 1173 | model(0).rv = tmprv; |
---|
| 1174 | model(0).rv_ret = RV("y", ysize, 0); |
---|
| 1175 | model(1).A = A2; |
---|
| 1176 | model(1).B = B2; |
---|
| 1177 | tmprv = RV("x", xsize, 0); |
---|
| 1178 | tmprv.add(RV("u", usize, 0)); |
---|
| 1179 | tmprv.add(RV("1",1,0)); |
---|
| 1180 | model(1).rv = tmprv; |
---|
| 1181 | model(1).rv_ret = RV("z", zsize, 0); |
---|
| 1182 | model(2).A = A3; |
---|
| 1183 | model(2).B = B3; |
---|
| 1184 | tmprv = RV("x", xsize, 0); |
---|
| 1185 | tmprv.add(RV("1",1,0)); |
---|
| 1186 | model(2).rv = tmprv; |
---|
| 1187 | model(2).rv_ret = RV("x", xsize, 1); |
---|
| 1188 | //setup |
---|
| 1189 | lq.Models = model; |
---|
| 1190 | |
---|
| 1191 | //loss |
---|
| 1192 | Array<quadraticfn> loss(7); |
---|
| 1193 | loss(0).Q.setCh(Q1); |
---|
| 1194 | loss(0).rv = RV("x", xsize, 1); |
---|
| 1195 | loss(1).Q.setCh(Q2); |
---|
| 1196 | loss(1).rv = RV("y", ysize, 0); |
---|
| 1197 | loss(2).Q.setCh(Q3); |
---|
| 1198 | loss(2).rv = RV("z", zsize, 0); |
---|
| 1199 | loss(3).Q.setCh(Q4); |
---|
| 1200 | loss(3).rv = RV("u", usize, 0); |
---|
| 1201 | loss(4).Q.setCh(Q5); |
---|
| 1202 | tmprv = RV("x", xsize, 1); |
---|
| 1203 | tmprv.add(RV("y", ysize, 0)); |
---|
| 1204 | tmprv.add(RV("1",1,0)); |
---|
| 1205 | loss(4).rv = tmprv; |
---|
| 1206 | loss(5).Q.setCh(Q6); |
---|
| 1207 | tmprv = RV("z", zsize, 0); |
---|
| 1208 | tmprv.add(RV("u", usize, 0)); |
---|
| 1209 | tmprv.add(RV("1",1,0)); |
---|
| 1210 | loss(5).rv = tmprv; |
---|
| 1211 | loss(6).Q.setCh(Q7); |
---|
| 1212 | tmprv = RV("x", xsize, 1); |
---|
| 1213 | tmprv.add(RV("y", ysize, 0)); |
---|
| 1214 | tmprv.add(RV("z", zsize, 0)); |
---|
| 1215 | tmprv.add(RV("u", usize, 0)); |
---|
| 1216 | tmprv.add(RV("1",1,0)); |
---|
| 1217 | loss(6).rv = tmprv; |
---|
| 1218 | //setup |
---|
| 1219 | lq.Losses = loss; |
---|
| 1220 | |
---|
| 1221 | //finalloss setup |
---|
| 1222 | tmpmat = urng(xsize,xsize); |
---|
| 1223 | lq.finalLoss.Q.setCh(tmpmat*tmpmat.transpose()); |
---|
| 1224 | lq.finalLoss.rv = RV("x", xsize, 1); |
---|
| 1225 | |
---|
| 1226 | lq.validate(); |
---|
| 1227 | |
---|
| 1228 | //produce last control matrix L |
---|
| 1229 | lq.redesign(); |
---|
| 1230 | cout << lq.getL() << endl; |
---|
| 1231 | |
---|
| 1232 | int i; |
---|
| 1233 | for(i = 0; i < lq.horizon - 1; i++) { |
---|
| 1234 | lq.redesign(); |
---|
| 1235 | } |
---|
| 1236 | cout << lq.getL() << endl; |
---|
| 1237 | |
---|
| 1238 | mat K; |
---|
| 1239 | K = A3.get_cols(0, xsize-1).transpose()*Q1.transpose()*Q1*A3.get_cols(0, xsize-1) |
---|
| 1240 | + A1.get_cols(0, xsize-1).transpose()*Q2.transpose()*Q2*A1.get_cols(0, xsize-1) |
---|
| 1241 | + A2.get_cols(0, xsize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(0, xsize-1) |
---|
| 1242 | + A3.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*A3.get_cols(0, xsize-1) |
---|
| 1243 | + A1.get_cols(0, xsize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*A1.get_cols(0, xsize-1) |
---|
| 1244 | + A2.get_cols(0, xsize-1).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(0, xsize-1) |
---|
| 1245 | + A3.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*A3.get_cols(0, xsize-1) |
---|
| 1246 | + A1.get_cols(0, xsize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*A1.get_cols(0, xsize-1) |
---|
| 1247 | + A2.get_cols(0, xsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(0, xsize-1); |
---|
| 1248 | |
---|
| 1249 | mat L; |
---|
| 1250 | L = A2.get_cols(xsize, xsize+usize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize, xsize+usize-1) |
---|
| 1251 | + Q4.transpose()*Q4 |
---|
| 1252 | + A2.get_cols(xsize, xsize+usize-1).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(xsize, xsize+usize-1) |
---|
| 1253 | + Q6.get_cols(zsize, zsize+usize-1).transpose()*Q6.get_cols(zsize, zsize+usize-1) |
---|
| 1254 | + A2.get_cols(xsize, xsize+usize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(xsize, xsize+usize-1) |
---|
| 1255 | + Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1).transpose()*Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1); |
---|
| 1256 | |
---|
| 1257 | double M; |
---|
| 1258 | M = (A3.get_cols(xsize,xsize).transpose()*Q1.transpose()*Q1*A3.get_cols(xsize,xsize))(0,0) |
---|
| 1259 | + (B3.transpose()*Q1.transpose()*Q1*B3)(0) |
---|
| 1260 | + (A1.get_cols(xsize,xsize).transpose()*Q2.transpose()*Q2*A1.get_cols(xsize,xsize))(0,0) |
---|
| 1261 | + (B1.transpose()*Q2.transpose()*Q2*B1)(0) |
---|
| 1262 | + (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize+usize,xsize+usize))(0,0) |
---|
| 1263 | + (B2.transpose()*Q3.transpose()*Q3*B2)(0) |
---|
| 1264 | + (A3.get_cols(xsize,xsize).transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*A3.get_cols(xsize,xsize))(0,0) |
---|
| 1265 | + (B3.transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*B3)(0) |
---|
| 1266 | + (A1.get_cols(xsize,xsize).transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*A1.get_cols(xsize,xsize))(0,0) |
---|
| 1267 | + (B1.transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*B1)(0) |
---|
| 1268 | + (Q5.get_cols(xsize+ysize,xsize+ysize).transpose()*Q5.get_cols(xsize+ysize,xsize+ysize))(0,0) |
---|
| 1269 | + (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*A2.get_cols(xsize+usize,xsize+usize))(0,0) |
---|
| 1270 | + (B2.transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*B2)(0) |
---|
| 1271 | + (Q6.get_cols(zsize+usize,zsize+usize).transpose()*Q6.get_cols(zsize+usize,zsize+usize))(0,0) |
---|
| 1272 | + (A3.get_cols(xsize,xsize).transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*A3.get_cols(xsize,xsize))(0,0) |
---|
| 1273 | + (B3.transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*B3)(0) |
---|
| 1274 | + (A1.get_cols(xsize,xsize).transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*A1.get_cols(xsize,xsize))(0,0) |
---|
| 1275 | + (B1.transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*B1)(0) |
---|
| 1276 | + (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*A2.get_cols(xsize+usize,xsize+usize))(0,0) |
---|
| 1277 | + (B2.transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*B2)(0) |
---|
| 1278 | + (Q7.get_cols(xsize+ysize+zsize+usize,xsize+ysize+zsize+usize).transpose()*Q7.get_cols(xsize+ysize+zsize+usize,xsize+ysize+zsize+usize))(0,0); |
---|
| 1279 | |
---|
| 1280 | |
---|
| 1281 | /*mat res; |
---|
| 1282 | res = -inv(sqrtm(L))*sqrtm(K);*/ |
---|
| 1283 | |
---|
| 1284 | /*cout << -inv(sqrtm(L))*sqrtm(K).get_cols(0,L.rows()).get_rows(0,L.cols()) << endl; |
---|
| 1285 | }*/ |
---|
| 1286 | |
---|
| 1287 | /*TEST (validation_test){ |
---|
[1194] | 1288 | RV rv1("x", 1); |
---|
| 1289 | RV rv2("y", 2); |
---|
| 1290 | RV rv3("z", 3); |
---|
| 1291 | RV rv4("u", 2); |
---|
| 1292 | RV rv5("v", 1); |
---|
| 1293 | RV all; |
---|
| 1294 | all = rv1; |
---|
| 1295 | all.add(rv2); |
---|
| 1296 | all.add(rv3); |
---|
| 1297 | all.add(rv4); |
---|
| 1298 | all.add(rv5); |
---|
| 1299 | all.add(RV("1",1,0)); |
---|
[1205] | 1300 | //cout << "all rv: " << all << endl; |
---|
[1194] | 1301 | |
---|
| 1302 | ivec fy = rv2.findself(all); |
---|
[1205] | 1303 | //cout << "finding y: " << fy << endl; |
---|
[1194] | 1304 | ivec dy = rv2.dataind(all); |
---|
[1205] | 1305 | //cout << "dataind y: " << dy << endl; |
---|
[1194] | 1306 | |
---|
| 1307 | RV rvzyu; |
---|
| 1308 | rvzyu = rv3; |
---|
| 1309 | rvzyu.add(rv2); |
---|
| 1310 | rvzyu.add(rv4); |
---|
| 1311 | fy = rvzyu.findself(all); |
---|
[1205] | 1312 | //cout << "finding zyu: " << fy << endl; |
---|
[1194] | 1313 | dy = rvzyu.dataind(all); |
---|
[1205] | 1314 | //cout << "dataind zyu: " << dy << endl; |
---|
[1194] | 1315 | |
---|
| 1316 | rvzyu.add(RV("1",1,0)); |
---|
| 1317 | fy = rvzyu.findself(all); |
---|
[1205] | 1318 | //cout << "finding zyu1: " << fy << endl; |
---|
[1194] | 1319 | dy = rvzyu.dataind(all); |
---|
[1205] | 1320 | //cout << "dataind zyu1: " << dy << endl; |
---|
[1194] | 1321 | |
---|
| 1322 | rvzyu.add(RV("k",1,0)); |
---|
| 1323 | fy = rvzyu.findself(all); |
---|
[1205] | 1324 | //cout << "finding zyu1 !k!: " << fy << endl; |
---|
[1194] | 1325 | dy = rvzyu.dataind(all); |
---|
[1205] | 1326 | //cout << "dataind zyu1 !k!: " << dy << endl; |
---|
[1194] | 1327 | |
---|
| 1328 | RV emptyrv; |
---|
| 1329 | fy = emptyrv.findself(all); |
---|
[1205] | 1330 | //cout << "finding empty: " << fy << " size " << fy.size() << endl; |
---|
[1194] | 1331 | dy = emptyrv.dataind(all); |
---|
[1205] | 1332 | //cout << "dataind empty: " << dy << " size " << dy.size() << endl; |
---|
[1194] | 1333 | |
---|
| 1334 | LQG_universal lq; |
---|
| 1335 | lq.validate(); |
---|
[1205] | 1336 | }*/ |
---|