Changeset 1226 for applications/pmsm

Timestamp:

10/22/10 21:15:11 (14 years ago)

Author:

smidl

Message:

kalman in Ch

Location:

applications/pmsm/simulator_zdenek

Files:

• CMakeLists.txt (modified) (1 diff)
• ekf_example/ekf_obj.cpp (modified) (10 diffs)
• ekf_example/ekf_obj.h (modified) (4 diffs)

applications/pmsm/simulator_zdenek/CMakeLists.txt

@@ -3 +3 @@
 
 EXEC (test_UD  pmsmsim ekf_obj)
+EXEC (test_Ch  pmsmsim ekf_obj)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -51 +51 @@
                 
                 
-        //      BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
+//              BM::ll = -0.5* ( 4 * 1.83787706640935 +log ( det ( Pfull ) ) +xdif* ( inv(Pfull)*xdif ) );*/
         }
 };
@@ -174 +174 @@
   cG=prevod(Tv*Wref*4/Thetaref,15);
 //  cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-  cH=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+  cH=prevod(Tv*Wref*Fmag/Iref/Ls,17); //cB in q
   //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
 
@@ -401 +401 @@
         // vypocet napeti v systemu (x,y)
         int uf[2];
-        uf[0]=prevod(ux/Uref,Qm);
-        uf[1]=prevod(uy/Uref,Qm);
+        uf[0]=prevod(ux/Uref,15);
+        uf[1]=prevod(uy/Uref,15);
         
         int Y_mes[2];
         // zadani mereni
-        Y_mes[0]=prevod(isxd/Iref,Qm);
-        Y_mes[1]=prevod(isyd/Iref,Qm);
-        
+        Y_mes[0]=prevod(isxd/Iref,15);
+        Y_mes[1]=prevod(isyd/Iref,15);
+                
         ////// vlastni rutina EKF -- /////////////////////////
         int t_sin,t_cos, tmp;
@@ -418 +418 @@
         t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
         
-        tmp=((long)cB*x_est[2])>>15;
-        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]<<2))>>15;
-        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]<<2))>>15;
+        tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
+        //             q15*q13 +          q13*q15      + q15*q13??
+        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
+        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
         x_est[2]=x_est[2];
+        //               q15              + q15*q13
         x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
         
@@ -428 +430 @@
         
         //void EKFfixed::update_psi(void)
