Changeset 1341 for applications/pmsm/simulator_zdenek/ekf_example

Timestamp:

04/28/11 22:18:55 (14 years ago)

Author:

smidl

Message:

extended reduced model for unknown Torque

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• ekf_obj.cpp (modified) (5 diffs)
• ekf_obj.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -573 +573 @@
         x_est[0]=x_est[0];
         //               q15              + q15*q13
-        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15;
+        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0]+(1<<14))>>15;
         
 //      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15);
@@ -636 +636 @@
 tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13
 //             q15*q13 +          q13*q15      + q15*q13??
-y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
-y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
+y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13
+y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13
 
         
@@ -674 +674 @@
 {
         // Tuning of matrix Q
-        Q[0]=prevod(0.0005,15);      // 1e-3
-        Q[3]=prevod(0.0001,15);      // 1e-3
+        Q[0]=prevod(0.01,15);      // 1e-3
+        Q[3]=prevod(0.01,15);      // 1e-3
         
         Uf[0]=0x7FFF;// !       // 0.05
@@ -685 +685 @@
         
         // Tuning of matrix R
-        R[0]=prevod(0.005,15);         // 0.05
+        R[0]=prevod(0.01,15);         // 0.05
         R[3]=R[0];
         
@@ -708 +708 @@
 }
 
