Changeset 232

Show
Ignore:
Timestamp:
01/15/09 10:53:57 (15 years ago)
Author:
smidl
Message:

test 2 order Taylor

Location:
pmsm
Files:
3 modified

Legend:

Unmodified
Added
Removed

  • pmsm/mpf_u_delta.cpp

    r230 r232  
    8484 
    8585        // internal model 
    86         IMpmsm fxu0; 
     86        IMpmsm2o fxu0; 
    8787        IMpmsm_delta fxu; 
    8888        //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz) 
    … …  
    9494        vec mu0= "0.0 0.0 0.0 0.0"; 
    9595        vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001 
    96         vec Rdiag ( "0.2 0.2" ); //var(diff(xth)) = "0.034 0.034" 
     96        vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034" 
    9797        chmat Q ( Qdiag ); 
     98        mat Q2o=diag(Qdiag); 
    9899        chmat R ( Rdiag ); 
    99         EKFCh KFE ( rx,ry,ru ); 
     100        EKFCh KFE ( rx,ry,rU ); 
    100101        KFE.set_parameters ( &fxu0,&hxu,Q,R ); 
    101102        KFE.set_est ( mu0, chmat ( ones ( 4 ) ) ); 
    … …  
    110111        MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp ); 
    111112        // initialize 
    112         vec Ud0="0 0 1.0"; 
    113         evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 0.0001" ))); 
     113        vec Ud0="0 0 100.0"; 
     114        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 10" ))); 
    114115        evolUd.condition ( Ud0 ); 
    115116        epdf& pfinit=evolUd._epdf(); 
    116117        M.set_est ( pfinit ); 
    117         evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 4e-3 4e-3 1e-3" ))); 
     118        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 9e-8 9e-8 9e-7" ))); 
    118119 
    119120        mat Xt=zeros ( Ndat ,4 ); //true state from simulator 
    … …  
    127128        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h ); 
    128129        vec dt ( 2 ); 
    129         vec ut ( 2 ); 
     130        vec ut ( 4 ); 
     131        vec utm ( 4 ); 
    130132        vec xt ( 4 ); 
    131133        vec xtm=zeros ( 4 ); 
    … …  
    143145                ut ( 0 ) = KalmanObs[0]; 
    144146                ut ( 1 ) = KalmanObs[1]; 
     147                ut ( 2 ) = utm(0)-ut(0); 
     148                ut ( 3 ) = utm(1)-ut(1); 
    145149                dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t ); 
    146150                dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t ); 
    147151                xt = vec ( x,4 ); 
    148152 
     153                utm =ut; 
     154                 
     155                Q2o*=0.9; 
     156                Q2o+=outer_product(x2o_dbg,x2o_dbg); 
    149157                //estimator 
    150158                KFE.bayes ( concat ( dt,ut ) ); 
     159                for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(Q2o));} 
    151160                M.bayes ( concat ( dt,ut ) ); 
    152161 
    153162                Xt.set_row ( tK, xt ); //vec from C-array 
    154                 Dt.set_row ( tK, concat ( dt,ut, vec_2 ( KalmanObs[4],KalmanObs[5] ) ) ); 
     163                Dt.set_row ( tK, concat ( dt,ut.left(2), vec_2 ( KalmanObs[4],KalmanObs[5] ) ) ); 
    155164                Qtr.set_row ( tK, Qdiag ); 
    156165                XtE.set_row ( tK,KFE._e()->mean() ); 

  • pmsm/pmsm.h

    r224 r232  
    1212RV ru ( "{ua ub }"); 
    1313RV ry ( "{oia oib }"); 
     14RV rU = concat(ru, RV("{dua dub }")); 
    1415 
    1516// class uipmsm : public uibase{ 
    1617//      double Rs, Ls, dt, Ypm, kp, p,  J, Mz; 
    1718// }; 
     19 
     20vec x2o_dbg(4); 
    1821 
    1922//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$ 
    … …  
    7477}; 
    7578 
     79//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$ 
     80class IMpmsm2o : public diffbifn { 
     81        protected: 
     82                double Rs, Ls, dt, Ypm, kp, p,  J, Mz; 
     83 
     84        public: 
     85                IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {}; 
     86        //! Set mechanical and electrical variables 
     87                void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;} 
     88 
     89                vec eval ( const vec &x0, const vec &u0 ) { 
     90                // last state 
     91                        double iam = x0 ( 0 ); 
     92                        double ibm = x0 ( 1 ); 
     93                        double omm = x0 ( 2 ); 
     94                        double thm = x0 ( 3 ); 
     95                        double uam = u0 ( 0 ); 
     96                        double ubm = u0 ( 1 ); 
     97                        double dua = u0 ( 2 )/dt; 
     98                        double dub = u0 ( 3 )/dt; 
     99 
     100                        double cth = cos(thm); 
     101                        double sth = sin(thm); 
     102                        double d2t = dt*dt/2; 
     103                         
     104                        double dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls); 
     105                        double dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls); 
     106                        double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz; 
     107                        double dth = omm; 
     108                         
     109                        double d2ia = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls); 
     110                        double d2ib = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls); 
     111                        double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth)); 
     112                        double d2th = dom; 
     113                         
     114                        vec xk=zeros ( 4 ); 
     115                        xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia; 
     116                        xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib; 
     117                        xk ( 2 ) = omm +dt*dom;// +d2t*d2om; 
     118                        xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi> 
     119                         
     120                        x2o_dbg(0)=d2t*d2ia; 
     121                        x2o_dbg(1)=d2t*d2ib; 
     122                        x2o_dbg(2)=d2t*d2om; 
     123                        x2o_dbg(3)=d2t*d2th; 
     124                         
     125                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi; 
     126                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi; 
     127                        return xk; 
     128                } 
     129 
     130                void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) { 
     131                        double iam = x0 ( 0 ); 
     132                        double ibm = x0 ( 1 ); 
     133                        double omm = x0 ( 2 ); 
     134                        double thm = x0 ( 3 ); 
     135                // d ia 
     136                        A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0; 
     137                        A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) ); 
     138                // d ib 
     139                        A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt ); 
     140                        A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) ); 
     141                // d om 
     142                        A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) ); 
     143                        A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) ); 
     144                        A ( 2,2 ) = 1.0; 
     145                        A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) ); 
     146                // d th 
     147                        A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0; 
     148                } 
     149 
     150                void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );}; 
     151 
     152}; 
     153 
    76154//! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$ 
    77155class IMpmsmStat : public IMpmsm { 

  • pmsm/sim_var.cpp

    r224 r232  
    3232        int Nsimstep = 125; 
    3333 
    34         dirfilelog L("exp/sim_var",1000); 
     34        dirfilelog L("exp/sim_var2",1000); 
    3535        //memlog L(Ndat); 
     36         
    3637         
    3738        // SET SIMULATOR 
    … …  
    3940        double Ww = 0.0; 
    4041        vec dt ( 2 ); 
    41         vec ut ( 2 ); 
     42        vec ut ( 4 ); 
     43        vec utm ( 4 ); 
    4244        vec dut ( 2 ); 
    4345        vec dit (2); 
    … …  
    4749        vec xt ( 4 ); 
    4850        vec ddif=zeros(2); 
    49         IMpmsm fxu; 
     51        IMpmsm2o fxu; 
    5052        //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz) 
    5153        fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989,   1.5 ,4.0, 0.04, 0.0 ); 
    … …  
    6062        mat Q =diag( Qdiag ); 
    6163        mat R =diag ( Rdiag ); 
    62         EKFfull Efix ( rx,ry,ru ); 
     64        EKFfull Efix ( rx,ry,rU ); 
    6365        Efix.set_est ( mu0, 1*eye ( 4 )  ); 
    6466        Efix.set_parameters ( &fxu,&hxu,Q,R); 
    6567 
    66         EKFfull Eop ( rx,ry,ru ); 
     68        EKFfull Eop ( rx,ry,rU ); 
    6769        Eop.set_est ( mu0, 1*eye ( 4 ) ); 
    6870        Eop.set_parameters ( &fxu,&hxu,Q,R); 
    6971 
    70         EKFfull Edi ( rx,ry,ru ); 
     72        EKFfull Edi ( rx,ry,rU ); 
    7173        Edi.set_est ( mu0, 1*eye ( 4 ) ); 
    7274        Edi.set_parameters ( &fxu,&hxu,Q,R); 
    … …  
    8082        RV rR( "{R }", "4"); 
    8183        RV rUD( "{u_isa u_isb i_isa i_isb }", ones_i(4)); 
    82         RV rDu("{dux duy duxf duyf }",ones_i(4)); 
     84        RV rDu("{dux duy }",ones_i(2)); 
    8385        RV rDi("{disa disb }",ones_i(2)); 
    8486        int X_log = L.add(rx,"X"); 
     87        int X2o_log = L.add(rx,"X2o"); 
     88        int Xdf_log = L.add(rx,"Xdf"); 
    8589        int Efix_log = L.add(rx,"XF"); 
    8690        int Eop_log = L.add(rx,"XO"); 
    … …  
    100104                }; 
    101105                // simulation via deterministic model 
    102                 ut ( 0 ) = KalmanObs[0]; 
    103                 ut ( 1 ) = KalmanObs[1]; 
     106                ut ( 0 ) = KalmanObs[4]; 
     107                ut ( 1 ) = KalmanObs[5]; 
     108                ut ( 2 ) = utm(0)-ut(0); 
     109                ut ( 3 ) = utm(1)-ut(1); 
     110                 
     111                utm = ut; 
     112                 
    104113                dt ( 0 ) = KalmanObs[2]; 
    105114                dt ( 1 ) = KalmanObs[3]; 
    … …  
    147156                //LOG 
    148157                L.logit(X_log,  vec(x,4)); //vec from C-array 
     158                L.logit(X2o_log, x2o_dbg);  
     159                L.logit(Xdf_log, xdif);  
    149160                L.logit(Efix_log, Efix_ep.mean() );  
    150161                L.logit(Eop_log,        Eop_ep.mean() );