Changeset 1217 for library/tests/testsuite/LQG_test.cpp

Timestamp:

10/19/10 22:35:02 (15 years ago)

Author:

vahalam

Message:

 

Files:

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

library/tests/testsuite/LQG_test.cpp

@@ -892 +892 @@
         //cout << "Trajektorie:\n" << omega(0,Kt-1)/N << endl;
         
-        ofstream log;
+        /*ofstream log;
         log.open ("log.txt");
         for(k = 0; k < Kt; k++)
                 log << k << "\t" << omega(k)/N + OMEGAt << endl;
-        log.close();
+        log.close();*/
 
 }
@@ -1043 +1043 @@
         int K = 200;
 
-        double yr = 10.0;
+        //double yr = 10.0;
 
         LQG_universal lq;
@@ -1082 +1082 @@
         qloss(0).Q.setCh(mat("1 -1"));
         qloss(0).rv = RV("yt", 1, 0);
-    qloss(0).rv.add(RV("yr", 1, 0));
+        qloss(0).rv.add(RV("yr", 1, 0));
         qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2)
         //qloss(1).Q = mat("0.001");//automatic sqrt
@@ -1104 +1104 @@
         cout << "L: " << lq.getL() << endl;
 }
+
+TEST (LQGU_Filatov){
+        /*                                               
+                system from:
+                Adaptive Predictive Control Policy for Nonlinear Stochastic Systems
+                (N. M. Filatov, H. Unbehauen)
+                VII. Simulations
+
+                q(k+1) = 1.654q(k) - 1.022q(k-1) + 0.2019q(k-2)
+                        +0.1098u(k) + 0.0792u(k-1) - 0.0229u(k-2)
+                J(k) = (w(k+1) - q(k+1))^2 + 0.0005(u(k))^2
+                w(k) == 5
+                K = 50
+                q(>=0) = 0.1;
+
+                ! used without noise
+        */
+
+        int K = 50;
+        double w = 5.0;
+        double q0 = 0.1;
+
+        LQG_universal lq;
+        lq.rv = RV("u", 1, 0); 
+        RV rvc; 
+        rvc = RV("q", 1, 0);    
+        rvc.add(RV("q", 1, -1));
+        rvc.add(RV("q", 1, -2));        
+        rvc.add(RV("u", 1, -1));
+        rvc.add(RV("u", 1, -2));
+        rvc.add(RV("w", 1, 0));
+        rvc.add(RV("1", 1, 0));
+        lq.set_rvc(rvc);
+        lq.horizon = K;
+
+        Array<linfnEx> model(7);  
+        model(0).A = mat("1.654");
+        model(0).B = vec("0");
+        model(0).rv = RV("q", 1, 0);
+        model(0).rv_ret = RV("q", 1, 1);         
+        model(1).A = mat("-1.022");
+        model(1).B = vec("0");
+        model(1).rv = RV("q", 1, -1);
+        model(1).rv_ret = RV("q", 1, 1);        
+        model(2).A = mat("0.2019");
+        model(2).B = vec("0");
+        model(2).rv = RV("q", 1, -2);
+        model(2).rv_ret = RV("q", 1, 1);        
+        model(3).A = mat("0.1098");
+        model(3).B = vec("0");
+        model(3).rv = RV("u", 1, 0);
+        model(3).rv_ret = RV("q", 1, 1);        
+        model(4).A = mat("0.0792");
+        model(4).B = vec("0");
+        model(4).rv = RV("u", 1, -1);
+        model(4).rv_ret = RV("q", 1, 1);
+        model(5).A = mat("-0.0229");
+        model(5).B = vec("0");
+        model(5).rv = RV("u", 1, -2);
+        model(5).rv_ret = RV("q", 1, 1);        
+        model(6).A = mat("1");
+        model(6).B = vec("0");
+        model(6).rv = RV("w", 1, 0);
+        model(6).rv_ret = RV("w", 1, 1);        
+        lq.Models = model;
+
+        Array<quadraticfn> qloss(2);  
+        qloss(0).Q.setCh(mat("1 -1"));
+        qloss(0).rv = RV("q", 1, 1);
+        qloss(0).rv.add(RV("w", 1, 1)); 
+        qloss(1).Q = mat("0.0005");//automatic sqrt
+        qloss(1).rv = RV("u", 1, 0);    
+        lq.Losses = qloss;
+ 
+        lq.finalLoss.Q.setCh(mat("1 -1"));
+        lq.finalLoss.rv = RV("q", 1, 1);        
+        lq.finalLoss.rv.add(RV("w", 1, 1));     
+
+        lq.validate(); 
+
+        lq.resetTime();
+        lq.redesign();
+
+        int k;
+
+        for(k = K-1; k > 0; k--) lq.redesign();
+        
+        cout << "L: " << lq.getL() << endl;
+
+        vec q(50);
+        q(0) = q0;
+
+        vec u(50);
+
+        //rvc = [q(k), q(k-1), q(k-2), u(k-1), u(k-2), w(k)]
+        vec xrvc(6);
+        xrvc(5) = w;
+                
+
+        //first step
+        xrvc(0) = q(0);
+        xrvc(1) = q0;
+        xrvc(2) = q0;
+        xrvc(3) = 0;
+        xrvc(4) = 0;
+        u(0) = (lq.ctrlaction(xrvc))(0);
+        q(1) = 1.654 * q(0) - 1.022*q0 + 0.2019*q0 + 0.1098*u(0);
+
+        //second step
+        xrvc(0) = q(1);
+        xrvc(1) = q(0);
+        xrvc(2) = q0;
+        xrvc(3) = u(0);
+        xrvc(4) = 0;
+        u(1) = (lq.ctrlaction(xrvc))(0);
+        q(2) = 1.654 * q(1) - 1.022*q(0) + 0.2019*q0 + 0.1098*u(1) + 0.0792*u(0);
+
+        //cycle
+        for(k = 2; k < K-1; k++){
+                xrvc(0) = q(k);
+                xrvc(1) = q(k-1);
+                xrvc(2) = q(k-2);
+                xrvc(3) = u(k-1);
+                xrvc(4) = u(k-2);
+                u(k) = (lq.ctrlaction(xrvc))(0);
+                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);
+        }
+        
+        
+        ofstream log;
+        log.open ("log.txt");
+        for(k = 0; k < K; k++)
+                log << k << "\t" << q(k) << endl;
+        log.close();
+}
+
+/*TEST (matrixdivisionspeedtest){
+        Real_Timer tmr;
+        Uniform_RNG urng;
+
+        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");
+        mat B("0.24835 -0.112361 0.000642142 -1.3721");// = urng(2,6);          
+        mat C(1,4);
+                C.zeros();
+
+        int n = 1;
+        int j,k,l,m;
+        
+        //cout << "A:" << endl << A << endl << "B:" << endl << B << endl;
+
+        cout << "Using inv function: ";
+        tmr.tic();
+                for(j = 0; j < n; j++) C = inv(A)*B;    
+        tmr.toc_print();
+        cout << endl << C << endl;
+        
+        cout << "Using ldutg function: ";
+        tmr.tic();
+                for(j = 0; j < n; j++) ldutg(A,B,C);    
+        tmr.toc_print();
+        cout << endl << C << endl;*/
+
+        /*cout << "Using Gauss elimination: ";
+                int utrows = A.rows();                     
+                int longrow = utrows + B.cols();                    
+                mat Cc(utrows,longrow);
+                        Cc.zeros();
+        tmr.tic();
+                for(j = 0; j < n; j++){  
+                    Cc.set_submatrix(0,utrows-1,0,utrows-1,A);//rrccM
+                    Cc.set_submatrix(0,utrows-1,utrows,longrow-1,B);                
+                    for(k = utrows-1; k >= 0; k--){        
+                           Cc.set_row(k, Cc.get_row(k) / A(k,k));        
+                           for(l = 0;l < k; l++){            
+                                  Cc.set_row(l,Cc.get_row(l) - A(l,k)*Cc.get_row(k));
+                           }                                                      
+                    }    
+                    C = Cc(0,utrows-1,utrows,longrow-1);
+                }
+        tmr.toc_print();
+        cout << endl << C << endl;*/
+        
+        /*cout << "Using Gauss elimination Direct access!: ";
+         //data da pointer co jede jako pole po sloupcich 
+        tmr.tic();
+                for(j = 0; j < n; j++){  
+                    
+                                for(l=0;l<25;l++)
+                                        (Cc._data())[l] = (A._data())[l];
+                                for(l=25;l<50;l++)
+                                        Cc._data()[l] = B._data()[l-25];                    
+                   
+                                    
+                    for(k = utrows-1; k >= 0; k--){       
+                           for(l=0;l<10;l++) Cc._data()[k+5*l] /= A._data()[k+5*k];                                        
+                           for(l = 0;l < k; l++){
+                                  for(m=0;m<10;m++)     Cc._data()[l+5*m] -= A._data()[l+5*k]*Cc._data()[k+5*m];                                          
+                           }                                                      
+                    }    
+                    
+                    for(k=0;k<25;k++)
+                                
+                                        C._data()[k] = Cc._data()[k+25];
+                }
+                
+        tmr.toc_print();                
+        cout << endl << C << endl;
+                
+        cout << "Using Gauss elimination ARRAY: ";
+        tmr.tic();
+        double arA[5][5];                   
+        double arB[5][5];                   
+        double arC[5][5];                   
+        double arCc[5][10];
+        for(k=0;k<5;k++){
+                for(l=0;l<5;l++){
+                        arA[k][l] = A(k,l);
+                        arB[k][l] = B(k,l);
+                }
+        }
+                                            
+                        
+        
+                for(j = 0; j < n; j++){  
+                    for(k=0;k<5;k++){
+                                for(l=0;l<5;l++)
+                                        arCc[k][l] = arA[k][l];
+                                for(l=5;l<10;l++)
+                                        arCc[k][l] = arB[k][l-5];                   
+                    }
+                                    
+                    for(k = utrows-1; k >= 0; k--){       
+                           for(l=0;l<10;l++) arCc[k][l] /= arA[k][k];                                      
+                           for(l = 0;l < k; l++){
+                                  for(m=0;m<10;m++)     arCc[l][m] -= arA[l][k]*arCc[k][m];                                       
+                           }                                                      
+                    }    
+                    
+                    for(k=0;k<5;k++)
+                                for(l=0;l<5;l++)
+                                        arC[k][l] = arCc[k][l+5];
+                }
+                
+                
+        
+        for(k=0;k<5;k++)
+                for(l=0;l<5;l++)
+                        C(k,l) = arC[k][l];                     
+        tmr.toc_print();
+        cout << endl << C << endl;*/
+//}
 
 //TEST (LQGU_large_test){
@@ -1276 +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)
         + (B2.transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1).transpose()*Q7.get_cols(xsize+ysize, xsize+ysize+zsize-1)*B2)(0)
-        + (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);
+        + (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);*/