Changeset 1222 for library

Show
Ignore:
Timestamp:
10/20/10 18:42:06 (15 years ago)
Author:
vahalam
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/tests/testsuite/LQG_test.cpp

    r1217 r1222  
    9292} 
    9393 
    94 //TEST (quadratic_test){ 
    95   /*quadraticfn qf; 
    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    
    106   lq.Models = Array<linfnEx>(1); 
    107   lq.Models(0) = linfnEx(mat("1"),vec("1")); 
    108   lq.Models(0).rv = RV("{x }"); 
    109    
    110   lq.rv = RV("u",1); 
    111    
    112   lq.redesign(); 
    113   CHECK_CLOSE_EX(lq.ctrlaction(vec("1,0")), vec("1.24, -5.6"), 0.0000001);*/ 
    114 //} 
    115  
    11694TEST (LQGU_static_vs_Riccati_test){ 
    11795  //test of universal LQG controller 
    … …  
    151129 
    152130  //starting configuration 
    153   vec x0("6 3");   
    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;*/ 
     131  vec x0("6 3");         
    162132 
    163133  //model 
    … …  
    192162 
    193163  lq.validate(); 
    194    
    195   //default L 
    196   //cout << "default L matrix:" << endl << lq.getL() << endl; 
    197  
     164     
    198165  //produce last control matrix L 
    199166  lq.redesign(); 
    … …  
    202169  mat mK = mS; 
    203170  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  
     171   
    208172  //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion 
    209173  //more important is reached loss compared in the next part 
    … …  
    223187          mL = - inv(mR + mB.transpose() * mK * mB) * mB.transpose() * mK * mA; 
    224188 
    225           //cout << "L matrix LQG_universal:" << endl << lq.getL() << endl <<  
    226                   //"L matrix LQG Riccati:" << endl << mL << endl; 
    227  
    228189          //check other times L matrix 
    229190          CHECK_CLOSE_EX(lq.getL().get_cols(0,1), mL, tolerr); 
    … …  
    233194     
    234195  //loss of LQG_universal 
    235   /*double*/vec loss_uni("0"); 
     196  vec loss_uni("0"); 
    236197 
    237198  //setup 
    … …  
    249210        xold = x; 
    250211  } 
    251   /*tmploss*/ loss_uni = x.transpose() * mS * x; 
    252   //loss_uni += tmploss.get(0); 
    253  
     212  loss_uni = x.transpose() * mS * x; 
     213   
    254214  //loss of LQG Riccati version 
    255   /*double*/ vec loss_rct("0"); 
     215  vec loss_rct("0"); 
    256216 
    257217  //setup 
    … …  
    263223        u = mL * xold; 
    264224        x = mA * xold + mB * u; 
    265         /*tmploss*/loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u; 
    266         //loss_rct += tmploss.get(0); 
     225        loss_rct = x.transpose() * mQ * x + u.transpose() * mR * u;      
    267226        xold = x; 
    268227  } 
    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; 
     228  loss_rct = x.transpose() * mS * x;   
     229   
    273230  CHECK_CLOSE_EX(loss_uni, loss_rct, 0.0001);   
    274231} 
    … …  
    307264  //starting configuration 
    308265  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;*/ 
     266        x0 = urng(matxsize);     
    319267  
    320268  vec zerovecx(matxsize); 
    … …  
    370318  //checking L matrix (universal vs Riccati), tolerance is high, but L is not main criterion 
    371319  //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 
     320  double tolerr = 2; 
    373321 
    374322  //check last time L matrix 
    … …  
    491439        int n, k; 
    492440        double Br; 
    493         //double x00; 
    494  
     441         
    495442        for(k = K-1; k >= 0;k--){ 
    496443                        lq.redesign(); 
    … …  
    499446 
    500447for(P = 0.01; P <= 10; (P*=10)){ 
    501         lq.resetTime(); 
    502          
    503 //cout << "Ls " << L << endl; 
     448        lq.resetTime();  
     449 
    504450        for(n = 0; n < N; n++){         
    505451        Br = Eb + sqrt(P)*randn();         
    … …  
    516462        } 
    517463     
    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  
    530464        double tolerr; 
    531465        //double P = 0.01;      //loss ~ 1.0???         e-2 
    … …  
    559493                x0(0) = y0 - yr; 
    560494                x0(1) = Eb; 
    561                 //x0(2) = inP; 
    562                 
     495                                
    563496    mat A("1 0 0; 0 1 0; 0 0 1");  
    564497    mat B("0; 0; 0"); 
    … …  
    568501                Q.zeros(); 
    569502                Q(1,1) = sigma*sigma; 
    570     //mat R("0");    
    571         //mat P(3,3); 
    572         //      P.zeros(); 
    573      
     503         
    574504    vec u(K); 
    575505                u.zeros(); 
    … …  
    582512        mat xn2(K+1, N); 
    583513                xn2.zeros(); 
    584     //vec S(K);             
    585         //      S.zeros(); 
    586     //vec L(K);         
    587     //    L.zeros(); 
     514     
    588515        mat L(1, 3); 
    589516                L.zeros(); 
    … …  
    597524 
    598525        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); 
    611   qloss(0).rv = RV("x", 3, 1);   
    612   qloss(1).Q.setCh(Y); 
    613   qloss(1).rv = RV("u", 1, 0);   
    614   lq.Losses = qloss; 
     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; 
    615542   
    616   lq.finalLoss.Q.setCh(X); 
    617   lq.finalLoss.rv = RV("x", 3, 1); 
    618  
    619   lq.validate();   
     543     lq.finalLoss.Q.setCh(X); 
     544     lq.finalLoss.rv = RV("x", 3, 1); 
     545 
     546     lq.validate();   
    620547           
    621548        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; 
     549         
    630550for(inP = 0.01; inP <= 10; (inP*=10)){ 
    631551        lq.resetTime(); 
    … …  
    668588                                   u(k) = L(0)*xn0(k, n) + L(1)*xn1(k, n) + L(2)*xn2(k, n); 
    669589                                   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);                                        
     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);                                        
    673593                                } 
    674594 
    675595 
    676                 loss(n) = 0; 
     596                    loss(n) = 0; 
    677597                                for(k=0;k<K;k++) loss(n) += xn0(k, n)*xn0(k, n); 
    678598                 
    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; 
     599        }        
    690600     
    691601        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  
     602         
    698603        CHECK_CLOSE_EX(1.0, sum(loss)/N, tolerr); 
    699604        } 
    … …  
    837742                                else if(u(1) < -50) u(1) = -50; 
    838743             
    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();  
     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; 
    843748                                x(4, k+1) = x(4, k);                             
    844749                        } 
    … …  
    883788                        x(4, k+1) = x(4, k);  
    884789         
    885                         losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);// + (u.transpose() * Y * u)(0);                       
     790                        losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);                       
    886791                } 
    887792 
    … …  
    889794        } 
    890795 
    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  
     796        cout << "Loss: " << sum(losses)/N << endl; 
    900797} 
    901798 
    … …  
    944841        qloss(0).Q.setCh(mat("1")); 
    945842        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 
     843        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)       
    948844        qloss(1).rv = RV("ut", 1, 0);    
    949845        lq.Losses = qloss; 
    … …  
    957853        lq.redesign(); 
    958854 
    959         for(int k = K-1; k > 0; k--){ 
    960                 //cout << "L: " << lq.getL() << endl; 
     855        for(int k = K-1; k > 0; k--){            
    961856                lq.redesign(); 
    962857        } 
    963858 
    964         cout << "L: " << lq.getL() << endl; 
     859        mat resref("-69.4086   47.2578   -0.2701   0.0"); 
     860        CHECK_CLOSE_EX(resref, lq.getL(), 0.1); 
    965861} 
    966862 
    … …  
    976872 
    977873        int K = 150; 
    978         //yr = 10.0; 
    979  
     874         
    980875        LQG_universal lq; 
    981876        lq.rv = RV("ut", 1, 0);  
    … …  
    1023918        lq.redesign(); 
    1024919 
    1025         for(int k = K-1; k > 0; k--){ 
    1026                 //cout << "L: " << lq.getL() << endl; 
     920        for(int k = K-1; k > 0; k--){            
    1027921                lq.redesign(); 
    1028922        } 
    1029923 
    1030         cout << "L: " << lq.getL() << endl; 
     924        mat resref("-69.4086   47.2578   -0.2701   233.758"); 
     925        CHECK_CLOSE_EX(resref, lq.getL(), 0.1); 
    1031926} 
    1032927 
    … …  
    1041936        */ 
    1042937 
    1043         int K = 200; 
    1044  
    1045         //double yr = 10.0; 
     938        int K = 200;     
    1046939 
    1047940        LQG_universal lq; 
    … …  
    1083976        qloss(0).rv = RV("yt", 1, 0); 
    1084977        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 
     978        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)       
    1087979        qloss(1).rv = RV("ut", 1, 0);    
    1088980        lq.Losses = qloss; 
    … …  
    1097989        lq.redesign(); 
    1098990 
    1099         for(int k = K-1; k > 0; k--){ 
    1100                 //cout << "L: " << lq.getL() << endl; 
     991        for(int k = K-1; k > 0; k--){            
    1101992                lq.redesign(); 
    1102993        } 
    1103994 
    1104         cout << "L: " << lq.getL() << endl; 
     995        mat resref("-69.4086   47.2578   -0.2701   23.3758   0.0"); 
     996        CHECK_CLOSE_EX(resref, lq.getL(), 0.1); 
    1105997} 
    1106998 
    … …  
    11231015 
    11241016        int K = 50; 
    1125         double w = 5.0; 
    1126         double q0 = 0.1; 
    1127  
     1017         
    11281018        LQG_universal lq; 
    11291019        lq.rv = RV("u", 1, 0);  
    … …  
    11911081        for(k = K-1; k > 0; k--) lq.redesign(); 
    11921082         
    1193         cout << "L: " << lq.getL() << endl; 
    1194  
    1195         vec q(50); 
    1196         q(0) = q0; 
    1197  
    1198         vec u(50); 
    1199  
    1200         //rvc = [q(k), q(k-1), q(k-2), u(k-1), u(k-2), w(k)] 
    1201         vec xrvc(6); 
    1202         xrvc(5) = w; 
     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);   
    12031096                 
    1204  
    1205         //first step 
    1206         xrvc(0) = q(0); 
    1207         xrvc(1) = q0; 
    1208         xrvc(2) = q0; 
    1209         xrvc(3) = 0; 
    1210         xrvc(4) = 0; 
    1211         u(0) = (lq.ctrlaction(xrvc))(0); 
    1212         q(1) = 1.654 * q(0) - 1.022*q0 + 0.2019*q0 + 0.1098*u(0); 
    1213  
    1214         //second step 
    1215         xrvc(0) = q(1); 
    1216         xrvc(1) = q(0); 
    1217         xrvc(2) = q0; 
    1218         xrvc(3) = u(0); 
    1219         xrvc(4) = 0; 
    1220         u(1) = (lq.ctrlaction(xrvc))(0); 
    1221         q(2) = 1.654 * q(1) - 1.022*q(0) + 0.2019*q0 + 0.1098*u(1) + 0.0792*u(0); 
    1222  
    1223         //cycle 
    1224         for(k = 2; k < K-1; k++){ 
    1225                 xrvc(0) = q(k); 
    1226                 xrvc(1) = q(k-1); 
    1227                 xrvc(2) = q(k-2); 
    1228                 xrvc(3) = u(k-1); 
    1229                 xrvc(4) = u(k-2); 
    1230                 u(k) = (lq.ctrlaction(xrvc))(0); 
    1231                 q(k+1) = 1.654 * q(k) - 1.022*q(k-1) + 0.2019*q(k-2) + 0.1098*u(k) + 0.0792*u(k-1) - 0.0229*u(k-2); 
    1232         } 
    1233          
    1234          
    1235         ofstream log; 
    1236         log.open ("log.txt"); 
    1237         for(k = 0; k < K; k++) 
    1238                 log << k << "\t" << q(k) << endl; 
    1239         log.close(); 
    1240 } 
    1241  
    1242 /*TEST (matrixdivisionspeedtest){ 
    1243         Real_Timer tmr; 
    1244         Uniform_RNG urng; 
    1245  
    1246         mat A("0.0319219"); //1 1 1 1; 0 2 2 2 2; 0 0 3 3 3; 0 0 0 4 4; 0 0 0 0 5"); 
    1247         mat B("0.24835 -0.112361 0.000642142 -1.3721");// = urng(2,6);           
    1248         mat C(1,4); 
    1249                 C.zeros(); 
    1250  
    1251         int n = 1; 
    1252         int j,k,l,m; 
    1253          
    1254         //cout << "A:" << endl << A << endl << "B:" << endl << B << endl; 
    1255  
    1256         cout << "Using inv function: "; 
    1257         tmr.tic(); 
    1258                 for(j = 0; j < n; j++) C = inv(A)*B;     
    1259         tmr.toc_print(); 
    1260         cout << endl << C << endl; 
    1261          
    1262         cout << "Using ldutg function: "; 
    1263         tmr.tic(); 
    1264                 for(j = 0; j < n; j++) ldutg(A,B,C);     
    1265         tmr.toc_print(); 
    1266         cout << endl << C << endl;*/ 
    1267  
    1268         /*cout << "Using Gauss elimination: "; 
    1269                 int utrows = A.rows();                      
    1270                 int longrow = utrows + B.cols();                     
    1271                 mat Cc(utrows,longrow); 
    1272                         Cc.zeros(); 
    1273         tmr.tic(); 
    1274                 for(j = 0; j < n; j++){   
    1275                     Cc.set_submatrix(0,utrows-1,0,utrows-1,A);//rrccM 
    1276                     Cc.set_submatrix(0,utrows-1,utrows,longrow-1,B);                 
    1277                     for(k = utrows-1; k >= 0; k--){         
    1278                            Cc.set_row(k, Cc.get_row(k) / A(k,k));         
    1279                            for(l = 0;l < k; l++){             
    1280                                   Cc.set_row(l,Cc.get_row(l) - A(l,k)*Cc.get_row(k)); 
    1281                            }                                                       
    1282                     }     
    1283                     C = Cc(0,utrows-1,utrows,longrow-1); 
    1284                 } 
    1285         tmr.toc_print(); 
    1286         cout << endl << C << endl;*/ 
    1287          
    1288         /*cout << "Using Gauss elimination Direct access!: "; 
    1289          //data da pointer co jede jako pole po sloupcich  
    1290         tmr.tic(); 
    1291                 for(j = 0; j < n; j++){   
    1292                      
    1293                                 for(l=0;l<25;l++) 
    1294                                         (Cc._data())[l] = (A._data())[l]; 
    1295                                 for(l=25;l<50;l++) 
    1296                                         Cc._data()[l] = B._data()[l-25];                     
    1297                     
    1298                                      
    1299                     for(k = utrows-1; k >= 0; k--){        
    1300                            for(l=0;l<10;l++) Cc._data()[k+5*l] /= A._data()[k+5*k];                                         
    1301                            for(l = 0;l < k; l++){ 
    1302                                   for(m=0;m<10;m++)     Cc._data()[l+5*m] -= A._data()[l+5*k]*Cc._data()[k+5*m];                                           
    1303                            }                                                       
    1304                     }     
    1305                      
    1306                     for(k=0;k<25;k++) 
    1307                                  
    1308                                         C._data()[k] = Cc._data()[k+25]; 
    1309                 } 
    1310                  
    1311         tmr.toc_print();                 
    1312         cout << endl << C << endl; 
    1313                  
    1314         cout << "Using Gauss elimination ARRAY: "; 
    1315         tmr.tic(); 
    1316         double arA[5][5];                    
    1317         double arB[5][5];                    
    1318         double arC[5][5];                    
    1319         double arCc[5][10]; 
    1320         for(k=0;k<5;k++){ 
    1321                 for(l=0;l<5;l++){ 
    1322                         arA[k][l] = A(k,l); 
    1323                         arB[k][l] = B(k,l); 
    1324                 } 
    1325         } 
    1326                                              
    1327                          
    1328          
    1329                 for(j = 0; j < n; j++){   
    1330                     for(k=0;k<5;k++){ 
    1331                                 for(l=0;l<5;l++) 
    1332                                         arCc[k][l] = arA[k][l]; 
    1333                                 for(l=5;l<10;l++) 
    1334                                         arCc[k][l] = arB[k][l-5];                    
    1335                     } 
    1336                                      
    1337                     for(k = utrows-1; k >= 0; k--){        
    1338                            for(l=0;l<10;l++) arCc[k][l] /= arA[k][k];                                       
    1339                            for(l = 0;l < k; l++){ 
    1340                                   for(m=0;m<10;m++)     arCc[l][m] -= arA[l][k]*arCc[k][m];                                        
    1341                            }                                                       
    1342                     }     
    1343                      
    1344                     for(k=0;k<5;k++) 
    1345                                 for(l=0;l<5;l++) 
    1346                                         arC[k][l] = arCc[k][l+5]; 
    1347                 } 
    1348                  
    1349                  
    1350          
    1351         for(k=0;k<5;k++) 
    1352                 for(l=0;l<5;l++) 
    1353                         C(k,l) = arC[k][l];                      
    1354         tmr.toc_print(); 
    1355         cout << endl << C << endl;*/ 
    1356 //} 
    1357  
    1358 //TEST (LQGU_large_test){ 
    1359         /* 
    1360         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] 
    1361         u = rv 
    1362         x = rvc 
    1363         y = A1*[x; 1] + B1 
    1364         z = A2*[x; u; 1] + B2 
    1365         x(+1) = A3*[x; 1] + B3 
    1366         */ 
    1367         //uniform random generator 
    1368   /*Uniform_RNG urng;   
    1369   int max_size = 10; 
    1370   double maxmult = 10.0; 
    1371    
    1372   urng.setup(2.0, max_size);   
    1373   int xsize = round(urng());     
    1374   int usize = round(urng()); 
    1375   int ysize = round(urng()); 
    1376   int zsize = round(urng()); 
    1377   cout << "CFG: " << xsize << " " << ysize << " " << zsize << " " << usize << " " << endl; 
    1378   urng.setup(-maxmult, maxmult); 
    1379   mat tmpmat; 
    1380   mat A1,A2,A3,Q1,Q2,Q3,Q4,Q5,Q6,Q7; 
    1381   vec B1,B2,B3; 
    1382  
    1383   A1 = urng(ysize,xsize+1); 
    1384   B1 = urng(ysize); 
    1385   A2 = urng(zsize,xsize+usize+1); 
    1386   B2 = urng(zsize); 
    1387   A3 = urng(xsize,xsize+1); 
    1388   B3 = urng(xsize); 
    1389  
    1390   tmpmat = urng(xsize,xsize); 
    1391   Q1 = tmpmat*tmpmat.transpose(); 
    1392   tmpmat = urng(ysize,ysize); 
    1393   Q2 = tmpmat*tmpmat.transpose(); 
    1394   tmpmat = urng(zsize,zsize); 
    1395   Q3 = tmpmat*tmpmat.transpose(); 
    1396   tmpmat = urng(usize,usize); 
    1397   Q4 = tmpmat*tmpmat.transpose(); 
    1398   tmpmat = urng(xsize+ysize+1,xsize+ysize+1); 
    1399   Q5 = tmpmat*tmpmat.transpose(); 
    1400   tmpmat = urng(zsize+usize+1,zsize+usize+1); 
    1401   Q6 = tmpmat*tmpmat.transpose(); 
    1402   tmpmat = urng(xsize+ysize+zsize+usize+1,xsize+ysize+zsize+usize+1); 
    1403   Q7 = tmpmat*tmpmat.transpose(); 
    1404  
    1405      
    1406   //starting configuration 
    1407   vec x0; 
    1408         x0 = urng(xsize);   
    1409  
    1410 //test of universal LQG controller 
    1411   LQG_universal lq; 
    1412   lq.rv = RV("u", usize, 0);   
    1413   lq.set_rvc(RV("x", xsize, 0)); 
    1414   lq.horizon = 100; 
    1415  
    1416         RV tmprv; 
    1417  
    1418   //model 
    1419   Array<linfnEx> model(3);   
    1420   model(0).A = A1; 
    1421   model(0).B = B1; 
    1422   tmprv = RV("x", xsize, 0); 
    1423   tmprv.add(RV("1",1,0)); 
    1424   model(0).rv = tmprv; 
    1425   model(0).rv_ret = RV("y", ysize, 0);     
    1426   model(1).A = A2; 
    1427   model(1).B = B2; 
    1428   tmprv = RV("x", xsize, 0); 
    1429   tmprv.add(RV("u", usize, 0)); 
    1430   tmprv.add(RV("1",1,0)); 
    1431   model(1).rv = tmprv; 
    1432   model(1).rv_ret = RV("z", zsize, 0); 
    1433   model(2).A = A3; 
    1434   model(2).B = B3; 
    1435   tmprv = RV("x", xsize, 0); 
    1436   tmprv.add(RV("1",1,0)); 
    1437   model(2).rv = tmprv; 
    1438   model(2).rv_ret = RV("x", xsize, 1); 
    1439   //setup 
    1440   lq.Models = model; 
    1441  
    1442   //loss 
    1443   Array<quadraticfn> loss(7);   
    1444   loss(0).Q.setCh(Q1); 
    1445   loss(0).rv = RV("x", xsize, 1); 
    1446   loss(1).Q.setCh(Q2); 
    1447   loss(1).rv = RV("y", ysize, 0); 
    1448   loss(2).Q.setCh(Q3); 
    1449   loss(2).rv = RV("z", zsize, 0); 
    1450   loss(3).Q.setCh(Q4); 
    1451   loss(3).rv = RV("u", usize, 0); 
    1452   loss(4).Q.setCh(Q5); 
    1453   tmprv = RV("x", xsize, 1); 
    1454   tmprv.add(RV("y", ysize, 0)); 
    1455   tmprv.add(RV("1",1,0)); 
    1456   loss(4).rv = tmprv; 
    1457   loss(5).Q.setCh(Q6); 
    1458   tmprv = RV("z", zsize, 0); 
    1459   tmprv.add(RV("u", usize, 0)); 
    1460   tmprv.add(RV("1",1,0)); 
    1461   loss(5).rv = tmprv; 
    1462   loss(6).Q.setCh(Q7); 
    1463   tmprv = RV("x", xsize, 1); 
    1464   tmprv.add(RV("y", ysize, 0)); 
    1465   tmprv.add(RV("z", zsize, 0)); 
    1466   tmprv.add(RV("u", usize, 0)); 
    1467   tmprv.add(RV("1",1,0)); 
    1468   loss(6).rv = tmprv; 
    1469   //setup 
    1470   lq.Losses = loss; 
    1471  
    1472   //finalloss setup 
    1473   tmpmat = urng(xsize,xsize);    
    1474   lq.finalLoss.Q.setCh(tmpmat*tmpmat.transpose()); 
    1475   lq.finalLoss.rv = RV("x", xsize, 1); 
    1476  
    1477   lq.validate();   
    1478  
    1479   //produce last control matrix L 
    1480   lq.redesign(); 
    1481   cout << lq.getL() << endl;   
    1482    
    1483   int i;   
    1484   for(i = 0; i < lq.horizon - 1; i++) { 
    1485           lq.redesign();           
    1486   } 
    1487   cout << lq.getL() << endl; 
    1488  
    1489   mat K; 
    1490   K = A3.get_cols(0, xsize-1).transpose()*Q1.transpose()*Q1*A3.get_cols(0, xsize-1) 
    1491     + A1.get_cols(0, xsize-1).transpose()*Q2.transpose()*Q2*A1.get_cols(0, xsize-1) 
    1492         + A2.get_cols(0, xsize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(0, xsize-1) 
    1493         + 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) 
    1494         + 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)  
    1495         + 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) 
    1496         + 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) 
    1497         + 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) 
    1498         + 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); 
    1499  
    1500   mat L; 
    1501   L = A2.get_cols(xsize, xsize+usize-1).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize, xsize+usize-1) 
    1502     + Q4.transpose()*Q4 
    1503         + 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) 
    1504         + Q6.get_cols(zsize, zsize+usize-1).transpose()*Q6.get_cols(zsize, zsize+usize-1) 
    1505         + 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) 
    1506         + Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1).transpose()*Q7.get_cols(xsize+ysize+zsize, xsize+ysize+zsize+usize-1); 
    1507  
    1508   double M; 
    1509   M = (A3.get_cols(xsize,xsize).transpose()*Q1.transpose()*Q1*A3.get_cols(xsize,xsize))(0,0) 
    1510     + (B3.transpose()*Q1.transpose()*Q1*B3)(0) 
    1511         + (A1.get_cols(xsize,xsize).transpose()*Q2.transpose()*Q2*A1.get_cols(xsize,xsize))(0,0) 
    1512         + (B1.transpose()*Q2.transpose()*Q2*B1)(0) 
    1513         + (A2.get_cols(xsize+usize,xsize+usize).transpose()*Q3.transpose()*Q3*A2.get_cols(xsize+usize,xsize+usize))(0,0) 
    1514         + (B2.transpose()*Q3.transpose()*Q3*B2)(0) 
    1515         + (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) 
    1516         + (B3.transpose()*Q5.get_cols(0, xsize-1).transpose()*Q5.get_cols(0, xsize-1)*B3)(0) 
    1517         + (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) 
    1518         + (B1.transpose()*Q5.get_cols(xsize, xsize+ysize-1).transpose()*Q5.get_cols(xsize, xsize+ysize-1)*B1)(0) 
    1519         + (Q5.get_cols(xsize+ysize,xsize+ysize).transpose()*Q5.get_cols(xsize+ysize,xsize+ysize))(0,0) 
    1520         + (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) 
    1521         + (B2.transpose()*Q6.get_cols(0, zsize-1).transpose()*Q6.get_cols(0, zsize-1)*B2)(0) 
    1522         + (Q6.get_cols(zsize+usize,zsize+usize).transpose()*Q6.get_cols(zsize+usize,zsize+usize))(0,0) 
    1523         + (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) 
    1524         + (B3.transpose()*Q7.get_cols(0, xsize-1).transpose()*Q7.get_cols(0, xsize-1)*B3)(0) 
    1525         + (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) 
    1526         + (B1.transpose()*Q7.get_cols(xsize, xsize+ysize-1).transpose()*Q7.get_cols(xsize, xsize+ysize-1)*B1)(0) 
    1527         + (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) 
    1528         + (B2.transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*B2)(0) 
    1529         + (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);*/ 
    1530  
    1531    
    1532   /*mat res; 
    1533   res = -inv(sqrtm(L))*sqrtm(K);*/ 
    1534  
    1535   /*cout << -inv(sqrtm(L))*sqrtm(K).get_cols(0,L.rows()).get_rows(0,L.cols()) << endl; 
    1536 }*/ 
    1537  
    1538 /*TEST (validation_test){ 
    1539         RV rv1("x", 1); 
    1540         RV rv2("y", 2); 
    1541         RV rv3("z", 3); 
    1542         RV rv4("u", 2); 
    1543         RV rv5("v", 1); 
    1544         RV all; 
    1545         all = rv1; 
    1546         all.add(rv2); 
    1547         all.add(rv3); 
    1548         all.add(rv4); 
    1549         all.add(rv5); 
    1550         all.add(RV("1",1,0)); 
    1551         //cout << "all rv: " << all << endl; 
    1552  
    1553         ivec fy = rv2.findself(all); 
    1554         //cout << "finding y: " << fy << endl; 
    1555         ivec dy = rv2.dataind(all); 
    1556         //cout << "dataind y: " << dy << endl; 
    1557  
    1558         RV rvzyu; 
    1559         rvzyu = rv3; 
    1560         rvzyu.add(rv2); 
    1561         rvzyu.add(rv4); 
    1562         fy = rvzyu.findself(all); 
    1563         //cout << "finding zyu: " << fy << endl; 
    1564         dy = rvzyu.dataind(all); 
    1565         //cout << "dataind zyu: " << dy << endl; 
    1566  
    1567         rvzyu.add(RV("1",1,0)); 
    1568         fy = rvzyu.findself(all); 
    1569         //cout << "finding zyu1: " << fy << endl; 
    1570         dy = rvzyu.dataind(all); 
    1571         //cout << "dataind zyu1: " << dy << endl; 
    1572  
    1573         rvzyu.add(RV("k",1,0)); 
    1574         fy = rvzyu.findself(all); 
    1575         //cout << "finding zyu1 !k!: " << fy << endl; 
    1576         dy = rvzyu.dataind(all); 
    1577         //cout << "dataind zyu1 !k!: " << dy << endl; 
    1578  
    1579         RV emptyrv; 
    1580         fy = emptyrv.findself(all); 
    1581         //cout << "finding empty: " << fy << " size " << fy.size() << endl; 
    1582         dy = emptyrv.dataind(all); 
    1583         //cout << "dataind empty: " << dy << " size " << dy.size() << endl; 
    1584  
    1585         LQG_universal lq; 
    1586         lq.validate(); 
    1587 }*/ 
     1097        CHECK_CLOSE_EX(C1, C2, 0.000001);        
     1098}