Show
Ignore:
Timestamp:
03/14/12 23:39:30 (13 years ago)
Author:
smidl
Message:

Kalman for om

Location:
applications/pmsm/simulator_zdenek/ekf_example
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

    r1381 r1438  
    228228        for (int i=0;i<N;i++) th1[i]=th[i]; 
    229229} 
     230 
     231double kalman_om( const double &isa, const double &isb , const double &usa, const double &usb, const double &th ) { 
     232 
     233        double isd; 
     234        double isq; 
     235        double usd; 
     236        double usq; 
     237 
     238        double Cd; 
     239        double Cq; 
     240        double CC; 
     241        double oCC; 
     242 
     243        double difid; 
     244        double difiq; 
     245 
     246        double zeta; 
     247        double Kd; 
     248        double Kq; 
     249        double ro; 
     250        double ypd; 
     251        double ypq; 
     252        double detRy; 
     253        double ydiffd; 
     254        double ydiffq; 
     255        double ydC; 
     256 
     257        static double Pt; 
     258        static double om; 
     259        double cth, sth; 
     260        static double isdm, isqm; 
     261 
     262         
     263                Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance 
     264                //while ( th>M_PI ) th=th-2*M_PI; 
     265                //while ( th<-M_PI ) th=th+2*M_PI; 
     266 
     267                sth=sin ( th ); 
     268                cth=cos ( th ); 
     269 
     270                isd = cth*isa+sth*isb; 
     271                isq = -sth*isa+cth*isb; 
     272                usd = cth*usa+sth*usb; 
     273                usq = -sth*usa+cth*usb; 
     274 
     275                Cd = isq*_dt; 
     276                Cq = -_b - isd* _dt; 
     277 
     278                difid=isd- _ad  *isdm - _cd *usd; 
     279                difiq=isq- _aq  *isqm - _cq *usq; 
     280 
     281                CC=Cd*Cd+Cq*Cq; 
     282                zeta = Pt/ ( r+Pt*CC ); 
     283                oCC = ( 1-zeta*CC ); 
     284                ro = oCC/r; 
     285 
     286                Kd = Pt*Cd*ro; 
     287                Kq = Pt*Cq*ro; 
     288 
     289                Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) ); 
     290 
     291                ypd = Cd*om; 
     292                ypq = Cq*om; 
     293 
     294                detRy = ro/r; 
     295                om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq ); 
     296                ydiffd = ( ypd-difid ); 
     297                ydiffq = ( ypq-difiq ); 
     298                ydC = ydiffd*Cd + ydiffq*Cq; 
     299 
     300                isdm=isd; 
     301                isqm=isq; 
     302                 
     303                return om; 
     304} 

  • applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h

    r1381 r1438  
    55#define N 20 
    66 
    7 #define Lsd Ls*1.1 
     7#define Lsd Ls*0.9 
    88#define Lsq Ls 
    99#define _dt 0.000125 
    10 #define _ad Rs*_dt/Lsd 
    11 #define _aq Rs*_dt/Lsq 
     10#define _ad (1.-Rs*_dt/Lsd) 
     11#define _aq (1.-Rs*_dt/Lsq) 
    1212#define _b  Fmag/Lsq*_dt 
    1313#define _cd  _dt/Lsd 
    … …  
    2121void mpf_mean(double *Ecosth, double *Esinth, double *Eome); 
    2222void mpf_th(double th1[N]); 
     23 
     24double kalman_om ( const double &isa, const double &isb , const double &usa, const double &usb, const double &th );