Changeset 1179 for applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

Timestamp:

09/02/10 23:21:43 (14 years ago)

Author:

smidl

Message:

fast versions of thorton and bierman

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -443 +443 @@
         UI::get(log_level, set, "log_level", UI::optional);
 }
+
+
+void EKFfixedUD::bayes(const itpp::vec& yt, const itpp::vec& ut)
+{
+ekf(ut[0],ut[1],yt[0],yt[1]);
+}
+
+
+void EKFfixedUD::ekf(double ux, double uy, double isxd, double isyd)
+{
+        // vypocet napeti v systemu (x,y)
+        int uf[2];
+        uf[0]=prevod(ux/Uref,Qm);
+        uf[1]=prevod(uy/Uref,Qm);
+        
+        int Y_mes[2];
+        // zadani mereni
+        Y_mes[0]=prevod(isxd/Iref,Qm);
+        Y_mes[1]=prevod(isyd/Iref,Qm);
+        
+        ////// 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);
+        
+        tmp=((long)cB*x_est[2])>>15;
+        x_est[0]=((long)cA*x_est[0]+(long)tmp*t_sin+(long)cC*uf[0])>>15;
+        x_est[1]=((long)cA*x_est[1]-(long)tmp*t_cos+(long)cC*uf[1])>>15;
+        x_est[2]=x_est[2];
+        x_est[3]=(((long)x_est[3]<<15)+(long)cG*x_est[2])>>15;
+        
+        //void EKFfixed::update_psi(void)
+        {               
+                PSI[2]=((long)cB*t_sin)>>15;
+                tmp=((long)cH*x_est[2])>>15;
+                PSI[3]=((long)tmp*t_cos)>>15;
+                PSI[6]=-((long)cB*t_cos)>>15;
+                PSI[7]=((long)tmp*t_sin)>>15;
+        }
+        
+        ///////// end of copy ///////////////
+        mmultAU(PSI,Uf,PSIU,4,4);
+        //thorton(int *U, int *D, int *PSIU, int *Q, int *G, int *Dold, unsigned int dimx);
+        thorton(Uf,Df,PSIU,Q,G,Dfold,4);
+        
+        int difz[2];
+        difz[0]=Y_mes[0]-x_est[0];
+        difz[1]=Y_mes[1]-x_est[1];
+        //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];
+        bierman_fast(difz,x_est,Uf,Df,dR,2,4);
+        
+        // navrat estimovanych hodnot regulatoru
+        vec& mu = E._mu();
+        (mu)(0)=zprevod(x_est[0],Qm)*Iref;
+        (mu)(1)=zprevod(x_est[1],Qm)*Iref;
+        (mu)(2)=zprevod(x_est[2],Qm)*Wref;
+        (mu)(3)=zprevod(x_est[3],15)*Thetaref;
+        
+//      mat T=diag(concat(vec_2(Iref,Iref), vec_2(Wref,Thetaref)));
+}
+
+void EKFfixedUD::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
+
+        Uf[0]=0x7FFF;       // 0.05
+        Uf[5]=0x7FFF;
+        Uf[10]=0x7FFF;      // 1e-3
+        Uf[15]=0x7FFF;      // 1e-3
+        
+        Df[0]=0x7FFF;
+        Df[1]=0x7FFF;
+        Df[2]=0x7FFF;
+        Df[3]=0x7FFF;
+        
+        // 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*4/Thetaref,15);
+        cH=prevod(Tv*Wref*Fmag/Iref/Ls*Thetaref,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;
+        
+        //////////////////////// =================
+        ///// 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;
+}
+