+void EKFfixedUD3::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+        ekf3(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedUD3::ekf3(double ux, double uy, double isxd, double isyd)
+{
+        // vypocet napeti v systemu (x,y)
+        int16 uf[2];
+        uf[0]=prevod(ux/Uref,15);
+        uf[1]=prevod(uy/Uref,15);
+        
+        int16 Y_mes[2];
+        // zadani mereni
+        Y_mes[0]=prevod(isxd/Iref,15);
+        Y_mes[1]=prevod(isyd/Iref,15);
+        
+        ////// vlastni rutina EKF -- /////////////////////////
+        int16 t_sin,t_cos;
+        int32 tmp;
+        
+        int16 Mm=x_est[2];
+        x_est[0]=(((int32)x_est[0]<<15) + (int32)cF*x_est[2] + (1<<14))>>15;
+        //               q15              + q15*q13
+        x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0] + (1<<14))>>15;
+        x_est[2]=x_est[2];
+        
+        //      if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15);
+        //      if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15);
+        
+        
+        ///////// end of copy ///////////////
+        mmultAU(PSI,Uf,PSIU,3,3);
+        
+        
+        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
+        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,3);
+        
+        // implementace v PC
+        /*      t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15);
+         *      t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/
+        tmp=prevod(sin(Thetaref*x_est[1]/32768.),15);
+        if (tmp>32767) t_sin =32767; else t_sin=tmp;
+        tmp=prevod(cos(Thetaref*x_est[1]/32768.),15);
+        if (tmp>32767) t_cos =32767; else t_cos=tmp;
+        
+        {       
+                C[0]=((int32)cB*t_sin)>>15;
+                tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!!
+                C[1]=((int32)tmp*t_cos)>>15;
+                C[2] = 0;
+                
+                C[3]=-((int32)cB*t_cos)>>15;
+                C[4]=((int32)tmp*t_sin)>>15;
+                C[5]=0;
+        }       
+        
+        tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13
+        //             q15*q13 +          q13*q15      + q15*q13??
+        y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0])+(1<<14))>>15; // in q13
+        y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1])+(1<<14))>>15; // q13
+        
+        
+        int16 difz[2];
+        difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!!
+        difz[1]=(Y_mes[1]-y_est[1]);//<<2;
+        
+        y_old[0] = Y_mes[0];
+        y_old[1] = Y_mes[1];
+        
+        //bierman(int16 *difz, int16 *xp, int16 *U, int16 *D, int16 *R, unsigned int16 dimy, unsigned int16 dimx );
+        int16 dR[2];dR[0]=R[0];dR[1]=R[3];
+        //int16 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_fastC(difz,x_est,Uf,Df,C,dR,2,3);
+        //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)*Wref;
+        (mu)(1)=zprevod(x_est[1],15)*Thetaref;
+        (mu)(2)=zprevod(x_est[2],15)*Mref;
+        
+        //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 EKFfixedUD3::init_ekf3(double Tv)
+{
+        // Tuning of matrix Q
+        Q[0]=prevod(0.001,15);      // 1e-3
+        Q[4]=prevod(0.001,15);      // 1e-3
+        Q[8]=prevod(0.001,15);      // 1e-3
+        
+        Uf[0]=0x7FFF;// !       // 0.05
+        Uf[1]=Uf[2]=Uf[3]=0;
+        Uf[4]=0x7FFF;//!
+        Uf[5]=Uf[6]=Uf[7]=0;
+        Uf[8]=0x7FFF;//!
+        
+        Df[0]=1<<14;
+        Df[1]=1<<14;
+        Df[2]=1<<14;
+        
+        // Tuning of matrix R
+        R[0]=prevod(0.01,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); // < ===========
+        cH=prevod(Tv*Wref*Fmag*Thetaref/Iref/Ls,15); // 
+        //  cI=prevod(kp*p*p*Tv*Fmag*Iref/J/Wref*Thetaref);
+        
+        /* Init matrix PSI with permanently constant terms */
+        PSI[0]=0x7FFF;
+        PSI[1]=0;
+        PSI[2]= cF;
+        
+        PSI[3]=cG;
+        PSI[4]=0x7FFF;
+        PSI[5]=0;
+        
+        PSI[6]=0;
+        PSI[7]=0;
+        PSI[8]=0x7FFF;
+}
+
 
 void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -270 +270 @@
 
 /*!
+ * \brief Extended Kalman Filter with UD matrices in fixed point16 arithmetic
+ * 
+ * An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean.
+ */
+class EKFfixedUD3 : public BM {
+public:
+        LOG_LEVEL(EKFfixedUD3,logU, logG, logD, logA, logC, logP);
+        
+        void init_ekf3(double Tv);
+        void ekf3(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int16 Q[9]; /* matrix [4,4] */
+        int16 R[4]; /* matrix [2,2] */
+        
+        int16 x_est[3]; /* estimate and prediction */
+        int16 y_est[2]; /* estimate and prediction */
+        int16 y_old[2]; /* estimate and prediction */
+        
+        int16 PSI[9]; /* matrix [4,4] */
+        int16 PSIU[9]; /* matrix PIS*U, [4,4] */
+        int16 C[6]; /* matrix [4,4] */
+        
+        int16 Uf[9]; // upper triangular of covariance (inplace)
+        int16 Df[3];  // diagonal covariance
+        int16 Dfold[3]; // temp of D
+        int16 G[9];  // temp for bierman
+        
+        int16 cA, cB, cC, cG, cF, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<fsqmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedUD3 ():BM(),E(),Ry(2,2){
+                int16 i;
+                for(i=0;i<9;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<3;i++){x_est[i]=0;}
+                for(i=0;i<2;i++){y_est[i]=0;}
+                for(i=0;i<2;i++){y_old[i]=0;}
+                for(i=0;i<9;i++){Uf[i]=0;}
+                for(i=0;i<3;i++){Df[i]=0;}
+                for(i=0;i<4;i++){G[i]=0;}
+                for(i=0;i<3;i++){Dfold[i]=0;}
+                
+                for(i=0;i<9;i++){PSI[i]=0;}
+                for(i=0;i<6;i++){C[i]=0;}
+                
+                set_dim(3);
+                dimc = 2;
+                dimy = 2;
+                E._mu()=zeros(3);
+                E._R()=zeros(3,3);
+                init_ekf3(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 );         
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedUD3);
+
+/*!
  * \brief Extended Kalman Filter with Chol matrices in fixed point16 arithmetic
  *