Changeset 1386

Show
Ignore:
Timestamp:
09/05/11 16:48:42 (14 years ago)
Author:
vahalam
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/pmsm/pmsm_ctrl.h

    r1316 r1386  
    1515using namespace bdm; 
    1616 
     17/**/ 
     18class myEKF{ 
     19private: 
     20        double ial, ibe, ome, the, ual, ube; 
     21        mat A, C, K, P, Q, R; 
     22        double a,b,c,d,e,dt; 
     23public: 
     24        myEKF():A(4,4),C(2,4),K(4,2),P(4,4),Q(4,4),R(2,2){ 
     25                ial = ibe = ome = the = ual = ube = 0.0; 
     26                 
     27                a = 0.9898; 
     28                b = 0.0072; 
     29                c = 0.0361; 
     30                d = 1.0; 
     31                e = 0.0149; 
     32                dt = 0.000125; 
     33                 
     34                A.zeros(); 
     35                C.zeros(); 
     36                K.zeros(); 
     37                P.zeros(); 
     38                Q.zeros(); 
     39                R.zeros(); 
     40                A(0,0) = a; 
     41                A(1,1) = a; 
     42                A(2,2) = d; 
     43                A(3,3) = 1.0; 
     44                A(3,2) = dt; 
     45                C(0,0) = 1.0; 
     46                C(1,1) = 1.0; 
     47                Q(0,0) = 0.1; 
     48                Q(1,1) = 0.1; 
     49                Q(2,2) = 0.1; 
     50                Q(3,3) = 0.01; 
     51                R(0,0) = 0.05; 
     52                R(1,1) = 0.05; 
     53        } 
     54         
     55        vec getEst(double yal, double ybe){ 
     56                //yal = yal; 
     57                //ybe = sqrt(3.0)/3.0 * (2.0*ybe + yal);                 
     58                //yal = 2.0*yal/3.0; 
     59                //ybe = 2.0*ybe/3.0; 
     60         
     61                A(0,2) = b*sin(the); 
     62                A(0,3) = b*ome*cos(the); 
     63                A(1,2) = -b*cos(the); 
     64                A(1,3) = b*ome*sin(the); 
     65                A(2,0) = -e*sin(the); 
     66                A(2,1) = e*cos(the); 
     67                A(2,3) = -e*(ial*cos(the)+ibe*sin(the));                 
     68                         
     69                double tial = a*ial + b*ome*sin(the) + c*ual; 
     70                double tibe = a*ibe - b*ome*cos(the) + c*ube; 
     71                double tome = d*ome + e*(ibe*cos(the) - ial*sin(the)); 
     72                double tthe = the + dt*ome;              
     73         
     74                P = A*P*A.transpose() + Q; 
     75                K = P*C.transpose()*inv(C*P*C.transpose() + R); 
     76                P = P - K*C*P;         
     77         
     78                vec yt(2); 
     79                yt(0) = yal - tial; 
     80                yt(1) = ybe - tibe;  
     81                 
     82                mat Kyt = K*yt;         
     83                tial += Kyt(0,0); 
     84                tibe += Kyt(1,0); 
     85                tome += Kyt(2,0); 
     86                tthe += Kyt(3,0); 
     87                 
     88                ial = tial; 
     89                ibe = tibe; 
     90                ome = tome; 
     91                the = tthe; 
     92 
     93                vec est(4); 
     94                est(0) = tial; 
     95                est(1) = tibe; 
     96                est(2) = tome; 
     97                est(3) = tthe; 
     98                 
     99                return est; 
     100        } 
     101         
     102        void setUab(double _ual, double _ube){ 
     103                ual = _ual; 
     104                ube = _ube; 
     105        } 
     106 
     107}; 
     108/**/ 
    17109 
    18110/*! PI Controller*/ 
    … …  
    136228        PI_ctrl Cuq; 
    137229        PI_ctrl Cud; 
     230/**/ //myEKF mykalm; 
    138231                 
    139232        PMSM_PICtrl():PMSMCtrl(), 
    … …  
    145238        virtual vec ctrlaction(const itpp::vec& cond) { 
    146239                PMSMCtrl::ctrlaction(cond); // fills isa,isb,om,the 
     240                //cout << isa << " " << isb << " "; 
     241/**/            //vec est = mykalm.getEst(isa, isb); 
     242/**/            //isa = est(0); 
     243/**/            //isb = est(1); 
     244/**/            //ome = est(2); 
     245/**/            //the = est(3); 
     246                //cout << isa << " " << isb << endl;     
    147247                 
    148248                double Isd = isa*cos(the)+isb*sin(the); 
    … …  
    168268                uab(0)=Um*cos(beta);               // usx = usa 
    169269                uab(1)=Um*sin(beta);    // usy 
    170                  
     270/**/            //mykalm.setUab(uab(0),uab(1));          
    171271                return uab; 
    172272        }; 
    … …  
    256356                 
    257357                //create prediction 
    258                 p_isa.zeros(); 
     358                /*p_isa.zeros(); 
    259359                p_isb.zeros(); 
    260360                p_ome.zeros(); 
    … …  
    270370                        p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i))); 
    271371                        p_the(i+1) = p_the(i) + Dt*p_ome(i); 
    272                 } 
     372                }*/ 
     373                 
     374                        A(0, 2) = b*sin(the); 
     375                        //A(0, 3) = b*ome*cos(the);                      
     376                        A(0, 4) = b*sin(the); 
     377                        A(1, 2) = -b*cos(the); 
     378                        //A(1, 3) = b*ome*sin(the);                      
     379                        A(1, 4) = -b*cos(the); 
     380                        A(2, 0) = -e*sin(the); 
     381                        A(2, 1) = e*cos(the);    
     382                        //A(2, 3) = -e*(isa*cos(the) + isb*sin(the));            
     383                         
     384                        lq.Models(0).A = A;              
    273385                 
    274386                //create control matrix 
    275387                for(i = rec_hor; i > 0; i--){            
    276388                        //set variable matrices elements (A matrix only) 
    277                         A(0, 2) = b*sin(p_the(i)); 
     389                        /*A(0, 2) = b*sin(p_the(i)); 
    278390                        A(0, 3) = b*(p_ome(i))*cos(p_the(i)); 
    279391                        A(0, 4) = b*sin(p_the(i)); 
    … …  
    285397                        A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i))); 
    286398                         
    287                         lq.Models(0).A = A;              
     399                        lq.Models(0).A = A;     */       
    288400                        lq.redesign(); 
    289401                } 
    … …  
    611723        LQG_universal lq;        
    612724         
    613 //prediction 
    614         vec p_isd, p_isq, p_ome, p_the; 
     725// 
     726//      vec p_isd, p_isq, p_ome, p_the; 
     727        double p_isd, p_isq; 
    615728                 
    616729        PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 
    … …  
    645758         
    646759                //create prediction 
    647                 p_isd.zeros(); 
     760                /*p_isd.zeros(); 
    648761                p_isq.zeros(); 
    649762                p_ome.zeros(); 
    650                 p_the.zeros(); 
    651                 p_isd(0) = isa*cos(the) + isb*sin(the);//isa; 
    652                 p_isq(0) = isb*cos(the) - isa*sin(the);//isb; 
    653                 p_ome(0) = ome; 
     763                p_the.zeros();*/ 
     764                p_isd = isa*cos(the) + isb*sin(the);//isa; 
     765                p_isq = isb*cos(the) - isa*sin(the);//isb; 
     766                /*p_ome(0) = ome; 
    654767                p_the(0) = the; 
    655768                 
    … …  
    659772                        p_ome(i+1) = d*p_ome(i) + e*p_isq(i); 
    660773                        p_the(i+1) = p_the(i) + Dt*p_ome(i);                     
    661                 } 
     774                }*/ 
     775                 
     776                A(0, 1) = Dt*ome; 
     777                //A(0, 2) = Dt*p_isq; 
     778                //A(0, 4) = Dt*p_isq; 
     779                A(1, 0) = -Dt*ome; 
     780                A(1, 2) = /*-Dt*p_isd*/-b; 
     781                A(1, 4) = /*-Dt*p_isd*/-b; 
     782                         
     783                lq.Models(0).A = A;      
    662784                 
    663785                //create control matrix 
    664786                for(i = rec_hor; i > 0; i--){            
    665787                        //set variable matrices elements (A matrix only)                         
    666                         A(0, 1) = Dt*p_ome(i); 
     788                        /*A(0, 1) = Dt*p_ome(i); 
    667789                        A(0, 2) = Dt*p_isq(i); 
    668790                        A(0, 4) = Dt*p_isq(i); 
    669791                        A(1, 0) = -Dt*p_ome(i); 
    670792                        A(1, 2) = -Dt*p_isd(i); 
    671                         A(1, 4) = -Dt*p_isq(i); 
     793                        A(1, 4) = -Dt*p_isd(i); 
    672794                         
    673                         lq.Models(0).A = A;              
     795                        lq.Models(0).A = A;     */       
    674796                        lq.redesign(); 
    675797                } 
    … …  
    729851                Rpd(0, 2) = Rpd(1, 3) = -rpd;            
    730852                 
    731                 p_isd.set_length(rec_hor+1); 
     853                /*p_isd.set_length(rec_hor+1); 
    732854                p_isq.set_length(rec_hor+1); 
    733855                p_ome.set_length(rec_hor+1); 
    734                 p_the.set_length(rec_hor+1); 
     856                p_the.set_length(rec_hor+1);*/ 
    735857 
    736858                Array<quadraticfn> qloss(3); 
    … …  
    774896UIREGISTER(PMSM_LQCtrl_dq2); 
    775897 
     898/*************************************/ 
     899/*              LQ d-q Bicriterial         */ 
     900/*************************************/ 
     901 
     902class PMSM_LQCtrl_bic: public PMSMCtrl{ 
     903public: 
     904/* 
     905PMSMCtrl: 
     906        double isa; 
     907        double isb; 
     908        double ome; 
     909        double the; 
     910        double Ww; 
     911*/       
     912 
     913//PMSM parameters 
     914        const double a; 
     915        const double b; 
     916        const double c; 
     917        const double d; 
     918        const double e; 
     919         
     920//input penalty 
     921        double r; 
     922        double rpd; //penalize differences u - u_old 
     923         
     924//time step 
     925        const double Dt; 
     926         
     927//receding horizon 
     928        int rec_hor; 
     929         
     930//system matrices 
     931        mat A; //5x5 
     932        mat B; //5x2 
     933        //mat C; //2x5 
     934        mat Q; //5x5 
     935        mat R; //2x2 
     936        mat Rpd; //2x4   
     937         
     938//control matrix 
     939        mat L; 
     940        vec uab,udq; 
     941        vec icondpd; 
     942        vec u_old; 
     943         
     944//control maximum 
     945        double MAXu; 
     946        int MAXuflag; 
     947         
     948//lqg controler class 
     949        LQG_universal lq;        
     950         
     951//pomega 
     952        //double Pome; 
     953        double qqq; 
     954        vec bcb; 
     955        double bcbv; 
     956         
     957// 
     958//      vec p_isd, p_isq, p_ome, p_the; 
     959        double p_isd, p_isq; 
     960                 
     961        PMSM_LQCtrl_bic():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 
     962                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
     963                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), //qqq(0.0), 
     964                                uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), bcb(2), bcbv(0.0)       {                        
     965                //set fix matrices elements & dimensions 
     966                A.zeros(); 
     967                        A(0, 0) = A(1, 1) = a; 
     968                        A(2, 1) = e; 
     969                        A(2, 2) = d; 
     970                        A(2, 4) = d - 1; 
     971                        A(3, 2) = A(3, 4) = Dt; 
     972                        A(3, 3) = A(4, 4) = 1.0; 
     973                        //A(5, 5) = 1.0; 
     974                B.zeros(); 
     975                        B(0, 0) = B(1, 1) = c;           
     976                Q.zeros(); 
     977                        Q(2, 2) = 1.0;                   
     978                R.zeros(); 
     979                Rpd.zeros(); 
     980                u_old.zeros();           
     981                 
     982                //Pome = 10.0;   
     983                                         
     984        } 
     985                 
     986         
     987        virtual vec ctrlaction(const itpp::vec& cond) { 
     988                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 
     989                 
     990                int i; 
     991                vec tmp; 
     992                 
     993                lq.resetTime(); 
     994         
     995                //create prediction              
     996                p_isd = isa*cos(the) + isb*sin(the);//isa; 
     997                p_isq = isb*cos(the) - isa*sin(the);//isb; 
     998                                 
     999                A(0, 1) = Dt*ome; 
     1000                //A(0, 2) = Dt*p_isq; 
     1001                //A(0, 4) = Dt*p_isq; 
     1002                A(1, 0) = -Dt*ome; 
     1003                A(1, 2) = /*-Dt*p_isd*/-b; 
     1004                A(1, 4) = /*-Dt*p_isd*/-b; 
     1005                         
     1006                lq.Models(0).A = A;      
     1007                 
     1008                //create control matrix 
     1009                for(i = rec_hor; i > 0; i--){                                    
     1010                        lq.redesign(); 
     1011                } 
     1012                lq.redesign(); 
     1013                 
     1014                L = lq.getL(); 
     1015         
     1016                icondpd(0) = isa*cos(the) + isb*sin(the); 
     1017                icondpd(1) = isb*cos(the) - isa*sin(the); 
     1018                icondpd(2) = ome - Ww; 
     1019                icondpd(3) = the; 
     1020                icondpd(4) = Ww; 
     1021                icondpd(5) = u_old(0); 
     1022                icondpd(6) = u_old(1); 
     1023                //icondpd(7) = sqrt(Pome); 
     1024                icondpd(7) = 0; 
     1025                         
     1026                tmp = L*icondpd;                         
     1027                                 
     1028                udq = tmp(0,1);//(1.0 + abs(Pome/ome)); 
     1029                 
     1030                //bicriterial 
     1031                udq += sign(ome)*bcb; 
     1032                 
     1033                //uab = udq; //set size 
     1034                uab(0) = udq(0)*cos(the) - udq(1)*sin(the); 
     1035                uab(1) = udq(1)*cos(the) + udq(0)*sin(the); 
     1036                 
     1037                if(MAXuflag == 1){ //square cut off 
     1038                        if(uab(0) > MAXu) uab(0) = MAXu; 
     1039                        else if(uab(0) < -MAXu) uab(0) = -MAXu; 
     1040                        if(uab(1) > MAXu) uab(1) = MAXu; 
     1041                        else if(uab(1) < -MAXu) uab(1) = -MAXu; 
     1042                } 
     1043                else if(MAXuflag == 2){ //circular cut off 
     1044                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 
     1045                        double uangl = atan2(uab(1), uab(0)); 
     1046                        if (uampl > MAXu) uampl = MAXu; 
     1047                        uab(0) = uampl*cos(uangl); 
     1048                        uab(1) = uampl*sin(uangl); 
     1049                } 
     1050                //else{ //(MAXuflag == 0) //no cut off } 
     1051                 
     1052                //variance Pome evolution 
     1053                //Pome *= 1.0/(1.0+abs(Pome/ome)); 
     1054                 
     1055                u_old = udq; 
     1056         
     1057                return uab; 
     1058        } 
     1059         
     1060        void from_setting(const Setting &set){ 
     1061                PMSMCtrl::from_setting(set); 
     1062                UI::get(r,set, "r", UI::optional); 
     1063                UI::get(rec_hor,set, "h", UI::optional); 
     1064                UI::get(MAXu,set, "MAXu", UI::optional); 
     1065                UI::get(MAXuflag,set, "MAXuflag", UI::optional); 
     1066                UI::get(rpd,set, "rpd", UI::optional); 
     1067                //UI::get(qqq,set, "qqq", UI::optional); 
     1068                UI::get(bcbv,set, "bcbv", UI::optional); 
     1069        } 
     1070 
     1071        void validate(){ 
     1072                R(0, 0) = R(1, 1) = r; 
     1073                Rpd(0, 0) = Rpd(1, 1) = rpd;             
     1074                Rpd(0, 2) = Rpd(1, 3) = -rpd;    
     1075                //Q(5, 5) = qqq;         
     1076                 
     1077                //bicriterial 
     1078                bcb(0) = -bcbv; 
     1079                bcb(1) = bcbv; 
     1080                 
     1081                /*p_isd.set_length(rec_hor+1); 
     1082                p_isq.set_length(rec_hor+1); 
     1083                p_ome.set_length(rec_hor+1); 
     1084                p_the.set_length(rec_hor+1);*/ 
     1085 
     1086                Array<quadraticfn> qloss(3); 
     1087                qloss(0).Q.setCh(Q); 
     1088                qloss(0).rv = RV("x", 5, 1);             
     1089                qloss(1).Q.setCh(R); 
     1090                qloss(1).rv = RV("u", 2, 0); 
     1091                qloss(2).Q.setCh(Rpd); 
     1092                qloss(2).rv = RV("u", 2, 0); 
     1093                qloss(2).rv.add(RV("u", 2, -1));                 
     1094                lq.Losses = qloss;               
     1095 
     1096                //set lq 
     1097                lq.rv = RV("u", 2, 0);           
     1098                RV tmp = RV("x", 5, 0); 
     1099                tmp.add(RV("u", 2, -1));         
     1100                lq.set_rvc(tmp); 
     1101                 
     1102                lq.horizon = rec_hor;    
     1103                 
     1104                Array<linfnEx> model(2); 
     1105                model(0).A = A; 
     1106                model(0).B = vec("0 0 0 0 0"); 
     1107                model(0).rv = RV("x", 5, 0); 
     1108                model(0).rv_ret = RV("x", 5, 1); 
     1109                model(1).A = B; 
     1110                model(1).B = vec("0 0"); 
     1111                model(1).rv = RV("u", 2, 0); 
     1112                model(1).rv_ret = RV("x", 5, 1); 
     1113                lq.Models = model; 
     1114                 
     1115                lq.finalLoss.Q.setCh(Q); 
     1116                lq.finalLoss.rv = RV("x", 5, 1); 
     1117                                 
     1118                lq.validate(); 
     1119                                                 
     1120                uab.zeros(); 
     1121                udq.zeros();             
     1122        } 
     1123}; 
     1124UIREGISTER(PMSM_LQCtrl_bic); 
     1125 
     1126/*************************************/ 
     1127/*                      LQ d-q 1 bicriterial 2*/ 
     1128/*************************************/ 
     1129 
     1130class PMSM_LQCtrl_bic2: public PMSMCtrl{ 
     1131public: 
     1132/* 
     1133PMSMCtrl: 
     1134        double isa; 
     1135        double isb; 
     1136        double ome; 
     1137        double the; 
     1138        double Ww; 
     1139*/       
     1140 
     1141//PMSM parameters 
     1142        const double a; 
     1143        const double b; 
     1144        const double c; 
     1145        const double d; 
     1146        const double e; 
     1147         
     1148//input penalty 
     1149        double r; 
     1150        double rpd; //penalize differences u - u_old 
     1151         
     1152//time step 
     1153        const double Dt; 
     1154         
     1155//receding horizon 
     1156        int rec_hor; 
     1157         
     1158//system matrices 
     1159        mat A; //5x5 
     1160        mat B; //5x2 
     1161        //mat C; //2x5 
     1162        mat Q; //5x5 
     1163        mat R; //2x2 
     1164        mat Rpd; //2x4 
     1165         
     1166//control matrix 
     1167        mat L; 
     1168        vec uab;         
     1169        vec icondpd; 
     1170        vec u_old; 
     1171         
     1172//control maximum 
     1173        double MAXu; 
     1174        int MAXuflag; 
     1175         
     1176//lqg controler class 
     1177        LQG_universal lq;        
     1178         
     1179//prediction 
     1180        vec p_isa, p_isb, p_ome, p_the; 
     1181         
     1182//bicrit param 
     1183        double bcbv; 
     1184         
     1185         
     1186//pi version     
     1187        PI_ctrl Cwq; 
     1188        PI_ctrl Cuq; 
     1189        PI_ctrl Cud; 
     1190         
     1191         
     1192                 
     1193        PMSM_LQCtrl_bic2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 
     1194                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
     1195                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 
     1196                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), 
     1197                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),  
     1198                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200), 
     1199                                Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/ 
     1200                                Cwq(10.0, 0.0005, -1200, 1200),  
     1201                                Cuq(0.4, 0.0005, -1200, 1200), 
     1202                                Cud(0.4, 0.0005, -1200, 1200)   {                        
     1203                //set fix matrices elements & dimensions 
     1204                A.zeros(); 
     1205                        A(0, 0) = A(1, 1) = a; 
     1206                        A(1, 2) = A(1, 4) = -b; 
     1207                        A(2, 1) = e; 
     1208                        A(2, 2) = d; 
     1209                        A(2, 4) = d - 1; 
     1210                        A(3, 2) = A(3, 4) = Dt; 
     1211                        A(3, 3) = A(4, 4) = 1.0; 
     1212                B.zeros(); 
     1213                        B(0, 0) = B(1, 1) = c;           
     1214                Q.zeros(); 
     1215                        Q(2, 2) = 1.0; 
     1216                R.zeros(); 
     1217                Rpd.zeros(); 
     1218                u_old.zeros();                                           
     1219        } 
     1220                 
     1221         
     1222        virtual vec ctrlaction(const itpp::vec& cond) { 
     1223                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 
     1224                                 
     1225                vec udq; 
     1226                vec tmp; 
     1227                 
     1228                lq.resetTime(); 
     1229                                 
     1230                L = lq.getL(); 
     1231                 
     1232                icondpd(0) = isa*cos(the) + isb*sin(the); 
     1233                icondpd(1) = isb*cos(the) - isa*sin(the); 
     1234                icondpd(2) = ome - Ww; 
     1235                icondpd(3) = the; 
     1236                icondpd(4) = Ww; 
     1237                icondpd(5) = u_old(0); 
     1238                icondpd(6) = u_old(1); 
     1239                icondpd(7) = 0;  
     1240                         
     1241                tmp = L*icondpd;                 
     1242                                                 
     1243                udq = tmp(0,1); 
     1244                 
     1245                 
     1246                 
     1247                double Isd = isa*cos(the)+isb*sin(the); 
     1248                double Isq = isb*cos(the)-isa*sin(the); 
     1249                 
     1250                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec 
     1251                 
     1252                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0); 
     1253                udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0); 
     1254                 
     1255                const double Ls0=0.003465; // inductance 
     1256                const double Fmag0= 0.1989; // Magnetic?? 
     1257                 
     1258                udq(0)-=Ls0*ome*Iqw; // har 
     1259                udq(1)+=Fmag0*ome;       
     1260                 
     1261                 
     1262                 
     1263                //bicriterial 
     1264                double omerand = ome;// + sqrt(5.0e-6)*randn(); 
     1265                udq(0) -= sign(omerand)*bcbv; 
     1266                udq(1) += sign(omerand)*bcbv;    
     1267                 
     1268                                 
     1269                //uab = udq; //set size 
     1270                uab(0) = udq(0)*cos(the) - udq(1)*sin(the); 
     1271                uab(1) = udq(1)*cos(the) + udq(0)*sin(the); 
     1272                 
     1273                if(MAXuflag == 1){ //square cut off 
     1274                        if(uab(0) > MAXu) uab(0) = MAXu; 
     1275                        else if(uab(0) < -MAXu) uab(0) = -MAXu; 
     1276                        if(uab(1) > MAXu) uab(1) = MAXu; 
     1277                        else if(uab(1) < -MAXu) uab(1) = -MAXu; 
     1278                } 
     1279                else if(MAXuflag == 2){ //circular cut off 
     1280                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 
     1281                        double uangl = atan2(uab(1), uab(0)); 
     1282                        if (uampl > MAXu) uampl = MAXu; 
     1283                        uab(0) = uampl*cos(uangl); 
     1284                        uab(1) = uampl*sin(uangl); 
     1285                } 
     1286                //else{ //(MAXuflag == 0) //no cut off } 
     1287                 
     1288                u_old = udq; 
     1289                 
     1290                return uab;                      
     1291        } 
     1292         
     1293        void from_setting(const Setting &set){ 
     1294                PMSMCtrl::from_setting(set); 
     1295                UI::get(r,set, "r", UI::optional); 
     1296                UI::get(rec_hor,set, "h", UI::optional); 
     1297                UI::get(MAXu,set, "MAXu", UI::optional); 
     1298                UI::get(MAXuflag,set, "MAXuflag", UI::optional); 
     1299                UI::get(rpd,set, "rpd", UI::optional);   
     1300                UI::get(bcbv,set, "bcbv", UI::optional);         
     1301        } 
     1302 
     1303        void validate(){ 
     1304                R(0, 0) = R(1, 1) = r; 
     1305                Rpd(0, 0) = Rpd(1, 1) = rpd; 
     1306                Rpd(0, 2) = Rpd(1, 3) = -rpd; 
     1307                                 
     1308                p_isa.set_length(rec_hor+1); 
     1309                p_isb.set_length(rec_hor+1); 
     1310                p_ome.set_length(rec_hor+1); 
     1311                p_the.set_length(rec_hor+1); 
     1312 
     1313                Array<quadraticfn> qloss(3); 
     1314                qloss(0).Q.setCh(Q); 
     1315                qloss(0).rv = RV("x", 5, 1);             
     1316                qloss(1).Q.setCh(R); 
     1317                qloss(1).rv = RV("u", 2, 0); 
     1318                qloss(2).Q.setCh(Rpd);           
     1319                qloss(2).rv = RV("u", 2, 0); 
     1320                qloss(2).rv.add(RV("u", 2, -1));                 
     1321                lq.Losses = qloss;               
     1322 
     1323                //set lq 
     1324                lq.rv = RV("u", 2, 0);   
     1325                RV tmp = RV("x", 5, 0); 
     1326                tmp.add(RV("u", 2, -1)); 
     1327                lq.set_rvc(tmp);                 
     1328                lq.horizon = rec_hor;    
     1329                 
     1330                Array<linfnEx> model(2); 
     1331                model(0).A = A; 
     1332                model(0).B = vec("0 0 0 0 0"); 
     1333                model(0).rv = RV("x", 5, 0); 
     1334                model(0).rv_ret = RV("x", 5, 1); 
     1335                model(1).A = B; 
     1336                model(1).B = vec("0 0"); 
     1337                model(1).rv = RV("u", 2, 0); 
     1338                model(1).rv_ret = RV("x", 5, 1); 
     1339                lq.Models = model; 
     1340                 
     1341                lq.finalLoss.Q.setCh(Q); 
     1342                lq.finalLoss.rv = RV("x", 5, 1); 
     1343                                 
     1344                lq.validate(); 
     1345                                                 
     1346                uab.zeros(); 
     1347                                 
     1348                //create control matrix 
     1349                for(int i = rec_hor; i > 0; i--){                                                                
     1350                        lq.redesign(); 
     1351                } 
     1352                lq.redesign(); 
     1353        } 
     1354}; 
     1355UIREGISTER(PMSM_LQCtrl_bic2); 
     1356 
     1357/*************************************/ 
     1358/*                      PI bicriterial 3 s vice EKF pro urceni nejlepsi bikrit*/ 
     1359/*************************************/ 
     1360 
     1361class PMSM_LQCtrl_bic3: public PMSMCtrl{ 
     1362public: 
     1363/* 
     1364PMSMCtrl: 
     1365        double isa; 
     1366        double isb; 
     1367        double ome; 
     1368        double the; 
     1369        double Ww; 
     1370*/       
     1371 
     1372//PMSM parameters 
     1373        const double a; 
     1374        const double b; 
     1375        const double c; 
     1376        const double d; 
     1377        const double e; 
     1378         
     1379//input penalty 
     1380        double r; 
     1381        double rpd; //penalize differences u - u_old 
     1382         
     1383//time step 
     1384        const double Dt; 
     1385         
     1386//receding horizon 
     1387        int rec_hor; 
     1388         
     1389//system matrices 
     1390        mat A; //5x5 
     1391        mat B; //5x2 
     1392        //mat C; //2x5 
     1393        mat Q; //5x5 
     1394        mat R; //2x2 
     1395        mat Rpd; //2x4 
     1396         
     1397//control matrix 
     1398        mat L; 
     1399        vec uab;         
     1400        vec icondpd; 
     1401        vec u_old; 
     1402         
     1403//control maximum 
     1404        double MAXu; 
     1405        int MAXuflag; 
     1406         
     1407//lqg controler class 
     1408        LQG_universal lq;        
     1409         
     1410//prediction 
     1411        vec p_isa, p_isb, p_ome, p_the; 
     1412         
     1413//bicrit param 
     1414        double bcbv; 
     1415         
     1416         
     1417//pi version     
     1418        PI_ctrl Cwq; 
     1419        PI_ctrl Cuq; 
     1420        PI_ctrl Cud; 
     1421         
     1422// 5KF 
     1423        mat Ptp, Kt, Ared, Cred, Qred, Rred; 
     1424        mat Pt1,Pt2,Pt3,Pt4,Pt5; 
     1425        vec varPth; 
     1426        int timeid; 
     1427        ofstream f; 
     1428 
     1429        int biver; 
     1430         
     1431// inj 
     1432        double injkon, injome, injphi; 
     1433         
     1434                 
     1435        PMSM_LQCtrl_bic3():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 
     1436                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
     1437                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), biver(0), 
     1438                                uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), 
     1439                                Ptp(2,2),Kt(2,2),Ared(2,2),Cred(2,2),Qred(2,2),Rred(2,2), 
     1440                                Pt1(2,2),Pt2(2,2),Pt3(2,2),Pt4(2,2),Pt5(2,2), varPth(5),                                 
     1441                                /*Cwq(3.0, 3.0*0.000125/0.1, -1200, 1200),  
     1442                                Cuq(20.0, 20.0*0.000125/0.005, -1200, 1200), 
     1443                                Cud(20.0, 20.0*0.000125/0.005, -1200, 1200)*/ 
     1444                                Cwq(10.0, 0.0005, -1200, 1200),  
     1445                                Cuq(0.4, 0.0005, -1200, 1200), 
     1446                                Cud(0.4, 0.0005, -1200, 1200)   {                        
     1447                //set fix matrices elements & dimensions 
     1448                A.zeros(); 
     1449                        A(0, 0) = A(1, 1) = a; 
     1450                        A(1, 2) = A(1, 4) = -b; 
     1451                        A(2, 1) = e; 
     1452                        A(2, 2) = d; 
     1453                        A(2, 4) = d - 1; 
     1454                        A(3, 2) = A(3, 4) = Dt; 
     1455                        A(3, 3) = A(4, 4) = 1.0; 
     1456                B.zeros(); 
     1457                        B(0, 0) = B(1, 1) = c;           
     1458                Q.zeros(); 
     1459                        Q(2, 2) = 1.0; 
     1460                R.zeros(); 
     1461                Rpd.zeros(); 
     1462                u_old.zeros();   
     1463                 
     1464                Ared(0,0) = d; 
     1465                Ared(1,0) = Dt; 
     1466                Ared(1,1) = 1.0; 
     1467                Qred(0,0) = 0.1; 
     1468                Qred(1,1) = 0.01; 
     1469                Qred(0,0) = Qred(1,1) = 0.5;  
     1470                Pt1(0,0) = Pt2(0,0) = Pt3(0,0) = Pt4(0,0) = Pt5(0,0) = 1.0;      
     1471                Pt1(1,1) = Pt2(1,1) = Pt3(1,1) = Pt4(1,1) = Pt5(1,1) = 1.0;      
     1472                 
     1473                timeid = 0;                      
     1474                f.open("skf.dat", ios::out); 
     1475                 
     1476                injkon = injome = injphi = 0.0; 
     1477        } 
     1478         
     1479        ~PMSM_LQCtrl_bic3(){ 
     1480                f.close(); 
     1481        }        
     1482         
     1483        virtual vec ctrlaction(const itpp::vec& cond) { 
     1484                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 
     1485                                 
     1486                vec udq(2); 
     1487                //vec tmp; 
     1488                 
     1489                /*lq.resetTime(); 
     1490                                 
     1491                L = lq.getL(); 
     1492                 
     1493                icondpd(0) = isa*cos(the) + isb*sin(the); 
     1494                icondpd(1) = isb*cos(the) - isa*sin(the); 
     1495                icondpd(2) = ome - Ww; 
     1496                icondpd(3) = the; 
     1497                icondpd(4) = Ww; 
     1498                icondpd(5) = u_old(0); 
     1499                icondpd(6) = u_old(1); 
     1500                icondpd(7) = 0;  
     1501                         
     1502                tmp = L*icondpd;                 
     1503                                                 
     1504                udq = tmp(0,1);*/ 
     1505                 
     1506                 
     1507                 
     1508                double Isd = isa*cos(the)+isb*sin(the); 
     1509                double Isq = isb*cos(the)-isa*sin(the); 
     1510                 
     1511                double Iqw=(Cwq.ctrlaction(vec_1(Ww-ome)))(0); // conversion from vec 
     1512                 
     1513                udq(0) = (Cud.ctrlaction(vec_1(-Isd)))(0); 
     1514                udq(1) = (Cuq.ctrlaction(vec_1(Iqw-Isq)))(0); 
     1515                 
     1516                const double Ls0=0.003465; // inductance 
     1517                const double Fmag0= 0.1989; // Magnetic?? 
     1518                 
     1519                udq(0)-=Ls0*ome*Iqw; // har 
     1520                udq(1)+=Fmag0*ome;       
     1521                 
     1522                 
     1523                 
     1524                //bicriterial///////////////////////////////////////////////////////////// 
     1525                //verze 1: signum 
     1526                if(biver == 1){ 
     1527                        udq(0) -= sign(ome)*bcbv; 
     1528                        udq(1) += sign(ome)*bcbv; 
     1529                } 
     1530                //*/     
     1531                 
     1532                //verze 2: casovy posun 
     1533                if(biver == 2){ 
     1534                     double psi = (d*d - b*e)*ome + (a + d)*e*Isq - e*Dt*Isd*ome; 
     1535                     udq(1) += sign(psi)*bcbv; 
     1536                     
     1537                     double ksi = d*psi + a*c*e*udq(1) - e*(a*b + b*d + a*Dt*(1+d)*Isd)*ome + e*(a*a - b*e - a*e*Dt*Isd)*Isq - e*Dt*Dt*(d*ome + e*Isq)*Isq*ome; 
     1538                     udq(0) -= sign(ksi)*bcbv; 
     1539          } 
     1540          //*/ 
     1541           
     1542          //verze 3: 3KFdq  
     1543          if(biver == 3){    
     1544                     double du_al = bcbv*cos(the); 
     1545                        double du_be = bcbv*sin(the);       
     1546                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + du_al); 
     1547                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + du_be);     
     1548                        double om = d*ome + e*(isb*cos(the) - isa*sin(the)); 
     1549                        double th = the + Dt*ome;                
     1550                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th)); 
     1551                        Cred(0,0) = b*sin(th); 
     1552                        Cred(0,1) = b*om*cos(th); 
     1553                        Cred(1,0) = -b*cos(th); 
     1554                        Cred(1,1) = b*om*sin(th);                
     1555                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1556                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1557                        Pt1 = (eye(2) - Kt*Cred)*Ptp; 
     1558                        varPth(0) = Pt1(1,1);            
     1559                 
     1560                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - du_al); 
     1561                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - du_be);                                
     1562                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1563                        Ptp = Ared*Pt2*Ared.T() + Qred; 
     1564                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1565                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1566                        varPth(1) = Pt2(1,1); 
     1567                                 
     1568                        ia = a*isa + b*ome*sin(the) + c*(uab(0)); 
     1569                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                                
     1570                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1571                        Ptp = Ared*Pt5*Ared.T() + Qred; 
     1572                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1573                        Pt5 = (eye(2) - Kt*Cred)*Ptp; 
     1574                        varPth(2) = Pt5(1,1); 
     1575                                 
     1576                        int minindex = 0; 
     1577                        for(int i = 1; i < 3; i++){ 
     1578                                if(varPth(i) < varPth(minindex)){                                
     1579                                        minindex = i; 
     1580                                } 
     1581                        } 
     1582                 
     1583                        f << timeid << "\t" << minindex << endl; 
     1584                        timeid++; 
     1585                 
     1586                        switch(minindex){ 
     1587                                case 0: 
     1588                                        udq(0) += bcbv;                          
     1589                                        break; 
     1590                                case 1: 
     1591                                        udq(0) -= bcbv;                          
     1592                                        break;                   
     1593                                case 2:                          
     1594                                        break; 
     1595                        } 
     1596                } 
     1597          //*/ 
     1598           
     1599          //verze 5: konst. v d 
     1600          if(biver == 5){ 
     1601                udq(0) += 2*bcbv; 
     1602          } 
     1603          //*/ 
     1604           
     1605          //verze 6: injektaz v dq 
     1606          if(biver == 6){ 
     1607                udq(0) += injkon*cos(injome*Dt*timeid + injphi); 
     1608                //udq(1) += injkon*cos(injome*Dt*timeid + injphi); 
     1609                 
     1610                timeid++; 
     1611          } 
     1612          //*/ 
     1613           
     1614          //verze 8: injektaz v dq 
     1615          if(biver == 8){ 
     1616                udq(0) += injkon*cos(injome*Dt*timeid + injphi); 
     1617                udq(1) += injkon*sin(injome*Dt*timeid + injphi); 
     1618                 
     1619                timeid++; 
     1620          } 
     1621          //*/ 
     1622           
     1623          //uab = udq; //set size 
     1624                uab(0) = udq(0)*cos(the) - udq(1)*sin(the); 
     1625                uab(1) = udq(1)*cos(the) + udq(0)*sin(the); 
     1626           
     1627          //verze 4: 5KF           
     1628          if(biver == 4){ 
     1629                        double ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
     1630                        double ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);     
     1631                        double om = d*ome + e*(isb*cos(the) - isa*sin(the)); 
     1632                        double th = the + Dt*ome;                
     1633                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th)); 
     1634                        Cred(0,0) = b*sin(th); 
     1635                        Cred(0,1) = b*om*cos(th); 
     1636                        Cred(1,0) = -b*cos(th); 
     1637                        Cred(1,1) = b*om*sin(th);                
     1638                        Ptp = Ared*Pt1*Ared.T() + Qred; 
     1639                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1640                        Pt1 = (eye(2) - Kt*Cred)*Ptp; 
     1641                        varPth(0) = Pt1(1,1); 
     1642                 
     1643                        ia = a*isa + b*ome*sin(the) + c*(uab(0) + bcbv); 
     1644                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1645                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1646                        Ptp = Ared*Pt2*Ared.T() + Qred; 
     1647                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1648                        Pt2 = (eye(2) - Kt*Cred)*Ptp; 
     1649                        varPth(1) = Pt2(1,1); 
     1650                 
     1651                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
     1652                        ib = a*isb - b*ome*cos(the) + c*(uab(1) + bcbv);                                 
     1653                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1654                        Ptp = Ared*Pt3*Ared.T() + Qred; 
     1655                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1656                        Pt3 = (eye(2) - Kt*Cred)*Ptp; 
     1657                        varPth(2) = Pt3(1,1); 
     1658                 
     1659                        ia = a*isa + b*ome*sin(the) + c*(uab(0) - bcbv); 
     1660                        ib = a*isb - b*ome*cos(the) + c*(uab(1) - bcbv);                                 
     1661                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1662                        Ptp = Ared*Pt4*Ared.T() + Qred; 
     1663                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1664                        Pt4 = (eye(2) - Kt*Cred)*Ptp; 
     1665                        varPth(3) = Pt4(1,1); 
     1666                 
     1667                        ia = a*isa + b*ome*sin(the) + c*(uab(0)); 
     1668                        ib = a*isb - b*ome*cos(the) + c*(uab(1));                                
     1669                        Ared(0,1) = -e*(ib*sin(th) + ia*cos(th));                                
     1670                        Ptp = Ared*Pt5*Ared.T() + Qred; 
     1671                        Kt = Ptp*Cred.T()*inv(Cred*Ptp*Cred.T() + Rred); 
     1672                        Pt5 = (eye(2) - Kt*Cred)*Ptp; 
     1673                        varPth(4) = Pt5(1,1); 
     1674                                 
     1675                        int minindex = 0; 
     1676                        for(int i = 1; i < 5; i++){ 
     1677                                if(varPth(i) < varPth(minindex)){                                
     1678                                        minindex = i; 
     1679                                } 
     1680                        } 
     1681                 
     1682                        f << timeid << "\t" << minindex << endl; 
     1683                        timeid++; 
     1684                 
     1685                        switch(minindex){ 
     1686                                case 0: 
     1687                                        uab(0) += bcbv; 
     1688                                        uab(1) += bcbv; 
     1689                                        break; 
     1690                                case 1: 
     1691                                        uab(0) += bcbv; 
     1692                                        uab(1) -= bcbv; 
     1693                                        break; 
     1694                                case 2: 
     1695                                        uab(0) -= bcbv; 
     1696                                        uab(1) += bcbv; 
     1697                                        break; 
     1698                                case 3: 
     1699                                        uab(0) -= bcbv; 
     1700                                        uab(1) -= bcbv; 
     1701                                        break; 
     1702                                case 4:                          
     1703                                        break; 
     1704                        } 
     1705                } 
     1706          //*/ 
     1707           
     1708          //verze 7: injektaz v alfa beta 
     1709          if(biver == 7){ 
     1710                uab(0) += injkon*cos(injome*Dt*timeid + injphi); 
     1711                uab(1) += injkon*sin(injome*Dt*timeid + injphi); 
     1712                 
     1713                timeid++; 
     1714          } 
     1715          //*/ 
     1716                 
     1717                ////////////////////////////////////////////////////////////////////////// 
     1718                                 
     1719                 
     1720                 
     1721                if(MAXuflag == 1){ //square cut off 
     1722                        if(uab(0) > MAXu) uab(0) = MAXu; 
     1723                        else if(uab(0) < -MAXu) uab(0) = -MAXu; 
     1724                        if(uab(1) > MAXu) uab(1) = MAXu; 
     1725                        else if(uab(1) < -MAXu) uab(1) = -MAXu; 
     1726                } 
     1727                else if(MAXuflag == 2){ //circular cut off 
     1728                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 
     1729                        double uangl = atan2(uab(1), uab(0)); 
     1730                        if (uampl > MAXu) uampl = MAXu; 
     1731                        uab(0) = uampl*cos(uangl); 
     1732                        uab(1) = uampl*sin(uangl); 
     1733                } 
     1734                //else{ //(MAXuflag == 0) //no cut off } 
     1735                 
     1736                //u_old = udq; 
     1737                 
     1738                return uab;                      
     1739        } 
     1740         
     1741        void from_setting(const Setting &set){ 
     1742                PMSMCtrl::from_setting(set); 
     1743                UI::get(r,set, "r", UI::optional); 
     1744                UI::get(rec_hor,set, "h", UI::optional); 
     1745                UI::get(MAXu,set, "MAXu", UI::optional); 
     1746                UI::get(MAXuflag,set, "MAXuflag", UI::optional); 
     1747                UI::get(rpd,set, "rpd", UI::optional);   
     1748                UI::get(bcbv,set, "bcbv", UI::optional);         
     1749                UI::get(biver,set, "biver", UI::optional); 
     1750                UI::get(injkon,set, "injkon", UI::optional); 
     1751                UI::get(injome,set, "injome", UI::optional); 
     1752                UI::get(injphi,set, "injphi", UI::optional); 
     1753        } 
     1754 
     1755        void validate(){ 
     1756                R(0, 0) = R(1, 1) = r; 
     1757                Rpd(0, 0) = Rpd(1, 1) = rpd; 
     1758                Rpd(0, 2) = Rpd(1, 3) = -rpd; 
     1759                                 
     1760                p_isa.set_length(rec_hor+1); 
     1761                p_isb.set_length(rec_hor+1); 
     1762                p_ome.set_length(rec_hor+1); 
     1763                p_the.set_length(rec_hor+1); 
     1764 
     1765                Array<quadraticfn> qloss(3); 
     1766                qloss(0).Q.setCh(Q); 
     1767                qloss(0).rv = RV("x", 5, 1);             
     1768                qloss(1).Q.setCh(R); 
     1769                qloss(1).rv = RV("u", 2, 0); 
     1770                qloss(2).Q.setCh(Rpd);           
     1771                qloss(2).rv = RV("u", 2, 0); 
     1772                qloss(2).rv.add(RV("u", 2, -1));                 
     1773                lq.Losses = qloss;               
     1774 
     1775                //set lq 
     1776                lq.rv = RV("u", 2, 0);   
     1777                RV tmp = RV("x", 5, 0); 
     1778                tmp.add(RV("u", 2, -1)); 
     1779                lq.set_rvc(tmp);                 
     1780                lq.horizon = rec_hor;    
     1781                 
     1782                Array<linfnEx> model(2); 
     1783                model(0).A = A; 
     1784                model(0).B = vec("0 0 0 0 0"); 
     1785                model(0).rv = RV("x", 5, 0); 
     1786                model(0).rv_ret = RV("x", 5, 1); 
     1787                model(1).A = B; 
     1788                model(1).B = vec("0 0"); 
     1789                model(1).rv = RV("u", 2, 0); 
     1790                model(1).rv_ret = RV("x", 5, 1); 
     1791                lq.Models = model; 
     1792                 
     1793                lq.finalLoss.Q.setCh(Q); 
     1794                lq.finalLoss.rv = RV("x", 5, 1); 
     1795                                 
     1796                lq.validate(); 
     1797                                                 
     1798                uab.zeros(); 
     1799                                 
     1800                //create control matrix 
     1801                for(int i = rec_hor; i > 0; i--){                                                                
     1802                        lq.redesign(); 
     1803                } 
     1804                lq.redesign(); 
     1805        } 
     1806}; 
     1807UIREGISTER(PMSM_LQCtrl_bic3); 
     1808 
     1809/*************************************/ 
     1810/*                      LQ_EKF_Inj                */ 
     1811/*************************************/ 
     1812 
     1813/********************************************************************************/ 
     1814/*      !!! potrebuje nutne rizeni z cidla, protoze si EKF dela sam                             */ 
     1815/********************************************************************************/ 
     1816 
     1817class PMSM_LQCtrl_EKF_Inj: public PMSMCtrl{ 
     1818public: 
     1819/* 
     1820PMSMCtrl: 
     1821        double isa; 
     1822        double isb; 
     1823        double ome; 
     1824        double the; 
     1825        double Ww; 
     1826*/       
     1827 
     1828//PMSM parameters 
     1829        const double a; 
     1830        const double b; 
     1831        const double c; 
     1832        const double d; 
     1833        const double e; 
     1834         
     1835        const double Rs; 
     1836        const double Lq; 
     1837        const double Ld; 
     1838         
     1839//input penalty 
     1840        double r; 
     1841        double rpd; //penalize differences u - u_old 
     1842         
     1843//time step 
     1844        const double Dt; 
     1845         
     1846//receding horizon 
     1847        int rec_hor; 
     1848         
     1849//system matrices 
     1850        mat A; //5x5 
     1851        mat B; //5x2 
     1852        //mat C; //2x5 
     1853        mat Q; //5x5 
     1854        mat R; //2x2 
     1855        mat Rpd; //2x4   
     1856         
     1857//control matrix 
     1858        mat L; 
     1859        vec uab,udq; 
     1860        vec icondpd; 
     1861        vec u_old; 
     1862         
     1863//control maximum 
     1864        double MAXu; 
     1865        int MAXuflag; 
     1866         
     1867//lqg controler class 
     1868        LQG_universal lq;        
     1869         
     1870//EKF input 
     1871        vec y; 
     1872        double isaold, isbold, omeold; 
     1873        mat Akf; 
     1874        mat Ckf; 
     1875        mat Qkf; 
     1876        mat Rkf; 
     1877        vec uoldab; 
     1878        mat Pkf;         
     1879        const double R0,R10; 
     1880         
     1881//Inj 
     1882        double iqold, iqnew; 
     1883        double theold; 
     1884        double obs, obsf, obsfold; 
     1885        unsigned int timesync; 
     1886        double koef; 
     1887        double RC, alp; 
     1888         
     1889        const double Vamp; //V 
     1890        const double freq; //Hz 
     1891         
     1892// 
     1893//      vec p_isd, p_isq, p_ome, p_the; 
     1894        double p_isd, p_isq; 
     1895                 
     1896        PMSM_LQCtrl_EKF_Inj():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), Rs(0.28), Lq(0.003465), Ld(1.5*0.003465), 
     1897                                r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10),                                  //for r is a default value rewrited by pcp.txt file value 
     1898                                A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 
     1899                                uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), Vamp(10),  
     1900                                Akf(4,4), Ckf(/*3*/2,4), Rkf(/*3,3*/2,2), Qkf(4,4), uoldab(2), Pkf(4,4), freq(500), 
     1901                                R0(0.003), R10(3), y(3) {        
     1902                                 
     1903                                 timesync = 0; 
     1904                                 iqnew = 0.0; 
     1905                                 theold = 0.0; 
     1906                                 isaold = 0.0; 
     1907                                 isbold = 0.0; 
     1908                                 omeold = 0.0; 
     1909                                                 
     1910                //set fix matrices elements & dimensions 
     1911                A.zeros(); 
     1912                        A(0, 0) = A(1, 1) = a; 
     1913                        A(2, 1) = e; 
     1914                        A(2, 2) = d; 
     1915                        A(2, 4) = d - 1; 
     1916                        A(3, 2) = A(3, 4) = Dt; 
     1917                        A(3, 3) = A(4, 4) = 1.0; 
     1918                B.zeros(); 
     1919                        B(0, 0) = B(1, 1) = c;           
     1920                Q.zeros(); 
     1921                        Q(2, 2) = 1.0; 
     1922                R.zeros(); 
     1923                Rpd.zeros(); 
     1924                u_old.zeros();   
     1925                uoldab.zeros(); 
     1926                 
     1927                koef = Vamp*(Ld-Lq)/(2*pi*freq*2*Ld*Lq); 
     1928                RC = 1/(2*pi*5); //48LP 
     1929                alp = Dt/(RC + Dt);              
     1930                 
     1931                Akf.zeros(); 
     1932                Akf(0,0) = a; 
     1933                Akf(1,1) = a; 
     1934                Akf(2,2) = d; 
     1935                Akf(3,3) = 1.0; 
     1936                Akf(3,2) = Dt; 
     1937                Ckf.zeros(); 
     1938                Ckf(0,0) = 1.0; 
     1939                Ckf(1,1) = 1.0; 
     1940                //Ckf(2,2) = 0.0;        
     1941                Qkf.zeros(); 
     1942                Qkf(0,0) = 0.1;//0.0013; 
     1943                Qkf(1,1) = 0.1;//0.0013; 
     1944                Qkf(2,2) = 0.1;//5e-6; 
     1945                Qkf(3,3) = 0.01;//1e-10;                 
     1946                Rkf.zeros(); 
     1947                Rkf(0,0) = 0.05;//0.0006;        
     1948                Rkf(1,1) = 0.05;//0.0006; 
     1949                //Rkf(2,2) = 0.05;//0006; 
     1950                //Pkf.zeros();   
     1951                Pkf = Qkf; 
     1952        } 
     1953         
     1954        double hfsignal(int time){                               
     1955                return Vamp*cos(freq*2*pi*time*Dt); 
     1956        } 
     1957                 
     1958         
     1959        virtual vec ctrlaction(const itpp::vec& cond) { 
     1960                PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 
     1961                 
     1962                //increase synchronisation timer 
     1963                timesync++; 
     1964                 
     1965                //get data from PMSM                             
     1966                //y(0) = isa;// + noise 
     1967                //y(1) = isb;// + noise 
     1968                //ome = 0; 
     1969                //the = 0; 
     1970                 
     1971                //evaluate injection             
     1972                /*iqold = iqnew;                 
     1973                iqnew = y(1)*cos(theold) - y(0)*sin(theold);                
     1974             
     1975          obs = -(iqnew-(1 - Dt*Rs/Lq)*iqold)*hfsignal(timesync)/koef;//multiplication by HF signal 
     1976                obsf = alp*obs + (1-alp)*obsfold;       //lowpass filter 
     1977                obsfold = obsf; 
     1978                 
     1979          double y2 = 2*(obsf);*/ 
     1980          double isah, isbh, omeh, theh; 
     1981          //EKF => isa=isaold,isb=isbold,ome=omeold,the=theold 
     1982          isah = a*isaold + b*omeold*sin(theold) + c*uoldab(0); 
     1983          isbh = a*isbold - b*omeold*cos(theold) + c*uoldab(1); 
     1984          omeh = d*omeold + e*(isbold*cos(theold) - isaold*sin(theold)); 
     1985          theh = theold + Dt*omeold; 
     1986 
     1987          Akf(0, 2) = b*sin(theold); 
     1988          Akf(0, 3) = b*omeold*cos(theold); 
     1989          Akf(1, 2) = -b*cos(theold); 
     1990          Akf(1, 3) = b*omeold*sin(theold); 
     1991          Akf(2, 0) = -e*sin(theold); 
     1992          Akf(2, 1) = e*cos(theold); 
     1993          Akf(2, 3) = -e*(isaold*cos(theold)+isbold*sin(theold)); 
     1994  
     1995                vec yiab(/*3*/2); 
     1996          yiab(0) = /*y(0)*/isa - isah; 
     1997          yiab(1) = /*y(1)*/isb - isbh; //ybe = sqrt(3.0)/3.0 * (2.0*ybe + yal);         
     1998          //yiab(2) = y2 - theh; 
     1999           
     2000//          Rkf(2,2) = R0 + (R10 - R0)*omeold/10;     
     2001                Pkf = Akf*Pkf*Akf.T() + Qkf;                   
     2002          mat EKK = Pkf*Ckf.T()*inv(Ckf*Pkf*Ckf.T() + Rkf); 
     2003          Pkf = Pkf - EKK*Ckf*Pkf; 
     2004          //mat EKK = Pkf*Ckf.transpose()*inv(Ckf*Pkf*Ckf.transpose()+Rkf); 
     2005          //Pkf = Akf*(Pkf - Pkf*Ckf.transpose()*inv(Ckf*Pkf*Ckf.transpose()+Rkf)*Ckf*Pkf)*Akf.transpose() + Qkf; 
     2006           
     2007//         EKK = -P*C'*inv(C*P*C' + R)*yiab'; 
     2008//         P = A*(P - P*C'*inv(C*P*C' + R)*C*P)*A' + Q; 
     2009 
     2010          mat EKKy = EKK*yiab;             
     2011          isah = isah + EKKy(0,0); 
     2012          isbh = isbh + EKKy(1,0); 
     2013          omeh = omeh + EKKy(2,0); 
     2014          theh = theh + EKKy(3,0);            
     2015            
     2016          //cout << omeh << endl;                   
     2017           
     2018          /*cout << "K" << isah << " " << isbh << " " << omeh << " " << theh << endl; 
     2019           
     2020          isah = isa; 
     2021          isbh = isb; 
     2022          omeh = ome; 
     2023          theh = the; 
     2024           
     2025          cout << "M" << isah << " " << isbh << " " << omeh << " " << theh << endl;*/ 
     2026           
     2027          isaold = isah; 
     2028          isbold = isbh; 
     2029          omeold = omeh; 
     2030          theold = theh; 
     2031                 
     2032                int i; 
     2033                vec tmp; 
     2034                 
     2035                lq.resetTime();  
     2036                 
     2037                p_isd = isah*cos(theh) + isbh*sin(theh);//isa; 
     2038                p_isq = isbh*cos(theh) - isah*sin(theh);//isb; 
     2039                                 
     2040                A(0, 1) = Dt*omeh; 
     2041                //A(0, 2) = Dt*p_isq; 
     2042                //A(0, 4) = Dt*p_isq; 
     2043                A(1, 0) = -Dt*omeh; 
     2044                A(1, 2) = /*-Dt*p_isd*/-b; 
     2045                A(1, 4) = /*-Dt*p_isd*/-b; 
     2046                         
     2047                lq.Models(0).A = A;      
     2048                 
     2049                //create control matrix 
     2050                for(i = rec_hor; i > 0; i--){                                    
     2051                        lq.redesign(); 
     2052                } 
     2053                lq.redesign(); 
     2054                 
     2055                L = lq.getL(); 
     2056         
     2057                icondpd(0) = isah*cos(theh) + isbh*sin(theh); 
     2058                icondpd(1) = isbh*cos(theh) - isah*sin(theh); 
     2059                icondpd(2) = omeh - /*2.65*/Ww; 
     2060                icondpd(3) = theh; 
     2061                icondpd(4) = /*2.65*/Ww; 
     2062                icondpd(5) = u_old(0); 
     2063                icondpd(6) = u_old(1); 
     2064                icondpd(7) = 0; 
     2065                         
     2066                tmp = L*icondpd;                         
     2067                                 
     2068                udq = tmp(0,1); 
     2069                 
     2070                //uab = udq; //set size 
     2071                uab(0) = udq(0)*cos(theh) - udq(1)*sin(theh);// + hfsignal(timesync-1)*cos(theh); 
     2072                uab(1) = udq(1)*cos(theh) + udq(0)*sin(theh);// + hfsignal(timesync-1)*sin(theh); 
     2073                         
     2074                 
     2075                if(MAXuflag == 1){ //square cut off 
     2076                        if(uab(0) > MAXu) uab(0) = MAXu; 
     2077                        else if(uab(0) < -MAXu) uab(0) = -MAXu; 
     2078                        if(uab(1) > MAXu) uab(1) = MAXu; 
     2079                        else if(uab(1) < -MAXu) uab(1) = -MAXu; 
     2080                } 
     2081                else if(MAXuflag == 2){ //circular cut off 
     2082                        double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 
     2083                        double uangl = atan2(uab(1), uab(0)); 
     2084                        if (uampl > MAXu) uampl = MAXu; 
     2085                        uab(0) = uampl*cos(uangl); 
     2086                        uab(1) = uampl*sin(uangl); 
     2087                } 
     2088                                 
     2089                u_old = udq; 
     2090                uoldab = uab; 
     2091         
     2092                return uab; 
     2093        } 
     2094         
     2095        void from_setting(const Setting &set){ 
     2096                PMSMCtrl::from_setting(set); 
     2097                UI::get(r,set, "r", UI::optional); 
     2098                UI::get(rec_hor,set, "h", UI::optional); 
     2099                UI::get(MAXu,set, "MAXu", UI::optional); 
     2100                UI::get(MAXuflag,set, "MAXuflag", UI::optional); 
     2101                UI::get(rpd,set, "rpd", UI::optional); 
     2102        } 
     2103 
     2104        void validate(){ 
     2105                R(0, 0) = R(1, 1) = r; 
     2106                Rpd(0, 0) = Rpd(1, 1) = rpd;             
     2107                Rpd(0, 2) = Rpd(1, 3) = -rpd;                    
     2108 
     2109                Array<quadraticfn> qloss(3); 
     2110                qloss(0).Q.setCh(Q); 
     2111                qloss(0).rv = RV("x", 5, 1);             
     2112                qloss(1).Q.setCh(R); 
     2113                qloss(1).rv = RV("u", 2, 0); 
     2114                qloss(2).Q.setCh(Rpd); 
     2115                qloss(2).rv = RV("u", 2, 0); 
     2116                qloss(2).rv.add(RV("u", 2, -1));                 
     2117                lq.Losses = qloss;               
     2118 
     2119                //set lq 
     2120                lq.rv = RV("u", 2, 0);           
     2121                RV tmp = RV("x", 5, 0); 
     2122                tmp.add(RV("u", 2, -1));         
     2123                lq.set_rvc(tmp); 
     2124                 
     2125                lq.horizon = rec_hor;    
     2126                 
     2127                Array<linfnEx> model(2); 
     2128                model(0).A = A; 
     2129                model(0).B = vec("0 0 0 0 0"); 
     2130                model(0).rv = RV("x", 5, 0); 
     2131                model(0).rv_ret = RV("x", 5, 1); 
     2132                model(1).A = B; 
     2133                model(1).B = vec("0 0"); 
     2134                model(1).rv = RV("u", 2, 0); 
     2135                model(1).rv_ret = RV("x", 5, 1); 
     2136                lq.Models = model; 
     2137                 
     2138                lq.finalLoss.Q.setCh(Q); 
     2139                lq.finalLoss.rv = RV("x", 5, 1); 
     2140                                 
     2141                lq.validate(); 
     2142                                                 
     2143                uab.zeros(); 
     2144                udq.zeros();             
     2145        } 
     2146}; 
     2147UIREGISTER(PMSM_LQCtrl_EKF_Inj); 
     2148 
    7762149/*!@}*/ 
    7772150#endif //PMSM_CTR_H