Changeset 48

Timestamp:

03/20/08 21:41:51 (16 years ago)

Author:

smidl

Message:

meziverze...

Files:

• matlab/pmsm_sim_disp.m (modified) (1 diff)
• tests/pmsm.h (modified) (3 diffs)
• tests/pmsm_sim.cpp (modified) (6 diffs)

matlab/pmsm_sim_disp.m

@@ -12 +12 @@
         hold on;
         plot(xthE(i,:)','r')
+        plot(xthM(i+4,:)','g')
+        plot(xthV(i,:)','m');
 end
 
 figure(3)
-ndat = size(ll,2);
-hold off
-plot(llgrid,ll(:,floor(ndat/3)),':');
-hold on
-plot(llgrid,ll(:,floor(2*ndat/3)),'--');
-plot(llgrid,ll(:,ndat));
+for i=1:4
+        subplot(4,1,i)
+        plot(xthM(i,:)')
+end
+
+
+% figure(3)
+% ndat = size(ll,2);
+% hold off
+% plot(llgrid,ll(:,floor(ndat/3)),':');
+% hold on
+% plot(llgrid,ll(:,floor(2*ndat/3)),'--');
+% plot(llgrid,ll(:,ndat));

tests/pmsm.h

@@ -2 +2 @@
 #define PMSM_H
 
+//TODO hardcoded RVs!!!
 RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 ));
 RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 ));
@@ -10 +11 @@
         double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 
-//TODO hardcoded RVs!!!
 public:
         IMpmsm() :diffbifn ( rx, ru ) {};
@@ -33 +33 @@
                 xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
                 //th
-                xk ( 3 ) = rem(thm + omm*dt,pi); //or 2*pi?
+                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
                 return xk;
         }

tests/pmsm_sim.cpp

@@ -20 +20 @@
 
 using namespace itpp;
-/*
-// PMSM with Q on Ia and Ib given externally
-class EKF_unQ : public EKF<chmat> , public BMcond {
+//!Extended Kalman filter with unknown \c Q
+class EKF_unQ : public EKFCh , public BMcond {
 public:
-        EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){};
-        void condition(const vec &Q0){};
-};*/
+        //! Default constructor
+        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {};
+        void condition ( const vec &Q0 ) {
+                Q.setD ( Q0,0 );
+                //from EKF
+                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
+        }; 
+};
 
 
@@ -33 +37 @@
         int Ndat = 10000;
         double h = 1e-6;
-        int Nsimstep = 20;
+        int Nsimstep = 125;
+        int Npart = 100;
         
-        const int Nll = 10;
-
-//      cout << KF;
         // internal model
         IMpmsm fxu;
@@ -46 +48 @@
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        vec Qdiag ( "0.03 0.03 0.001 0.00001" );
-        vec Rdiag ( "0.000001 0.000001" ); //var(diff(xth)) = "0.034 0.034"
-        vec vQ = "0.01:0.01:0.1";
+        vec Qdiag ( "0.05 0.05 0.001 0.001" ); //zdenek: 0.05 0.05 0.001 0.001
+        vec Rdiag ( "0.03 0.03" ); //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 ( 1000*ones ( 4 ) ) );
-        KFE.set_parameters ( &fxu,&hxu,Q,R );
 
-        mat ll ( Nll,Ndat );
+        RV rQ ( "100","{Q}","4","0" );
+        EKF_unQ KFEp ( rx,ry,ru,rQ );
+        KFEp.set_parameters ( &fxu,&hxu,Q,R );
+        KFEp.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
 
-        EKFCh* kfArray[Nll];
+        mgamma evolQ ( rQ,rQ );
+        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+        // initialize
+        evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
+        evolQ.condition ( "0.05 0.05 0.001 0.001" ); //Zdenek default
+        epdf& pfinit=evolQ._epdf();
+        M.set_est ( pfinit );
+        evolQ.set_parameters ( 1000.0 ); 
 
-
-        for ( int i=0;i<Nll;i++ ) {
-                vec Qid ( Qdiag );
-                Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i );
-                kfArray[i]= new EKFCh ( rx,ry,ru );
-                kfArray[i]->set_est ( mu0, chmat ( 100*ones ( 4 ) ) );
-                kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R );
-        }
+        //
 
         epdf& KFEep = KFE._epdf();
+        epdf& Mep = M._epdf();
 
         mat Xt=zeros ( 9,Ndat ); //true state from simulator
         mat Dt=zeros ( 4,Ndat ); //observation
         mat XtE=zeros ( 4,Ndat );
+        mat XtM=zeros ( 8,Ndat ); //Q + x
 
         // SET SIMULATOR
@@ -80 +86 @@
         static long k_rampa_tmp=0;
         vec dt ( 2 );
+        vec dtVS =zeros( 2 );
+        vec xtVS =zeros(4);
+        vec et ( 4 );
+        vec wt ( 2 );
         vec ut ( 2 );
+        mat XtV=zeros ( 4,Ndat );
 
         for ( int tK=1;tK<Ndat;tK++ ) {
@@ -101 +112 @@
                 dt ( 0 ) = KalmanObs[2];
                 dt ( 1 ) = KalmanObs[3];
+                
+                // My own simulator for testing : Asuming ut is OK
+                NorRNG.sample_vector ( 4,et );
+                NorRNG.sample_vector ( 2,wt );
+                xtVS = fxu.eval ( xtVS,ut ) + Q.sqrt_mult ( et );
+                dtVS = hxu.eval ( xtVS,ut ) + R.sqrt_mult ( wt );
+
                 //estimator
                 KFE.bayes ( concat ( dt,ut ) );
-                for ( int i=0;i<Nll;i++ ) {
-                        kfArray[i]->bayes ( concat ( dt,ut ) );
-                        ll ( i,tK ) =ll ( i,tK-1 ) + kfArray[i]->_ll();
-                }
+                M.bayes ( concat ( dt,ut ) );
 
-                Xt.set_col ( tK,vec ( x,9 ) );
+                Xt.set_col ( tK,vec ( x,9 ) ); //vec from C-array
                 Dt.set_col ( tK, concat ( dt,ut ) );
                 XtE.set_col ( tK,KFEep.mean() );
+                XtM.set_col ( tK,Mep.mean() );
+                XtV.set_col ( tK,xtVS );
         }
 
@@ -118 +135 @@
         fou << Name ( "Dt" ) << Dt;
         fou << Name ( "xthE" ) << XtE;
-        fou << Name ( "ll" ) << ll;
-        fou << Name ( "llgrid" ) << vQ;
+        fou << Name ( "xthM" ) << XtM;
+        fou << Name ( "xthV" ) << XtV;
         //Exit program:
         return 0;