-        {               
+        {       
                 PSI[2]=((long)cB*t_sin)>>15;
-                tmp=((long)cB*x_est[2])>>15;
+                tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!!
                 PSI[3]=((long)tmp*t_cos)>>15;
                 PSI[6]=-((long)cB*t_cos)>>15;
@@ -456 +458 @@
         
         int difz[2];
-        difz[0]=(Y_mes[0]<<2)-x_est[0]; // Y_mes in q13!!
-        difz[1]=(Y_mes[1]<<2)-x_est[1];
+        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
+        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
 
         {
-                vec dd(4);dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
-                log_level.store(logD,dd);
+                ivec dd(Df,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1];
+                log_level.store(logD,get_from_ivec(dd));
         }
         
         //bierman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx );
         int dR[2];dR[0]=R[0];dR[1]=R[3];
+        //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
         bierman(difz,x_est,Uf,Df,dR,2,4);
+        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
         
         // navrat estimovanych hodnot regulatoru
@@ -475 +479 @@
         (mu)(3)=zprevod(x_est[3],15)*Thetaref;
         
+        //x_est[2]=x[2]*32768/Wref;
+        //x_est[3]=x[3]*32768/Thetaref;
 //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
 }
@@ -486 +492 @@
         Q[15]=prevod(0.0001,15);      // 1e-3
 
-        Uf[0]=0x7FFF;       // 0.05
+        Uf[0]=0x7FFF;// !       // 0.05
         Uf[1]=Uf[2]=Uf[3]=Uf[4]=0;
-        Uf[5]=0x7FFF;
+        Uf[5]=0x7FFF;//!
         Uf[6]=Uf[6]=Uf[8]=Uf[9]=0;
-        Uf[10]=0x7FFF;      // 1e-3
+        Uf[10]=0x7FFF;//!      // 1e-3
         Uf[11]=Uf[12]=Uf[13]=Uf[4]=0;
         Uf[15]=0x7FFF;      // 1e-3
         
-        Df[0]=0x7FFF;
-        Df[1]=0x7FFF;
-        Df[2]=0x7FFF;
-        Df[3]=0x7FFF;
+        Df[0]=1<<14;
+        Df[1]=1<<14;
+        Df[2]=1<<14;
+        Df[3]=1<<14;
         
         // Tuning of matrix R
@@ -510 +516 @@
         //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
         //  cF=prevod(p*Tv*Mref/J/Wref,15);
-        cG=prevod(Tv*Wref/Thetaref,15); //no *4!!
+        cG=prevod(Tv*Wref/Thetaref,15); //in q15!
         //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
-        // cH=prevod(Tv*Wref*Fmag/Iref/Ls,15); <<< use cB instead
+        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // 
         //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
         
@@ -522 +528 @@
         PSI[15]=0x7FFF;
         
-        //////////////////////// =================
-        ///// TEST thorton vs. thorton_fast
-        
-/*      int Ut[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
-        int Dt[4]={100,200,300,400};
-        int PSIu[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
-        int Dold[4];
-        
-        thorton(Ut,Dt,PSIu,Q,G,Dold,4);
-        int Ut2[16]={0x7FFF, 100, 200, 300, 0, 0x7FFF, 500, 600, 0,0,0x7FFF, 800, 0,0,0, 0x7FFF};
-        int Dt2[4]={100,200,300,400};
-        int PSIu2[16] = {100, 200,300, 400 , 500, 600,700,800, 900,1000,1100,1200, 1300,1400,1500,1600};
-        thorton_fast(Ut2,Dt2,PSIu2,Q,G,Dold,4);
-        cout<< Q<<endl;*/
-}
-
+}
+
+void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+        ekf(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedCh::ekf(double ux, double uy, double isxd, double isyd)
+{
+        // vypocet napeti v systemu (x,y)
+        int uf[2];
+        uf[0]=prevod(ux/Uref,15);
+        uf[1]=prevod(uy/Uref,15);
+        
+        int Y_mes[2];
+        // zadani mereni
+        Y_mes[0]=prevod(isxd/Iref,15);
+        Y_mes[1]=prevod(isyd/Iref,15);
+        
+        ////// vlastni rutina EKF -- /////////////////////////
+        int t_sin,t_cos, tmp;
+        
+        // implementace v PC
+        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
+        t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+        t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);
+        
+        tmp=((long)cB*x_est[2])>>15; // q15*q13 -> q13
+        //             q15*q13 +          q13*q15      + q15*q13??
+        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*(uf[0]))>>15; // in q13
+        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*(uf[1]))>>15; // q13
+        x_est[2]=x_est[2];
+        //               q15              + q15*q13
+        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+        
+        if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15);
+        if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15);
+        
+        //void EKFfixed::update_psi(void)
+        {       
+                PSI[2]=((long)cB*t_sin)>>15;
+                tmp=((long)cH*x_est[2])>>15; // ! cH =cB with different scale!!
+                PSI[3]=((long)tmp*t_cos)>>15;
+                PSI[6]=-((long)cB*t_cos)>>15;
+                PSI[7]=((long)tmp*t_sin)>>15;
+        }
+        {
+                ivec Ad(PSI,16);
+                log_level.store(logA,get_from_ivec(Ad));
+        }
+        
+        ///////// end of copy ///////////////
+        mmultACh(PSI,Chf,PSICh,4,4);
+        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+        householder(PSICh,Q,4);
+        
+        {
+                ivec Chd(Chf,16);
+                log_level.store(logCh,get_from_ivec(Chd));
+        }
+        
+        
+        int difz[2];
+        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
+        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
+        
+        
+        //
+        int dR[2];dR[0]=R[0];dR[1]=R[3];
+        //int xb[4]; xb[0]=x_est[0]<<2; xb[1]=x_est[1]<<2;  xb[2]=x_est[2]<<2;  xb[3]=x_est[3];  
+        carlson(difz,x_est,Chf,dR,2,4);
+        //x_est[0] = xb[0]>>2; x_est[1]=xb[1]>>2; x_est[2]=xb[2]>>2; x_est[3]=xb[3];
+        
+        // navrat estimovanych hodnot regulatoru
+        vec& mu = E._mu();
+        (mu)(0)=zprevod(x_est[0],15)*Iref;
+        (mu)(1)=zprevod(x_est[1],15)*Iref;
+        (mu)(2)=zprevod(x_est[2],15)*Wref;
+        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
+        
+        //x_est[2]=x[2]*32768/Wref;
+        //x_est[3]=x[3]*32768/Thetaref;
+        //      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
+}
+
+void EKFfixedCh::init_ekf(double Tv)
+{
+        // Tuning of matrix Q
+        Q[0]=prevod(.01,15);       // 0.05
+        Q[5]=Q[0];
+        Q[10]=prevod(0.0001,15);      // 1e-3
+        Q[15]=prevod(0.0001,15);      // 1e-3
+        
+        Chf[0]=0x7FFF;// !       // 0.05
+        Chf[1]=Chf[2]=Chf[3]=Chf[4]=0;
+        Chf[5]=0x7FFF;//!
+        Chf[6]=Chf[6]=Chf[8]=Chf[9]=0;
+        Chf[10]=0x7FFF;//!      // 1e-3
+        Chf[11]=Chf[12]=Chf[13]=Chf[4]=0;
+        Chf[15]=0x7FFF;      // 1e-3
+                
+        // Tuning of matrix R
+        R[0]=prevod(0.05,15);         // 0.05
+        R[3]=R[0];
+        
+        // Motor model parameters 
+        cA=prevod(1-Tv*Rs/Ls,15);
+        cB=prevod(Tv*Wref*Fmag/Iref/Ls,15);
+        cC=prevod(Tv/Ls/Iref*Uref,15);
+        //  cD=prevod(1-Tv*Bf/J,15);
+        //  cE=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref,15);
+        //  cF=prevod(p*Tv*Mref/J/Wref,15);
+        cG=prevod(Tv*Wref/Thetaref,15); //in q15!
+        //cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,15);
+        cH=prevod(Tv*Wref*Thetaref*Fmag/Iref/Ls,15); // 
+        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+        
+        /* Init matrix PSI with permanently constant terms */
+        PSI[0]=cA;
+        PSI[5]=PSI[0];
+        PSI[10]=0x7FFF;
+        PSI[14]=cG;
+        PSI[15]=0x7FFF;
+        
+}
+

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -19 +19 @@
 #include "matrix.h"
 #include "matrix_vs.h"
