Show
Ignore:
Timestamp:
10/03/10 23:23:10 (14 years ago)
Author:
vahalam
Message:

dalsi test

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • library/tests/testsuite/LQG_test.cpp

    r1206 r1209  
    700700} 
    701701 
    702 /*TEST (LQGU_PMSM_test){ 
     702TEST (LQGU_PMSM_test){ 
    703703         
    704704        int K = 10; 
    705705        int Kt = 100; 
    706         int N = 50; 
     706        int N = 15; 
    707707 
    708708        double a = 0.9898; 
    … …  
    712712        double e = 0.0149;  
    713713 
    714         double OMEGAt = 2.15; 
     714        double OMEGAt = 1.15; 
    715715        double DELTAt = 0.000125; 
    716716 
    … …  
    772772        P(2,2) = 0.01; 
    773773        P(3,3) = 0.01; 
    774  
    775  
    776         //vec u0(Kt+K); 
    777         //vec u1(Kt+K); 
    778         //mat u(2,Kt+K) 
     774         
    779775        vec u(2); 
    780  
    781         //vec x0(Kt+K); 
    782         //vec x1(Kt+K); 
    783         //vec x2(Kt+K); 
    784         //vec x3(Kt+K); 
    785         //vec x4(Kt+K); 
    786         mat x(5,Kt+K); 
    787  
    788         //vec y0(Kt+K); 
    789         //vec y1(Kt+K); 
    790         mat y(2, Kt+K); 
    791  
    792         mat Pk(5, 5); 
    793776         
    794         mat KK(5, 2); 
     777        mat x(5,Kt+K);   
    795778 
    796779        mat L(2, 5); 
    … …  
    832815        vec losses(N); 
    833816                losses.zeros(); 
    834  
    835         double loss; 
    836817         
    837818        for(n = 0; n < N; n++) { 
    838819                L0.zeros(); 
    839820                L1.zeros(); 
    840                 Pk.zeros(); 
    841                                      
    842                 for(i=0;i<4;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();    
     821                                                     
     822                for(i=0;i<5;i++) x00(i) = x0(i) + sqrt(P(i,i))*randn();    
    843823                        
    844824                for(kt = 0; kt < Kt; kt++){             
    845825                        x.set_col(0, x00); 
    846                         y.set_col(0, x.get_col(0).get(0,1));             
     826 
    847827                        for(k = 0; k < kt+K-1; k++){ 
    848828                                L.set_size(2,5); 
    … …  
    850830                                L.set_row(1, L1.get_col(kt)); 
    851831                                u = L*x.get_col(k); 
    852  
    853                                 /*if(k > 2){ 
    854                                                 cout << "Pk " << Pk << endl; 
    855                                                 cout << "C " << C << endl; 
    856                                                 cout << "R " << R << endl; 
    857                                                 cout << "Pk * C.transpose() " << Pk * C.transpose() << endl; 
    858                                                 cout << "inv(C * Pk * C.transpose() + R) " << inv(C * Pk * C.transpose() + R) << endl; 
    859                                                 cout << "Pk * C.transpose() * inv(C * Pk * C.transpose() + R) " << Pk * C.transpose() * inv(C * Pk * C.transpose() + R) << endl; 
    860                                 }*/ 
    861                                 /*KK = Pk * C.transpose() * inv(C * Pk * C.transpose() + R);                
    862               
    863                                 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();   
    864                                 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(); 
    865                                 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(); 
    866                                 x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt + sqrt(Q(3, 3))*randn();  
    867                                 x(4, k+1) = x(4, k);                
    868  
    869                                 x.set_col(k+1, x.get_col(k+1) - KK * (y.get_col(k) - x.get_col(k).get(0,1)));  
    870                                 randvec(0) = randn(); 
    871                                 randvec(1) = randn(); 
    872                                 y.set_col(k+1, x.get_col(k+1).get(0,1) + R * randvec);   
    873  
    874                                 A(2, 0) = -e*sin(x(3, k)); 
    875                                 A(2, 1) = e*cos(x(3, k)); 
    876                                 A(0, 2) = b*sin(x(3, k)); 
    877                                 A(1, 2) = -b*cos(x(3, k)); 
    878                                 A(0, 3) = b*(x(2, k) + x(4, k))*cos(x(3, k)); 
    879                                 A(1, 3) = b*(x(2, k) + x(4, k))*sin(x(3, k));     
    880                                 A(2, 3) = -e*(x(1, k)*sin(x(3, k) + x(0, k)*cos(x(3, k)))); 
    881                                 A(0, 4) = b*sin(x(3, k)); 
    882                                 A(1, 4) = -b*cos(x(3, k));      
    883 //cout << "A(" << k << "): " << endl << A << endl; 
    884                                 Pk = A * (Pk - Pk * C.transpose() * inv(C * Pk * C.transpose() + R) * C * Pk) * A.transpose() + Q;               
    885 cout << "Pk(" << k << "): " << endl << Pk << endl; 
     832                                if(u(0) > 50) u(0) = 50; 
     833                                else if(u(0) < -50) u(0) = -50; 
     834                                if(u(1) > 50) u(1) = 50; 
     835                                else if(u(1) < -50) u(1) = -50; 
     836             
     837                                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();   
     838                                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(); 
     839                                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(); 
     840                                x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt;// + sqrt(Q(3, 3))*randn();  
     841                                x(4, k+1) = x(4, k);                             
    886842                        } 
    887843                         
    … …  
    905861                        L1.set_col(kt, L.get_row(1).get(0,4)); 
    906862                }         
    907                  
     863        
    908864        x.set_col(0, x00); 
    909                 y.set_col(0, x.get_col(0).get(0,1)); 
    910  
    911                 loss = 0; 
    912  
    913         for(k = 0; k < Kt; k++){                
     865                 
     866        for(k = 0; k < Kt; k++){  
     867         
     868                        L.set_size(2,5); 
    914869                        L.set_row(0, L0.get_col(k)); 
    915870                        L.set_row(1, L1.get_col(k)); 
    916871                        u = L*x.get_col(k);      
    917  
    918                         KK = Pk * C.transpose() * inv(C * Pk * C.transpose() + R); 
    919  
     872                        if(u(0) > 50) u(0) = 50; 
     873                        else if(u(0) < -50) u(0) = -50; 
     874                        if(u(1) > 50) u(1) = 50; 
     875                        else if(u(1) < -50) u(1) = -50; 
     876                                 
    920877            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();   
    921878                        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(); 
    … …  
    923880                        x(3, k+1) = x(3, k) + (x(2, k) + x(4, k))*DELTAt + sqrt(Q(3, 3))*randn();  
    924881                        x(4, k+1) = x(4, k);  
    925  
    926                         loss += (x.get_col(k+1).transpose() * X * x.get_col(k+1).transpose())(0,0); 
    927                         loss += (u.transpose() * Y * u)(0); 
    928                 
    929             x.set_col(k+1, x.get_col(k+1) - KK * (y.get_col(k) - x.get_col(k).get(0,1)));  
    930                         randvec(0) = randn(); 
    931                         randvec(1) = randn(); 
    932                         y.set_col(k+1, x.get_col(k+1).get(0,1) + R * randvec);                    
    933  
    934             A(2, 0) = -e*sin(x(3, k)); 
    935                         A(2, 1) = e*cos(x(3, k)); 
    936                         A(0, 2) = b*sin(x(3, k)); 
    937                         A(1, 2) = -b*cos(x(3, k)); 
    938                         A(0, 3) = b*(x(2, k) + x(4, k))*cos(x(3, k)); 
    939                         A(1, 3) = b*(x(2, k) + x(4, k))*sin(x(3, k));     
    940                         A(2, 3) = -e*(x(1, k)*sin(x(3, k) + x(0, k)*cos(x(3, k)))); 
    941                         A(0, 4) = b*sin(x(3, k)); 
    942                         A(1, 4) = -b*cos(x(3, k));      
    943  
    944                         Pk = A * (Pk - Pk * C.transpose() * inv(C * Pk * C.transpose() + R) * C * Pk) * A.transpose() + Q;                  
    945                 }  
    946  
    947                 losses(n) = loss;         
    948         
     882         
     883                        losses(n) += (x.get_col(k+1).transpose() * X * x.get_col(k+1))(0);// + (u.transpose() * Y * u)(0);                       
     884                }        
    949885        } 
    950886 
    951887        cout << "Ztrata: " << sum(losses)/n << endl; 
     888} 
     889 
     890TEST (LQGU_181_zero){ 
     891        /*                                               v ????? .8189 
     892                y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} 
     893 
     894                S = y^2 + u^2/1000 
     895 
     896                y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} 
     897                y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581 
     898        */ 
     899 
     900        int K = 150; 
     901 
     902        LQG_universal lq; 
     903        lq.rv = RV("ut", 1, 0);  
     904        RV rvc;  
     905        rvc = RV("yt", 1, -1);   
     906        rvc.add(RV("yt", 1, -2)); 
     907        rvc.add(RV("ut", 1, -1));        
     908        rvc.add(RV("1", 1, 0)); 
     909        lq.set_rvc(rvc); 
     910        lq.horizon = K; 
     911 
     912        Array<linfnEx> model(4);   
     913        model(0).A = mat("1.81"); 
     914        model(0).B = vec("0"); 
     915        model(0).rv = RV("yt", 1, -1); 
     916        model(0).rv_ret = RV("yt", 1, 0);         
     917        model(1).A = mat("-0.8189"); 
     918        model(1).B = vec("0"); 
     919        model(1).rv = RV("yt", 1, -2); 
     920        model(1).rv_ret = RV("yt", 1, 0);        
     921        model(2).A = mat("0.00438"); 
     922        model(2).B = vec("0"); 
     923        model(2).rv = RV("ut", 1, 0); 
     924        model(2).rv_ret = RV("yt", 1, 0);        
     925        model(3).A = mat("0.00468"); 
     926        model(3).B = vec("0"); 
     927        model(3).rv = RV("ut", 1, -1); 
     928        model(3).rv_ret = RV("yt", 1, 0);        
     929        lq.Models = model; 
     930 
     931        Array<quadraticfn> qloss(2);   
     932        qloss(0).Q.setCh(mat("1")); 
     933        qloss(0).rv = RV("yt", 1, 0);    
     934        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) 
     935        //qloss(1).Q = mat("0.001");//automatic sqrt 
     936        qloss(1).rv = RV("ut", 1, 0);    
     937        lq.Losses = qloss; 
     938  
     939        lq.finalLoss.Q.setCh(mat("1")); 
     940        lq.finalLoss.rv = RV("yt", 1, 0);        
     941 
     942        lq.validate();  
     943 
     944        lq.resetTime(); 
     945        lq.redesign(); 
     946 
     947        for(int k = K-1; k > 0; k--){ 
     948                //cout << "L: " << lq.getL() << endl; 
     949                lq.redesign(); 
     950        } 
     951 
     952        cout << "L: " << lq.getL() << endl; 
     953} 
     954 
     955TEST (LQGU_181_reqv){ 
     956        /*                                               v ????? .8189 
     957                y_t = 1.81 y_{t-1} -.9 y_{t-2} + .00438 u_t + .00468 u_{t-1} 
     958 
     959                S = y^2 + u^2/1000 
     960 
     961                y_r = 0  :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} 
     962                y_r = 10 :> u_t = -69.0908 y_{t-1} + 46.8955 y_{t-2}  -0.2509 u_{t-1} + 233.7581 
     963        */ 
     964 
     965        int K = 200; 
     966 
     967        double yr = 10.0; 
     968 
     969        LQG_universal lq; 
     970        lq.rv = RV("ut", 1, 0);  
     971        RV rvc;  
     972        rvc = RV("yt", 1, -1);   
     973        rvc.add(RV("yt", 1, -2)); 
     974        rvc.add(RV("ut", 1, -1)); 
     975        rvc.add(RV("yr", 1, 0)); 
     976        rvc.add(RV("1", 1, 0)); 
     977        lq.set_rvc(rvc); 
     978        lq.horizon = K; 
     979 
     980        Array<linfnEx> model(5);   
     981        model(0).A = mat("1.81"); 
     982        model(0).B = vec("0"); 
     983        model(0).rv = RV("yt", 1, -1); 
     984        model(0).rv_ret = RV("yt", 1, 0);         
     985        model(1).A = mat("-0.8189"); 
     986        model(1).B = vec("0"); 
     987        model(1).rv = RV("yt", 1, -2); 
     988        model(1).rv_ret = RV("yt", 1, 0);        
     989        model(2).A = mat("0.00438"); 
     990        model(2).B = vec("0"); 
     991        model(2).rv = RV("ut", 1, 0); 
     992        model(2).rv_ret = RV("yt", 1, 0);        
     993        model(3).A = mat("0.00468"); 
     994        model(3).B = vec("0"); 
     995        model(3).rv = RV("ut", 1, -1); 
     996        model(3).rv_ret = RV("yt", 1, 0);        
     997        model(4).A = mat("1"); 
     998        model(4).B = vec("0"); 
     999        model(4).rv = RV("yr", 1, 0); 
     1000        model(4).rv_ret = RV("yr", 1, 1); 
     1001        lq.Models = model; 
     1002 
     1003        Array<quadraticfn> qloss(2);   
     1004        qloss(0).Q.setCh(mat("1 -1")); 
     1005        qloss(0).rv = RV("yt", 1, 0); 
     1006    qloss(0).rv.add(RV("yr", 1, 0)); 
     1007        qloss(1).Q.setCh(mat("0.03162")); //(1/1000)^(1/2) 
     1008        //qloss(1).Q = mat("0.001");//automatic sqrt 
     1009        qloss(1).rv = RV("ut", 1, 0);    
     1010        lq.Losses = qloss; 
     1011  
     1012        lq.finalLoss.Q.setCh(mat("1 -1")); 
     1013        lq.finalLoss.rv = RV("yt", 1, 0);        
     1014        lq.finalLoss.rv.add(RV("yr", 1, 0));     
     1015 
     1016        lq.validate();  
     1017 
     1018        lq.resetTime(); 
     1019        lq.redesign(); 
     1020 
     1021        for(int k = K-1; k > 0; k--){ 
     1022                //cout << "L: " << lq.getL() << endl; 
     1023                lq.redesign(); 
     1024        } 
     1025 
     1026        cout << "L: " << lq.getL() << endl; 
    9521027} 
    9531028