Changeset 1438 for applications

Timestamp:

03/14/12 23:39:30 (12 years ago)

Author:

smidl

Message:

Kalman for om

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• mpf_double.cpp (modified) (1 diff)
• mpf_double.h (modified) (2 diffs)

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.cpp

@@ -228 +228 @@
         for (int i=0;i<N;i++) th1[i]=th[i];
 }
+
+double kalman_om( const double &isa, const double &isb , const double &usa, const double &usb, const double &th ) {
+
+        double isd;
+        double isq;
+        double usd;
+        double usq;
+
+        double Cd;
+        double Cq;
+        double CC;
+        double oCC;
+
+        double difid;
+        double difiq;
+
+        double zeta;
+        double Kd;
+        double Kq;
+        double ro;
+        double ypd;
+        double ypq;
+        double detRy;
+        double ydiffd;
+        double ydiffq;
+        double ydC;
+
+        static double Pt;
+        static double om;
+        double cth, sth;
+        static double isdm, isqm;
+
+        
+                Pt = 1.0*1.0*Pt+qom; // Pt is now predictive variance
+                //while ( th>M_PI ) th=th-2*M_PI;
+                //while ( th<-M_PI ) th=th+2*M_PI;
+
+                sth=sin ( th );
+                cth=cos ( th );
+
+                isd = cth*isa+sth*isb;
+                isq = -sth*isa+cth*isb;
+                usd = cth*usa+sth*usb;
+                usq = -sth*usa+cth*usb;
+
+                Cd = isq*_dt;
+                Cq = -_b - isd* _dt;
+
+                difid=isd- _ad  *isdm - _cd *usd;
+                difiq=isq- _aq  *isqm - _cq *usq;
+
+                CC=Cd*Cd+Cq*Cq;
+                zeta = Pt/ ( r+Pt*CC );
+                oCC = ( 1-zeta*CC );
+                ro = oCC/r;
+
+                Kd = Pt*Cd*ro;
+                Kq = Pt*Cq*ro;
+
+                Pt=Pt* ( 1- ( Kd*Cd+Kq*Cq ) );
+
+                ypd = Cd*om;
+                ypq = Cq*om;
+
+                detRy = ro/r;
+                om = om + Kd* ( difid - ypd ) +Kq* ( difiq-ypq );
+                ydiffd = ( ypd-difid );
+                ydiffq = ( ypq-difiq );
+                ydC = ydiffd*Cd + ydiffq*Cq;
+
+                isdm=isd;
+                isqm=isq;
+                
+                return om;
+}

applications/pmsm/simulator_zdenek/ekf_example/mpf_double.h

@@ -5 +5 @@
 #define N 20
 
-#define Lsd Ls*1.1
+#define Lsd Ls*0.9
 #define Lsq Ls
 #define _dt 0.000125
-#define _ad Rs*_dt/Lsd
-#define _aq Rs*_dt/Lsq
+#define _ad (1.-Rs*_dt/Lsd)
+#define _aq (1.-Rs*_dt/Lsq)
 #define _b  Fmag/Lsq*_dt
 #define _cd  _dt/Lsd
@@ -21 +21 @@
 void mpf_mean(double *Ecosth, double *Esinth, double *Eome);
 void mpf_th(double th1[N]);
+
+double kalman_om ( const double &isa, const double &isb , const double &usa, const double &usb, const double &th );