-#include "reference.h"
+#include "reference_Q15.h"
 #include "parametry_motoru.h"
 
@@ -109 +109 @@
 class EKFfixedUD : public BM {
         public:
-                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA);
+                LOG_LEVEL(EKFfixedUD,logU, logG, logD, logA, logP);
                 
                 void init_ekf(double Tv);
@@ -164 +164 @@
                                 L.add_vector ( log_level, logD, RV ("D", 4 ), prefix );
                                 L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+                                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
                                 
                 };
@@ -170 +171 @@
 
 UIREGISTER(EKFfixedUD);
+
+/*!
+ * \brief Extended Kalman Filter with Chol matrices in fixed point arithmetic
+ * 
+ * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+ */
+class EKFfixedCh : public BM {
+public:
+        LOG_LEVEL(EKFfixedCh,logCh, logA, logP);
+        
+        void init_ekf(double Tv);
+        void ekf(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int Q[16]; /* matrix [4,4] */
+        int R[4]; /* matrix [2,2] */
+        
+        int x_est[4]; /* estimate and prediction */
+        
+        int PSI[16]; /* matrix [4,4] */
+        int PSICh[16]; /* matrix PIS*U, [4,4] */
+        
+        int Chf[16]; // upper triangular of covariance (inplace)
+        
+        int cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<chmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedCh ():BM(),E(),Ry(2,2){
+                int i;
+                for(i=0;i<16;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<4;i++){x_est[i]=0;}
+                for(i=0;i<16;i++){Chf[i]=0;}
+                
+                for(i=0;i<16;i++){PSI[i]=0;}
+                
+                set_dim(4);
+                E._mu()=zeros(4);
+                E._R()=zeros(4,4);
+                init_ekf(0.000125);
+        };
+        //! Here dt = [yt;ut] of appropriate dimensions
+        void bayes ( const vec &yt, const vec &ut );
+        //!dummy!
+        const epdf& posterior() const {return E;};
+        void log_register(logger &L, const string &prefix){
+                BM::log_register ( L, prefix );
+                
+                L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix );
+                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix );
+                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );
+                
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedCh);
+
 
 //! EKF for comparison of EKF_UD with its fixed-point implementation