Changeset 1305 for applications

Timestamp:

03/23/11 22:23:22 (13 years ago)

Author:

smidl

Message:

object for reduced order filter

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

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

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -549 +549 @@
 }
 
+void EKFfixedUD2::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+        ekf2(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedUD2::ekf2(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;
+        
+        // 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[3]/32768.),15);
+        if (tmp>32767) t_sin =32767; else t_sin=tmp;
+        tmp=prevod(cos(Thetaref*x_est[3]/32768.),15);
+        if (tmp>32767) t_cos =32767; else t_cos=tmp;
+        
+        tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13
+        //             q15*q13 +          q13*q15      + q15*q13??
+        x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13
+        x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13
+        x_est[2]=x_est[2];
+        //               q15              + q15*q13
+        x_est[3]=(((int32)x_est[3]<<15)+(int32)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]=((int32)cB*t_sin)>>15;
+                tmp=((int32)cH*x_est[2])>>15; // ! cH =cB with different scale!!
+                PSI[3]=((int32)tmp*t_cos)>>15;
+                PSI[6]=-((int32)cB*t_cos)>>15;
+                PSI[7]=((int32)tmp*t_sin)>>15;
+        }
+        {
+                int Ai[16];
+                for (int i=0;i<16; i++) Ai[i]=(int)(PSI[i]);
+                ivec Ad(Ai,16);
+                log_level.store(logA,get_from_ivec(Ad));
+        }
+        
+        ///////// end of copy ///////////////
+        mmultAU(PSI,Uf,PSIU,4,4);
+        //thorton(int16 *U, int16 *D, int16 *PSIU, int16 *Q, int16 *G, int16 *Dold, unsigned int16 dimx);
+        thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4);
+        
+        {
+                int Ui[16];     for (int i=0;i<16; i++) Ui[i]=(int)Uf[i];
+                
+                ivec Ud(Ui,16);
+                log_level.store(logU,get_from_ivec(Ud));
+                
+                int di[16];     for (int i=0;i<16; i++) di[i]=(int)Df[i];
+                ivec dd(di,4);
+                imat U(Ui,4,4);
+                mat U2=to_mat(U)/32768;
+                mat PP=U2*diag(to_vec(dd))*U2.T();
+                vec pp(PP._data(),16);
+                log_level.store(logP,pp);
+        }
+        {
+                int Gi[16];     for (int i=0;i<16; i++) Gi[i]=(int)G[i];
+                ivec Gd(Gi,16);
+                log_level.store(logG,get_from_ivec(Gd));
+        }
+        
+        
+        int16 difz[2];
+        difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!!
+        difz[1]=(Y_mes[1]-x_est[1]);//<<2;
+        
+        {
+                int Di[4];               for (int i=0;i<4; i++) Di[i]=(int)Df[i];
+                ivec dd(Di,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(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_fast(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
+        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 EKFfixedUD2::init_ekf2(double Tv)
+{
+        // Tuning of matrix Q
+        Q[0]=prevod(0.0005,15);      // 1e-3
+        Q[3]=prevod(0.0001,15);      // 1e-3
+        
+        Uf[0]=0x7FFF;// !       // 0.05
+        Uf[1]=Uf[2]=0;
+        Uf[5]=0x7FFF;//!
+        
+        Df[0]=1<<14;
+        Df[1]=1<<14;
+        
+        // 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); // < ===========
+        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]=0x7FFF;
+        PSI[2]=cG;
+        PSI[3]=0x7FFF;
+        
+}
+
+
 void EKFfixedCh::bayes(const itpp::vec& yt, const itpp::vec& ut)
 {

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -189 +189 @@
 
 UIREGISTER(EKFfixedUD);
+
+/*!
+ * \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 EKFfixedUD2 : public BM {
+public:
+        LOG_LEVEL(EKFfixedUD2,logU, logG, logD, logA, logP);
+        
+        void init_ekf2(double Tv);
+        void ekf2(double ux, double uy, double isxd, double isyd);
+        
+        /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/
+        int16 Q[4]; /* matrix [4,4] */
+        int16 R[4]; /* matrix [2,2] */
+        
+        int16 x_est[2]; /* estimate and prediction */
+        
+        int16 PSI[4]; /* matrix [4,4] */
+        int16 PSIU[4]; /* matrix PIS*U, [4,4] */
+        
+        int16 Uf[4]; // upper triangular of covariance (inplace)
+        int16 Df[2];  // diagonal covariance
+        int16 Dfold[2]; // temp of D
+        int16 G[4];  // temp for bierman
+        
+        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane
+        
+        enorm<fsqmat> E;
+        mat Ry;
+        
+public:
+        //! Default constructor
+        EKFfixedUD2 ():BM(),E(),Ry(2,2){
+                int16 i;
+                for(i=0;i<4;i++){Q[i]=0;}
+                for(i=0;i<4;i++){R[i]=0;}
+                
+                for(i=0;i<2;i++){x_est[i]=0;}
+                for(i=0;i<4;i++){Uf[i]=0;}
+                for(i=0;i<2;i++){Df[i]=0;}
+                for(i=0;i<4;i++){G[i]=0;}
+                for(i=0;i<2;i++){Dfold[i]=0;}
+                
+                for(i=0;i<4;i++){PSI[i]=0;}
+                
+                set_dim(2);
+                E._mu()=zeros(2);
+                E._R()=zeros(2,2);
+                init_ekf2(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, logG, RV("G",4), prefix );
+                L.add_vector ( log_level, logU, RV ("U", 4 ), prefix );
+                L.add_vector ( log_level, logD, RV ("D", 2 ), prefix );
+                L.add_vector ( log_level, logA, RV ("A", 4 ), prefix );
+                L.add_vector ( log_level, logP, RV ("P", 4 ), prefix );
+                
+        };
+        //void from_setting();
+};
+
+UIREGISTER(EKFfixedUD2);
 
 /*!