root/library/tests/testsuite/LQG_test.cpp @ 1222

Revision 1222, 26.0 kB (checked in by vahalam, 14 years ago)
  • Property svn:eol-style set to native
Line 
1#define BDMLIB
2#include "../mat_checks.h"
3#include "design/lq_ctrl.h"
4
5using namespace bdm;
6
7TEST ( LQG_test ) {
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();
15
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 );
19}
20
21TEST ( to_state_test ) {
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 ) );
30
31    shared_ptr<StateCanonical > Stsp = new StateCanonical;
32    Stsp->connect_mlnorm ( ml );
33
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 );
40}
41
42TEST ( to_state_arx_test ) {
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 ) ) ) );
50
51    shared_ptr<StateFromARX> Stsp = new StateFromARX;
52    RV xrv;
53    RV urv;
54    Stsp->connect_mlnorm ( ml, xrv, urv );
55
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 );
62}
63
64TEST ( arx_LQG_test ) {
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 );
75
76    ml.set_rv ( yr );
77    ml.set_rvc ( rgr );
78    ml.validate();
79
80    shared_ptr<StateFromARX> Stsp = new StateFromARX;
81    RV xrv;
82    RV urv;
83    Stsp->connect_mlnorm ( ml, xrv, urv );
84
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();
89
90    L.redesign();
91    cout << L.to_string() << endl;
92}
93
94TEST (LQGU_static_vs_Riccati_test){
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
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;
129
130  //starting configuration
131  vec x0("6 3");       
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);
152  loss(0).rv = RV("x", 2, 1);
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);
162
163  lq.validate();
164   
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;
171 
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
196  vec loss_uni("0");
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  }
212  loss_uni = x.transpose() * mS * x;
213 
214  //loss of LQG Riccati version
215  vec loss_rct("0");
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;
225        loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;     
226        xold = x;
227  }
228  loss_rct = x.transpose() * mS * x; 
229 
230  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001); 
231}
232
233TEST (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); 
242  int matxsize = round_i(urng());
243  urng.setup(2.0, maxusize); 
244  int matusize = round_i(urng());
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;
266        x0 = urng(matxsize);   
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);
298  loss(0).rv = RV("x", matxsize, 1);
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
320  double tolerr = 2;
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
379TEST (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);
429  qloss(0).rv = RV("x", 1, 1); 
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;
441       
442        for(k = K-1; k >= 0;k--){
443                        lq.redesign();
444                        L(k) = (lq.getL())(0,0);
445                }
446
447for(P = 0.01; P <= 10; (P*=10)){
448        lq.resetTime(); 
449
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
479TEST (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;
495                               
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;
503       
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();
514   
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); 
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;
535
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;
542 
543     lq.finalLoss.Q.setCh(X);
544     lq.finalLoss.rv = RV("x", 3, 1);
545
546     lq.validate(); 
547         
548        int n, k;
549       
550for(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);
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);                                       
593                                }
594
595
596                    loss(n) = 0;
597                                for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n);
598               
599        }       
600   
601        double tolerr = 0.2;   
602       
603        CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
604        }
605}
606
607TEST (LQGU_PMSM_test){
608       
609        int K = 5;
610        int Kt = 200;
611        int N = 15;
612
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
619        double OMEGAt = 11.5;
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;
679       
680        vec u(2);
681       
682        mat x(5,Kt+K); 
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();
722        vec omega(Kt+K);
723                omega.zeros();
724       
725        for(n = 0; n < N; n++) {
726                L0.zeros();
727                L1.zeros();
728                                                   
729                for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();   
730                       
731                for(kt = 0; kt < Kt; kt++){           
732                        x.set_col(0, x00);
733
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);
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           
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;
748                                x(4, k+1) = x(4, k);                           
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                }       
770       
771        x.set_col(0, x00);
772               
773        for(k = 0; k < Kt; k++){ 
774       
775                        L.set_size(2,5);
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);     
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                               
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); 
789       
790                        losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);                     
791                }
792
793                omega += x.get_row(2);
794        }
795
796        cout << "Loss: " << sum(losses)/N << endl;
797}
798
799TEST (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}
802
803                S = y^2 + u^2/1000
804
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);   
843        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)     
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
855        for(int k = K-1; k > 0; k--){           
856                lq.redesign();
857        }
858
859        mat resref("-69.4086   47.2578   -0.2701   0.0");
860        CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
861}
862
863TEST (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;
874       
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
920        for(int k = K-1; k > 0; k--){           
921                lq.redesign();
922        }
923
924        mat resref("-69.4086   47.2578   -0.2701   233.758");
925        CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
926}
927
928TEST (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
938        int K = 200;   
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);
977        qloss(0).rv.add(RV("yr", 1, 0));
978        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)     
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
991        for(int k = K-1; k > 0; k--){           
992                lq.redesign();
993        }
994
995        mat resref("-69.4086   47.2578   -0.2701   23.3758   0.0");
996        CHECK_CLOSE_EX(resref, lq.getL(), 0.1);
997}
998
999TEST (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;
1017       
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       
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);       
1085}
1086
1087TEST (Matrix_Division){ 
1088        Uniform_RNG urng;
1089
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;
1093
1094        C1 = inv(A)*B; 
1095        ldutg(A,B,C2); 
1096               
1097        CHECK_CLOSE_EX(C1, C2, 0.000001);       
1098}
Note: See TracBrowser for help on using the browser.