Changeset 63 for tests/pmsm_sim2.cpp

Timestamp:

04/09/08 15:29:53 (16 years ago)

Author:

smidl

Message:

odstraneni net a drobne upravy

Files:

• tests/pmsm_sim2.cpp (modified) (7 diffs)

tests/pmsm_sim2.cpp

@@ -32 +32 @@
                 preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
         };
+        void bayes(const vec dt){
+        EKFCh::bayes(dt);
+                
+                vec xtrue(4);
+                //UGLY HACK!!! reliance on a predictor!!
+                xtrue(0)=x[0];
+                xtrue(1)=x[1];
+                xtrue(2)=x[2];
+                xtrue(3)=x[3];
+                
+                ll = -0.5* ( 4 * 1.83787706640935 +_P->logdet() +xtrue* ( inv(_P->to_mat())*xtrue ) );
+        }
+};
+class EKF_unQful : public EKFfull , public BMcond {
+public:
+        //! Default constructor
+        EKF_unQful ( RV rx, RV ry,RV ru,RV rQ ) :EKFfull ( rx,ry,ru ),BMcond ( rQ ) {};
+        void condition ( const vec &Q0 ) {
+                Q=diag(Q0);
+        };
+        void bayes(const vec dt){
+        EKFfull::bayes(dt);
+                
+                vec xtrue(4);
+                //UGLY HACK!!! reliance on a predictor!!
+                xtrue(0)=x[0];
+                xtrue(1)=x[1];
+                xtrue(2)=x[2];
+                xtrue(3)=x[3];
+                
+                BM::ll = -0.5* ( 4 * 1.83787706640935 +log(det(P)) +xtrue* ( inv(P)*xtrue ) );
+        }
 };
 
@@ -82 +114 @@
         if (t>8.5) x[8]=25.2;  // 21A
         
-//        x[8]=0.0;
+        if (t>9) Ww=2.*M_PI*0;
+        x[8]=0.0;
 }
 
@@ -90 +123 @@
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 100;
+        int Npart = 50;
         
         //StrSim:06:
@@ -104 +137 @@
         vec mu0= "0.0 0.0 0.0 0.0";
 //      vec Qdiag ( "0.01 0.01 0.01 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-        vec Qdiag ( "18 18 157.9 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-        vec Rdiag ( "90 90" ); //var(diff(xth)) = "0.034 0.034"
+        vec Qdiag ( "10 10 10 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+        vec Rdiag ( "100 100" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
         chmat R ( Rdiag );
         EKFCh KFE ( rx,ry,ru );
-        KFE.set_est ( mu0, ( 1*eye ( 4 ) ) );
-        KFE.set_parameters ( &fxu,&hxu,Qdiag,Rdiag);
+        KFE.set_est ( mu0, chmat( 1*eye ( 4 ) ) );
+        KFE.set_parameters ( &fxu,&hxu,Q,R);
 
         RV rQ ( "100","{Q}","4","0" );
-        EKF_unQ KFEp ( rx,ry,ru,rQ );
-        KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
-        KFEp.set_parameters ( &fxu,&hxu,Q,R );
+        EKF_unQful KFEp ( rx,ry,ru,rQ );
+        KFEp.set_est ( mu0,  1*ones ( 4 ) );
+        KFEp.set_parameters ( &fxu,&hxu,diag(Qdiag),diag(Rdiag) );
 
         mgamma_fix evolQ ( rQ,rQ );
-        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
+        MPF<EKF_unQful> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
         // initialize
         evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
@@ -124 +157 @@
         epdf& pfinit=evolQ._epdf();
         M.set_est ( pfinit );
-        evolQ.set_parameters ( 5000.0, Qdiag, 0.9999 );
+        evolQ.set_parameters ( 100000.0, Qdiag, 0.9999 );
 
         //
@@ -166 +199 @@
         }
 
-        it_file fou ( "pmsm_sim.it" );
+/*      it_file fou ( "pmsm_sim.it" );
 
         fou << Name ( "xth" ) << Xt;
@@ -172 +205 @@
         fou << Name ( "xthE" ) << XtE;
         fou << Name ( "xthM" ) << XtM;
-        fou << Name ( "SSAT" ) << SSAT;
+        fou << Name ( "SSAT" ) << SSAT;*/
         //Exit program: