Changeset 1194

Timestamp:

09/21/10 20:41:46 (14 years ago)

Author:

vahalam

Message:

pridany nejake testy

Files:

• library/tests/testsuite/LQG_test.cpp (modified) (4 diffs)

library/tests/testsuite/LQG_test.cpp

@@ -114 +114 @@
 }
 
-TEST (lqguniversal_test){
+TEST (LQGU_static_vs_Riccati_test){
   //test of universal LQG controller
   LQG_universal lq;
@@ -144 +144 @@
   */
 
-  //mat mA("2 -1;0 0.5"); 
-  //mat mB("1 -0.1;-0.2 2"); 
-  //mat mQ("5 0;0 1"); 
-  //mat mR("0.01 0;0 0.1"); 
-  //mat mS = mQ;
-
-  ////starting configuration
-  //vec x0("6 3");
-
-  //uniform random generator
-  Uniform_RNG urng;
-  double maxmult = 10;
-  vec tmpdiag;
-  
-  mat mA;   
-        urng.sample_matrix(2, 2, mA);
-        mA *= maxmult;
-  mat mB; 
-        urng.sample_matrix(2, 2, mB);
-        mB *= maxmult;
-  mat mQ(2, 2); 
-        urng.sample_vector(2, tmpdiag);
-        tmpdiag *= maxmult;
-        mQ.zeros();
-        mQ.set(0, 0, tmpdiag.get(0));
-        mQ.set(1, 1, tmpdiag.get(1));
-  mat mR(2, 2); 
-        urng.sample_vector(2, tmpdiag);
-        tmpdiag *= maxmult;
-        mR.zeros();
-        mR.set(0, 0, tmpdiag.get(0));
-        mR.set(1, 1, tmpdiag.get(1));
-  mat mS(2, 2); 
-        urng.sample_vector(2, tmpdiag);
-        tmpdiag *= maxmult;
-        mS.zeros();
-        mS.set(0, 0, tmpdiag.get(0));
-        mS.set(1, 1, tmpdiag.get(1));
+  mat mA("2 -1;0 0.5"); 
+  mat mB("1 -0.1;-0.2 2"); 
+  mat mQ("5 0;0 1"); 
+  mat mR("0.01 0;0 0.1"); 
+  mat mS = mQ;
 
   //starting configuration
-  vec x0;
-        urng.sample_vector(2, x0);
-        x0 *= maxmult;
+  vec x0("6 3");  
  
         /*cout << "configuration:" << endl 
@@ -225 +190 @@
   lq.finalLoss.Q.setCh(mS);
   lq.finalLoss.rv = RV("x", 2, 1);
+
+  lq.validate();
   
   //default L
@@ -304 +271 @@
 
   //cout << "Loss LQG_universal: " << loss_uni << " vs Loss LQG Riccati: " << loss_rct << endl;
-  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);
-}
+  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
+}
+
+TEST (LQGU_random_vs_Riccati_test){
+    
+  //uniform random generator
+  Uniform_RNG urng;
+  double maxmult = 10.0;
+  int maxxsize = 5;
+  int maxusize = 5;
+
+  urng.setup(2.0, maxxsize);  
+  int matxsize = round(urng());
+  urng.setup(2.0, maxusize);  
+  int matusize = round(urng());
+
+  urng.setup(-maxmult, maxmult);        
+
+  mat tmpmat;
+    
+  mat mA;   
+        mA = urng(matxsize, matxsize);  
+  mat mB; 
+        mB = urng(matxsize, matusize);  
+  mat mQ; 
+        tmpmat = urng(matxsize, matxsize);
+        mQ = tmpmat*tmpmat.transpose();
+  mat mR; 
+        tmpmat = urng(matusize, matusize);
+        mR = tmpmat*tmpmat.transpose();
+  mat mS; 
+        tmpmat = urng(matxsize, matxsize);
+        mS = tmpmat*tmpmat.transpose();
+
+  //starting configuration
+  vec x0;
+        x0 = urng(matxsize);
+        
+        /*cout << "configuration:" << endl
+                << "x size " << matxsize << "  u size " << matusize << endl
+                << "mA:" << endl << mA << endl
+                << "mB:" << endl << mB << endl
+                << "mQ:" << endl << mQ << endl
+                << "mR:" << endl << mR << endl
+                << "mS:" << endl << mS << endl
+                << "x0:" << endl << x0 << endl;*/
+ 
+  vec zerovecx(matxsize);
+        zerovecx.zeros();
+  vec zerovecu(matusize);
+        zerovecu.zeros();
+
+//test of universal LQG controller
+  LQG_universal lq;
+  lq.rv = RV("u", matusize, 0);  
+  lq.set_rvc(RV("x", matxsize, 0));
+  lq.horizon = 10;
+
+  //model
+  Array<linfnEx> model(2);
+  //model Ax part
+  model(0).A = mA;
+  model(0).B = zerovecx;
+  model(0).rv = RV("x", matxsize, 0);
+  model(0).rv_ret = RV("x", matxsize, 1);
+  //model Bu part
+  model(1).A = mB;
+  model(1).B = zerovecu;
+  model(1).rv = RV("u", matusize, 0);
+  model(1).rv_ret = RV("x", matxsize, 1);       
+  //setup
+  lq.Models = model;
+
+  //loss
+  Array<quadraticfn> loss(2);
+  //loss x'Qx part
+  loss(0).Q.setCh(mQ);
+  loss(0).rv = RV("x", matxsize, 0);
+  //loss u'Ru part
+  loss(1).Q.setCh(mR);
+  loss(1).rv = RV("u", matusize, 0);
+  //setup
+  lq.Losses = loss;
+
+  //finalloss setup
+  lq.finalLoss.Q.setCh(mS);
+  lq.finalLoss.rv = RV("x", matxsize, 1);
+
+  lq.validate();  
+
+  //produce last control matrix L
+  lq.redesign();
+   
+  //verification via Riccati LQG version
+  mat mK = mS;
+  mat mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;  
+
+  //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion
+  //more important is reached loss compared in the next part
+  double tolerr = 2;//1;//0.01; //0.01 OK x 0.001 NO OK
+
+  //check last time L matrix
+  CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
+  
+  mat oldK;
+  int i;
+
+  //produce next control matrix L
+  for(i = 0; i < lq.horizon - 1; i++) {
+          lq.redesign();
+          oldK = mK;
+          mK = mA.transpose() * (oldK - oldK * mB * inv(mR + mB.transpose() * oldK * mB) * mB.transpose() * oldK) * mA + mQ;
+          mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA;   
+
+          //check other times L matrix
+          CHECK_CLOSE_EX(lq.getL().get_cols(0,(matxsize-1)), mL, tolerr);
+  }
+
+ //check losses of LQG control - compare LQG_universal and Riccati version, no noise
+    
+  //loss of LQG_universal
+  vec loss_uni("0");
+
+  //setup
+  vec x = x0;
+  vec xold = x;
+  vec u;
+  //vec tmploss;
+
+  //iteration
+  for(i = 0; i < lq.horizon - 1; i++){
+        u = lq.getL().get_cols(0,(matxsize-1)) * xold;
+        x = mA * xold + mB * u;
+        loss_uni = x.transpose() * mQ * x + u.transpose() * mR * u;     
+        xold = x;
+  }
+
+  loss_uni = x.transpose() * mS * x;
+  
+  //loss of LQG Riccati version
+  vec loss_rct("0");
+
+  //setup
+  x = x0;
+  xold = x;
+
+  //iteration
+  for(i = 0; i < lq.horizon - 1; i++){
+        u = mL * xold;
+        x = mA * xold + mB * u;
+        loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;     
+        xold = x;
+  }
+  loss_rct = x.transpose() * mS * x;  
+  
+  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);  
+}
+
+TEST (LQGU_SiSy_test){
+    int K = 5;
+    int N = 100;    
+    double sigma = 0.1;        
+    double yr = 1;
+        double y0 = 0;
+    double x0 = y0 - yr;
+        double Eb = 1;
+        double P;
+        //double P = 0.01;      //loss ~ 1.0???         e-2
+        //double P = 0.1;       //loss ~ 1.???          e-1
+        //double P = 1;         //loss ~ ???.???        e+2
+        //double P = 10;        //loss ~ ?e+6
+               
+    mat A("1"); 
+    mat B(1,1);
+                B(0,0) = Eb;
+    mat Q("1");
+    mat R("0");   
+    
+    vec u(K);
+                u.zeros();
+    mat xn(K, N);
+                xn.zeros();
+    vec S(K);            
+                S.zeros();
+    vec L(K);        
+        L.zeros();
+
+    vec loss(N); 
+                loss.zeros();  
+
+        LQG_universal lq;
+        lq.rv = RV("u", 1, 0);  
+        lq.set_rvc(RV("x", 1, 0));
+        lq.horizon = K;
+
+        Array<linfnEx> model(2);  
+  model(0).A = A;
+  model(0).B = vec("0 0");
+  model(0).rv = RV("x", 1, 0);
+  model(0).rv_ret = RV("x", 1, 1);  
+  model(1).A = B;
+  model(1).B = vec("0 0");
+  model(1).rv = RV("u", 1, 0);
+  model(1).rv_ret = RV("x", 1, 1);  
+  lq.Models = model;
+
+  Array<quadraticfn> qloss(2);  
+  qloss(0).Q.setCh(Q);
+  qloss(0).rv = RV("x", 1, 0);  
+  qloss(1).Q.setCh(R);
+  qloss(1).rv = RV("u", 1, 0);  
+  lq.Losses = qloss;
+  
+  lq.finalLoss.Q.setCh(Q);
+  lq.finalLoss.rv = RV("x", 1, 1);
+
+  lq.validate();  
+          
+        int n, k;
+        double Br;
+        //double x00;
+
+        for(k = K-1; k >= 0;k--){
+                        lq.redesign();
+                        L(k) = (lq.getL())(0,0);
+                }
+
+for(P = 0.01; P <= 10; (P*=10)){
+        lq.resetTime();
+        
+//cout << "Ls " << L << endl;
+        for(n = 0; n < N; n++){        
+        Br = Eb + sqrt(P)*randn();        
+   
+                xn(0, n) = x0 + sigma*randn();            
+                                for(k = 0; k < K-1; k++){               
+                   u(k) = L(k)*xn(k, n);                     
+                   xn(k+1, n) = (A*xn(k, n))(0,0) + Br*u(k) + sigma*randn();                                       
+                                }
+                
+                loss(n) = 0;
+                                for(k=0;k<K;k++) loss(n) += xn(k, n)*xn(k, n);
+                
+        }
+    
+        /*vec mtraj(K);
+        for(k=0;k<K;k++){
+                mtraj(k) = 0;
+                for(n = 0; n < N; n++) mtraj(k) += xn(k, n);
+                mtraj(k) /= N;
+                mtraj(k) += yr;
+        }
+        cout << "prum trajek " << mtraj << endl;*/
+    //cout << "prum ztrata " << sum(loss)/N << endl;
+    //rfloss = mean(loss);
+    //rftraj = (mean((xn(1:K, :)), 2) + yr*ones(K, 1))';
+
+        double tolerr;
+        //double P = 0.01;      //loss ~ 1.0???         e-2
+        //double P = 0.1;       //loss ~ 1.???          e-1
+        //double P = 1;         //loss ~ ???.???        e+2
+        //double P = 10;        //loss ~ ?e+6
+        if(P == 0.01) tolerr = 0.2;
+        else if(P == 0.1) tolerr = 2;
+        else if(P == 1) tolerr = 2000;
+        else if(P == 10) tolerr = 2e+7;
+        else tolerr = 0;
+
+        CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
+        }
+}
+
+TEST (LQGU_SiSy_SuSt_test){
+        int K = 5;
+    int N = 100;    
+    double sigma = 0.1;        
+    double yr = 1;
+        double y0 = 0;    
+        double Eb = 1;
+        double inP;
+        //double inP = 0.01;    //loss ~ 1.0???         e-2
+        //double inP = 0.1;     //loss ~ 1.0???         e-2
+        //double inP = 1;               //loss ~ 1.0???         e-2
+        //double inP = 10;      //loss ~ 1.0???         e-2
+
+        vec x0(3);
+                x0(0) = y0 - yr;
+                x0(1) = Eb;
+                //x0(2) = inP;
+               
+    mat A("1 0 0; 0 1 0; 0 0 1"); 
+    mat B("0; 0; 0");
+        mat X("1 0 0; 0 0 0; 0 0 0");   
+        mat Y("0.00001");
+    mat Q(3,3);
+                Q.zeros();
+                Q(1,1) = sigma*sigma;
+    //mat R("0");   
+        //mat P(3,3);
+        //      P.zeros();
+    
+    vec u(K);
+                u.zeros();
+        vec Kk(K);
+                Kk.zeros();
+    mat xn0(K+1, N);
+                xn0.zeros();
+        mat xn1(K+1, N);
+                xn1.zeros();
+        mat xn2(K+1, N);
+                xn2.zeros();
+    //vec S(K);            
+        //      S.zeros();
+    //vec L(K);        
+    //    L.zeros();
+        mat L(1, 3);
+                L.zeros();
+    vec loss(N); 
+                loss.zeros();  
+
+        LQG_universal lq;
+        lq.rv = RV("u", 1, 0);  
+        lq.set_rvc(RV("x", 3, 0));
+        lq.horizon = K;
+
+        Array<linfnEx> model(2);  
+  model(0).A = A;
+  model(0).B = vec("0 0");
+  model(0).rv = RV("x", 3, 0);
+  model(0).rv_ret = RV("x", 3, 1);  
+  model(1).A = B;
+  model(1).B = vec("0 0");
+  model(1).rv = RV("u", 1, 0);
+  model(1).rv_ret = RV("x", 3, 1);  
+  lq.Models = model;
+
+  Array<quadraticfn> qloss(2);  
+  qloss(0).Q.setCh(X);
+  qloss(0).rv = RV("x", 3, 0);  
+  qloss(1).Q.setCh(Y);
+  qloss(1).rv = RV("u", 1, 0);  
+  lq.Losses = qloss;
+  
+  lq.finalLoss.Q.setCh(X);
+  lq.finalLoss.rv = RV("x", 3, 1);
+
+  lq.validate();  
+          
+        int n, k;
+        //double Br;
+        //double x00;
+
+        /*for(k = K-1; k >= 0;k--){
+                lq.redesign();
+                L(k) = (lq.getL())(0,0);
+        }*/
+//cout << "Ls " << L << endl;
+for(inP = 0.01; inP <= 10; (inP*=10)){
+        lq.resetTime();
+        x0(2) = inP;
+        for(n = 0; n < N; n++){  
+                L.zeros();
+        xn0(0, n) = x0(0);           
+                xn1(0, n) = x0(1) + sqrt(inP) * randn();            
+                xn2(0, n) = x0(2); 
+                //^neznalost, sum zatim ne
+
+                                for(k = 0; k < K-1; k++){               
+                   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n);
+                                   Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma);
+                   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
+                   xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
+                   xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
+                                }
+                
+                                lq.resetTime();
+                                lq.redesign();
+                                for(k = K-1; k > 0; k--){               
+                                        A(0, 1) = u(k);
+                    A(1, 0) = -Kk(k);
+                    A(1, 1) = 1-Kk(k)*u(k);
+                    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);
+                    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);
+                    B(0) = xn1(k, n);
+                    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);
+                    B(2) = -2*u(k)*xn2(k, n)*xn2(k, n)*sigma*sigma / sqr(u(k)*u(k)*xn2(k, n) + sigma*sigma);
+                                        lq.Models(0).A = A;
+                    lq.Models(1).A = B;                                      
+                                        lq.redesign();
+                                }
+                                L = lq.getL();
+
+                                //simulation
+                                xn0(0, n) += sigma*randn(); 
+                                for(k = 0; k < K-1; k++){
+                                   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n);
+                                   Kk(k) = u(k)*xn2(k, n)/(u(k)*u(k)*xn2(k, n) + sigma*sigma);
+                   xn0(k+1, n) = xn0(k, n) + xn1(k, n)*u(k) + sigma*randn();
+                   xn1(k+1, n) = xn1(k, n) + Kk(k)*(xn0(k+1, n) - xn0(k, n) - xn1(k, n)*u(k));
+                   xn2(k+1, n) = (1 - Kk(k)*u(k))*xn2(k, n);                                       
+                                }
+
+
+                loss(n) = 0;
+                                for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n);
+                
+        }
+    
+        /*vec mtraj(K);
+        for(k=0;k<K;k++){
+                mtraj(k) = 0;
+                for(n = 0; n < N; n++) mtraj(k) += xn0(k, n);
+                mtraj(k) /= N;
+                mtraj(k) += yr;
+        }
+        cout << "prum trajek " << mtraj << endl;*/
+    //cout << "prum ztrata " << sum(loss)/N << endl;
+    
+        double tolerr = 0.2;    
+        /*if(inP == 0.01) tolerr = 0.2;
+        else if(inP == 0.1) tolerr = 2;
+        else if(inP == 1) tolerr = 2000;
+        else if(inP == 10) tolerr = 2e+7;
+        else tolerr = 0;*/
+
+        CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr);
+        }
+}
+
+TEST (LQGU_PMSM_test){
+        bdm_assert(0, "Test not implemented.");
+}
+
+TEST (LQGU_large_test){
+        bdm_assert(0, "Test not implemented.");
+}
+
+TEST (validation_test){
+        RV rv1("x", 1);
+        RV rv2("y", 2);
+        RV rv3("z", 3);
+        RV rv4("u", 2);
+        RV rv5("v", 1);
+        RV all;
+        all = rv1;
+        all.add(rv2);
+        all.add(rv3);
+        all.add(rv4);
+        all.add(rv5);
+        all.add(RV("1",1,0));
+        cout << "all rv: " << all << endl;
+
+        ivec fy = rv2.findself(all);
+        cout << "finding y: " << fy << endl;
+        ivec dy = rv2.dataind(all);
+        cout << "dataind y: " << dy << endl;
+
+        RV rvzyu;
+        rvzyu = rv3;
+        rvzyu.add(rv2);
+        rvzyu.add(rv4);
+        fy = rvzyu.findself(all);
+        cout << "finding zyu: " << fy << endl;
+        dy = rvzyu.dataind(all);
+        cout << "dataind zyu: " << dy << endl;
+
+        rvzyu.add(RV("1",1,0));
+        fy = rvzyu.findself(all);
+        cout << "finding zyu1: " << fy << endl;
+        dy = rvzyu.dataind(all);
+        cout << "dataind zyu1: " << dy << endl;
+
+        rvzyu.add(RV("k",1,0));
+        fy = rvzyu.findself(all);
+        cout << "finding zyu1 !k!: " << fy << endl;
+        dy = rvzyu.dataind(all);
+        cout << "dataind zyu1 !k!: " << dy << endl;
+
+        RV emptyrv;
+        fy = emptyrv.findself(all);
+        cout << "finding empty: " << fy << " size " << fy.size() << endl;
+        dy = emptyrv.dataind(all);
+        cout << "dataind empty: " << dy << " size " << dy.size() << endl;
+
+        LQG_universal lq;
+        lq.validate();
+}