Changeset 1341

Timestamp:

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

Author:

smidl

Message:

extended reduced model for unknown Torque

Location:

applications/pmsm

Files:

• pmsm.h (modified) (5 diffs)
• simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (5 diffs)
• simulator_zdenek/ekf_example/ekf_obj.h (modified) (1 diff)

applications/pmsm/pmsm.h

@@ -29 +29 @@
         bool cutoff;
 public:
-        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=true;cutoff=true;};
+        IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2; dimc=6;compensate=false;cutoff=true;};
         //! Set mechanical and electrical variables
         virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
@@ -134 +134 @@
         bool cutoff;
 public:
-        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=true;cutoff=true;};
+        IMpmsmOT() :diffbifn ( ) {dimy=2; dimx = 2; dimu=4; dimc=6;compensate=false;cutoff=true;};
         //! Set mechanical and electrical variables
         virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
@@ -188 +188 @@
         
         void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
-                const double &omm = x0 ( 0 );
+//              const double &omm = x0 ( 0 );
                 const double &thm = x0 ( 1 );
 
@@ -223 +223 @@
 
 UIREGISTER ( IMpmsmOT );
+
+//! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$
+class IMpmsmOTM : public diffbifn {
+protected:
+        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+        
+        bool compensate;
+        bool cutoff;
+public:
+        IMpmsmOTM() :diffbifn ( ) {dimy=2; dimx = 3; dimu=4; dimc=6;compensate=false;cutoff=true;};
+        //! Set mechanical and electrical variables
+        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
+        
+        void modelpwm(const vec &x0, const vec u0, double &ua, double &ub){
+                /*              ua=u0[0];
+                 *              ub=u0[1];
+                 *              return;*/
+                double sq3=sqrt ( 3.0 );
+                double i1=x0(0);
+                double i2=0.5* ( -i1+sq3*x0[1] );
+                double i3=0.5* ( -i1-sq3*x0[1] );
+                double u1=u0(0);
+                double u2=0.5* ( -u1+sq3*u0(1) );
+                double u3=0.5* ( -u1-sq3*u0(1) );
+                
+                double du1=1.4* ( double ( i1>0.3 ) - double ( i1<-0.3 ) ) +0.2*i1;
+                double du2=1.4* ( double ( i2>0.3 ) - double ( i2<-0.3 ) ) +0.2*i2;
+                double du3=1.4* ( double ( i3>0.3 ) - double ( i3<-0.3 ) ) +0.2*i3;
+                ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
+                ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
+        }
+        
+        vec eval ( const vec &x0, const vec &u0 ) {
+                // last state
+                const double &omm = x0 ( 0 );
+                const double &thm = x0 ( 1 );
+                const double &Mm  = x0 ( 2 );
+                double uam;
+                double ubm;
+                
+                if (compensate){
+                        modelpwm(x0,u0,uam,ubm);
+                } else {
+                        uam = u0(0);
+                        ubm = u0(1);
+                }
+                
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                vec xk( 3 );
+                //ia
+                //om
+                xk ( 0 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mm;
+                //th
+                xk ( 1 ) = thm + omm*dt; // <0..2pi>
+                xk (2) = Mm;
+                
+                if (cutoff) {
+                        if ( xk ( 1 ) >pi ) xk ( 1 )-=2*pi;
+                        if ( xk ( 1 ) <-pi ) xk ( 1 ) +=2*pi;
+                }
+                return xk;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+//              const double &omm = x0 ( 0 );
+                const double &thm = x0 ( 1 );
+//              const double &Mm  = x0 ( 2 );
+                
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                // d ia
+                // d om
+                A ( 0,0 ) = 1.0;
+                A ( 0,1 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
+                A ( 0,2 ) = -p/J*dt;
+                // d th
+                A ( 1,0 ) = dt; 
+                A ( 1,1 ) = 1.0;
+                A ( 1,2 ) = 0.0;
+                // d M
+                A ( 2,0 ) = 0.0;
+                A ( 2,1 ) = 0.0;
+                A ( 2,2 ) = 1.0;
+        }
+        
+        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
+        
+        void from_setting( const Setting &root )
+        {       
+                
+                const SettingResolver& params_b=root["params"];
+                const Setting& params=params_b.result;
+                
+                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
+                params["kp"], params["p"], params["J"], 0.0);
+                int comp=0;
+                if (UI::get(comp,root,"compensate",UI::optional)){compensate=(comp==1);}
+                int cuto=0;
+                if (UI::get(cuto,root,"cutoff",UI::optional)){cutoff=(cuto==1);}
+        };
+        
+        // TODO dodelat void to_setting( Setting &root ) const;
+};
+
+UIREGISTER ( IMpmsmOTM );
 
 //! Evolution model of \f$ \omega, \vartheta\f$ for a PMSM drive and its derivative with respect to \f$x\f$ -- Scaled to [-1,1]
@@ -766 +874 @@
 UIREGISTER ( OMpmsmOT );
 
+//! Observation model for PMSM drive in reduced form coordinates
+class OMpmsmOTM: public diffbifn {
+public:
+        double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+        
+        virtual void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
+        
+        OMpmsmOTM() :diffbifn () {dimy=2;dimx=2;dimu=4;};
+        
+        vec eval ( const vec &x0, const vec &u0 ) {
+                vec y ( 2 );
+                const double &omm = x0(0);
+                const double &thm = x0(1);
+                
+                const double &uam = u0 ( 0 );
+                const double &ubm = u0 ( 1 );
+                const double &iam = u0 ( 2 );
+                const double &ibm = u0 ( 3 );
+                
+                y ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
+                //ib
+                y ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
+                
+                return y;
+        }
+        
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                const double &omm = x0(0);
+                const double &thm = x0(1);
+                
+                A ( 0,0 ) = Ypm/Ls*dt* sin ( thm ); 
+                A ( 0,1 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
+                A( 0,2) = 0.0;
+                // d ib
+                A ( 1,0 ) = -Ypm/Ls*dt* cos ( thm ); 
+                A ( 1,1 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
+                A(1,2) = 0.0;
+                
+        }
+        
+        void from_setting( const Setting &root )
+        {       
+                
+                const SettingResolver& params_b=root["params"];
+                const Setting& params=params_b.result;
+                
+                set_parameters ( params["Rs"], params["Ls"], 125e-6, params["Fmag"], \
+                params["kp"], params["p"], params["J"], 0.0);
+        };
+        
+};
+
+UIREGISTER ( OMpmsmOTM );
+
+
+
 //! Observation model for PMSM drive in reduced form coordinates -- scaled to [-1,1]
 class OMpmsmOTsc: public diffbifn {

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
  *