Changeset 226

Timestamp:

01/12/09 13:28:19 (15 years ago)

Author:

smidl

Message:

korekce pro Theta

Files:

• pmsm/mpf_u_delta.cpp (modified) (6 diffs)

pmsm/mpf_u_delta.cpp

@@ -31 +31 @@
         EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {};
         void condition ( const vec &val ) {
-                pfxu->condition( val );
+                pfxu->condition ( val );
         };
 };
 
 class IMpmsm_delta :  public IMpmsm {
-        protected:
-                vec ud;
-        public:
-                IMpmsm_delta() :IMpmsm(),ud(2) {};
+protected:
+        vec ud;
+public:
+        IMpmsm_delta() :IMpmsm(),ud ( 2 ) {ud=zeros ( 2 );};
         //! Set mechanical and electrical variables
 
-                void condition(const vec &val){ud = val;}
-                vec eval ( const vec &x0, const vec &u0 ) {
+        void condition ( const vec &val ) {ud = val;}
+        vec eval ( const vec &x0, const vec &u0 ) {
                 // last state
-                        double iam = x0 ( 0 );
-                        double ibm = x0 ( 1 );
-                        double omm = x0 ( 2 );
-                        double thm = x0 ( 3 );
-                        double uam = u0 ( 0 );
-                        double ubm = u0 ( 1 );
+                double iam = x0 ( 0 );
+                double ibm = x0 ( 1 );
+                double omm = x0 ( 2 );
+                double thm = x0 ( 3 );
+                double uam = u0 ( 0 );
+                double ubm = u0 ( 1 );
 
-                        vec xk=zeros ( 4 );
+                vec xk=zeros ( 4 );
                 //ia
-                        xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + (uam+ud(0))*dt/Ls;
+                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + ( uam+ud ( 0 ) ) *dt/Ls;
                 //ib
-                        xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + (ubm+ud(1))*dt/Ls;
+                xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ( ubm+ud ( 1 ) ) *dt/Ls;
                 //om
-                        xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
+                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
                 //th
-                        xk ( 3 ) = thm + omm*dt; // <0..2pi>
-                        if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
-                        if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
-                        return xk;
-                }
+                xk ( 3 ) = thm + omm*dt; // <0..2pi>
+                if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
+                if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
+                return xk;
+        }
 
 };
@@ -76 +76 @@
         int Npart = 200;
 
+        mat Rnoise = randn ( 2,Ndat );
+
         // internal model
+        IMpmsm fxu0;
         IMpmsm_delta fxu;
         //                  Rs    Ls        dt       Fmag(Ypm)    kp   p    J     Bf(Mz)
         fxu.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
+        fxu0.set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
         // observation model
         OMpmsm hxu;
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        vec Qdiag ( "0.6 0.6 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-        vec Rdiag ( "1e-8 1e-8" ); //var(diff(xth)) = "0.034 0.034"
+        vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+        vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
         chmat R ( Rdiag );
         EKFCh KFE ( rx,ry,ru );
-        KFE.set_parameters ( &fxu,&hxu,Q,R );
-        KFE.set_est ( mu0, chmat ( zeros ( 4 ) ) );
+        KFE.set_parameters ( &fxu0,&hxu,Q,R );
+        KFE.set_est ( mu0, chmat ( ones ( 4 ) ) );
 
         RV rUd ( "{ud }", "2" );
         EKFCh_cond KFEp ( rx,ry,ru,rUd );
         KFEp.set_parameters ( &fxu,&hxu,Q,R );
-        KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );
+        KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) );
 
         mlnorm<ldmat> evolUd ( rUd,rUd );
@@ -101 +105 @@
         // initialize
         vec Ud0="0 0";
-        evolUd.set_parameters ( eye(2), vec_2(0.0,0.0), ldmat(eye(2)) ); 
-        evolUd.condition (Ud0 );
+        evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 10.0*eye ( 2 ) ) );
+        evolUd.condition ( Ud0 );
         epdf& pfinit=evolUd._epdf();
         M.set_est ( pfinit );
-        evolUd.set_parameters ( eye(2), vec_2(0.0,0.0), ldmat(0.1*eye(2)) ); 
-        
+        evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 0.1*eye ( 2 ) ) );
+
         mat Xt=zeros ( Ndat ,4 ); //true state from simulator
-        mat Dt=zeros ( Ndat,2+2 ); //observation
+        mat Dt=zeros ( Ndat,2+2+2 ); //observation
         mat XtE=zeros ( Ndat, 4 );
         mat Qtr=zeros ( Ndat, 4 );
         mat XtM=zeros ( Ndat,2+4 ); //W + x
+        mat XtMTh=zeros ( Ndat,1 );
 
         // SET SIMULATOR
@@ -118 +123 @@
         vec ut ( 2 );
         vec xt ( 4 );
-        vec xtm=zeros(4);
+        vec xtm=zeros ( 4 );
         double Ww=0.0;
-        vec vecW="1 2 4 9 4 2 0 -4 -9 -16 -4 0 0 0";
+        vec vecW="1 2 4 8 4 2 0 -4 -9 -16 -4 0 0 0";
         vecW*=10.0;
 
@@ -127 +132 @@
                 for ( int ii=0; ii<Nsimstep;ii++ ) {
                         //simulator
-                        sim_profile_vec01t(Ww,vecW);
+                        sim_profile_vec01t ( Ww,vecW );
                         pmsmsim_step ( Ww );
                 };
-                ut(0) = KalmanObs[0];
-                ut(1) = KalmanObs[1];
-                dt(0) = KalmanObs[2];
-                dt(1) = KalmanObs[3];
-                xt = vec(x,4);
-                
+                ut ( 0 ) = KalmanObs[0];
+                ut ( 1 ) = KalmanObs[1];
+                dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t );
+                dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t );
+                xt = vec ( x,4 );
+
                 //estimator
                 KFE.bayes ( concat ( dt,ut ) );
                 M.bayes ( concat ( dt,ut ) );
 
-                Xt.set_row ( tK, xt); //vec from C-array
-                Dt.set_row ( tK, concat ( dt,ut));
-                Qtr.set_row ( tK, Qdiag);
+                Xt.set_row ( tK, xt ); //vec from C-array
+                Dt.set_row ( tK, concat ( dt,ut, vec_2 ( KalmanObs[4],KalmanObs[5] ) ) );
+                Qtr.set_row ( tK, Qdiag );
                 XtE.set_row ( tK,KFE._e()->mean() );
                 XtM.set_row ( tK,M._e()->mean() );
+                // correction for theta
+
+                {
+                        double sumSin=0.0;
+                        double sumCos=0.0;
+                        vec mea ( 4 );
+                        vec* _w;
+
+                        for ( int p=0; p<Npart;p++ ) {
+                                mea = M._BM ( p )->_e()->mean();
+                                _w = M.__w();
+                                sumSin += ( *_w ) ( p ) *sin ( mea ( 3 ) );
+                                sumCos += ( *_w ) ( p ) *cos ( mea ( 3 ) );
+                        }
+                        double Th = atan2 ( sumSin, sumCos );
+
+                        XtMTh.set_row ( tK,vec_1 ( Th ) );
+                }
+
         }
 
@@ -154 +178 @@
         fou << Name ( "xthE" ) << XtE;
         fou << Name ( "xthM" ) << XtM;
+        fou << Name ( "xthMTh" ) << XtMTh;
         //Exit program: