Changeset 744 for applications/pmsm

Timestamp:

11/28/09 14:38:48 (15 years ago)

Author:

smidl

Message:

Working unitsteps and controlloop + corresponding fixes

Location:

applications/pmsm

Files:

• CMakeLists.txt (modified) (1 diff)
• TR2245/CMakeLists.txt (modified) (1 diff)
• TR2245/unitsteps.cfg (modified) (1 diff)
• TR2245/unitsteps.cpp (modified) (8 diffs)
• pmsmDS.h (modified) (5 diffs)

applications/pmsm/CMakeLists.txt

@@ -33 +33 @@
 #EXEC (sim_var pmsmsim)
 
-#add_subdirectory (TR2245)
+add_subdirectory (TR2245)

applications/pmsm/TR2245/CMakeLists.txt

@@ -2 +2 @@
 EXEC (unitsteps pmsmsim)
 EXEC (wishart pmsmsim)
-EXEC (pmsm_wishart pmsmsim)
+#EXEC (pmsm_wishart pmsmsim)

applications/pmsm/TR2245/unitsteps.cfg

@@ -1 +1 @@
 ndat = 9000;
-Npart = 1000;
+Npart = 100;
 
 //simulator 
 profile = [1,2,4,9,4,2,0,-4,-9,-16,-4,0,0];
 
-logger = {type="dirfilelog"; dirname="exp/unitstep"; maxlen=100;};
+logger = {class="dirfilelog"; dirname="kst/unitstep"; maxlen=100;};
 
-Qrw = {type="migamma_ref";
-        ref = [1e-6, 1e-6, 1e-3, 1e-4];
+Qrw = {class="migamma_ref";
+        ref = [1e-4, 1e-4, 1e-3, 1e-4];
         l = 0.999;
-        k = 0.1;
+        k = 0.06;
 };
 // For EKF
 dQ=[1e-5, 1e-5, 1e-4, 1e-5];
-dR=[1e-8, 1e-8];
+dR=[1e-10, 1e-10];
 

applications/pmsm/TR2245/unitsteps.cpp

@@ -15 +15 @@
 #include <estim/particles.h>
 #include <estim/ekf_template.h>
-#include <stat/functions.h>
+#include <base/loggers.h>
 
 
@@ -21 +21 @@
 #include "simulator.h"
 #include "../sim_profiles.h"
-#include "user_info.h"
-#include "stat/loggers.h"
 
 using namespace bdm;
@@ -41 +39 @@
         F.lookupValue ( "ndat", Ndat );
         F.lookupValue ( "Npart",Npart );
-        pdf* evolQ = UI::build<pdf>( F, "Qrw" );
+        shared_ptr<pdf> evolQ = UI::build<pdf>( F, "Qrw" );
         vec Qdiag;
         vec Rdiag;
@@ -49 +47 @@
         // internal model
 
-IMpmsm fxu;
+        shared_ptr<IMpmsm> fxu= new IMpmsm;
 //                  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 );
+        fxu->set_parameters ( 0.28, 0.003465, Nsimstep*h, 0.1989, 1.5 ,4.0, 0.04, 0.0 );
         // observation model
-        OMpmsm hxu;
+        shared_ptr<OMpmsm> hxu=new OMpmsm;
 
         vec mu0= "0.0 0.0 0.0 0.0";
@@ -59 +57 @@
         chmat R ( Rdiag );
         EKFCh KFE ;
-        KFE.set_parameters ( &fxu,&hxu,Q,R );
-        KFE.set_est ( mu0, chmat ( zeros ( 4 ) ) );
+        KFE.set_parameters ( fxu,hxu,Q,R );
+        KFE.set_statistics ( mu0, chmat ( zeros ( 4 ) ) );
         KFE.set_rv ( rx );
+        KFE.validate();
 
         RV rQ ( "{Q }","4" );
+        RV rU ("{u }","2");
+        RV rY ("{y }","2");
         EKFCh_dQ KFEp ;
-        KFEp.set_parameters ( &fxu,&hxu,Q,R );
-        KFEp.set_est ( mu0, chmat ( zeros ( 4 ) ) );
+        KFEp.set_parameters ( fxu,hxu,Q,R );
+        KFEp.set_statistics ( mu0, chmat ( zeros ( 4 ) ) );
+        KFEp.set_rv(rx);
+        KFEp.set_yrv(rY); 
+        KFEp.set_rvc(concat(rU, rQ)); 
+        KFEp.validate();
 
-        MPF<EKFCh_dQ> M;
-        M.set_parameters ( evolQ,evolQ,Npart );
-        // initialize
-        evolQ->condition ( 10*Qdiag ); //Zdenek default
-        M.set_statistics ( evolQ->_e() , &KFEp );
-        //
+        MPF M;
+        evolQ->set_rv(rQ);
+        M.set_pf ( evolQ,Npart );
+        M._pf().set_statistics(ones(Npart), euni(zeros(4),2*Qdiag));
+        M.set_BM(KFEp);
+        M.set_yrv ( rY   );
+        M.set_rvc ( rU   );
+        M.validate();
 
-        M.set_rv ( concat ( rQ,rx ) );
-
-        dirfilelog *L = UI::build<dirfilelog>( F, "logger" );// ( "exp/mpf_test",100 );
-        int l_X = L->add ( rx, "xt" );
-        int l_D = L->add ( concat ( ry,ru ), "" );
-        int l_Q= L->add ( rQ, "" );
+        shared_ptr<dirfilelog> L = UI::build<dirfilelog>( F, "logger" );// ( "exp/mpf_test",100 );
+        int l_X = L->add_vector ( rx, "xt" );
+        int l_D = L->add_vector ( concat ( ry,ru ), "" );
+        int l_Q= L->add_vector ( rQ, "" );
 
         KFE.set_options ( "logbounds" );
-        KFE.log_add ( *L,"KF" );
+        KFE.log_register ( *L,"KF" );
         M.set_options ( "logbounds" );
-        M.log_add ( *L,"M" );
+        M.log_register ( *L,"M" );
         L->init();
 
@@ -107 +112 @@
                 ut ( 0 ) = KalmanObs[4];
                 ut ( 1 ) = KalmanObs[5];
-                xt = fxu.eval ( xtm,ut ) + diag ( sqrt ( Qdiag ) ) *randn ( 4 );
-                dt = hxu.eval ( xt,ut );
+                xt = fxu->eval ( xtm,ut ) + diag ( sqrt ( Qdiag ) ) *randn ( 4 );
+                dt = hxu->eval ( xt,ut ) + diag(sqrt(Rdiag))*randn(2);
                 xtm = xt;
 
@@ -122 +127 @@
 
                 //estimator
-                KFE.bayes ( concat ( dt,ut ) );
-                M.bayes ( concat ( dt,ut ) );
+                KFE.bayes (  dt,ut  );
+                M.bayes ( dt,ut );
 
-                L->logit ( l_X,xt );
-                L->logit ( l_D,concat ( dt,ut ) );
-                L->logit ( l_Q,Qdiag );
+                L->log_vector ( l_X,xt );
+                L->log_vector ( l_D,concat ( dt,ut ) );
+                L->log_vector ( l_Q,Qdiag );
 
-                KFE.logit ( *L );
-                M.logit ( *L );
+                KFE.log_write ( );
+                M.log_write (  );
                 L->step();
         }
@@ -136 +141 @@
         //Exit program:
 
-        delete L;
         return 0;
 }

applications/pmsm/pmsmDS.h

@@ -118 +118 @@
     void log_register ( logger &L )
     {
-        L_x = L.add ( rx, "x" );
-        L_oy = L.add ( ry, "o" );
-        L_ou = L.add ( ru, "o" );
-        L_iu = L.add ( ru, "t" );
+        L_x = L.add_vector ( rx, "x" );
+        L_oy = L.add_vector ( ry, "o" );
+        L_ou = L.add_vector ( ru, "o" );
+        L_iu = L.add_vector ( ru, "t" );
         // log differences
         if ( opt_modu )
         {
-            L_optu = L.add ( ru, "model" );
+            L_optu = L.add_vector ( ru, "model" );
         }
     }
@@ -131 +131 @@
     void log_write ( logger &L )
     {
-        L.logit ( L_x, vec ( x,4 )      );
-        L.logit ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
-        L.logit ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
-        L.logit ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
+        L.log_vector ( L_x, vec ( x,4 ) );
+        L.log_vector ( L_oy, vec_2 ( KalmanObs[2],KalmanObs[3] ) );
+        L.log_vector ( L_ou, vec_2 ( KalmanObs[0],KalmanObs[1] ) );
+        L.log_vector ( L_iu, vec_2 ( KalmanObs[4],KalmanObs[5] ) );
         if ( opt_modu )
         {
@@ -151 +151 @@
             ua = ( 2.0* ( u1-du1 )- ( u2-du2 )- ( u3-du3 ) ) /3.0;
             ub = ( ( u2-du2 )- ( u3-du3 ) ) /sq3;
-            L.logit ( L_optu , vec_2 ( ua,ub ) );
+            L.log_vector ( L_optu , vec_2 ( ua,ub ) );
         }
 
@@ -238 +238 @@
     void log_add(logger &L, const string &name="" )
     {
-        L_CRB=L.add(rx,"crb");
-        L_err=L.add(rx,"err");
-        L_sec=L.add(rx,"d2");
+        L_CRB=L.add_vector(rx,"crb");
+        L_err=L.add_vector(rx,"err");
+        L_sec=L.add_vector(rx,"d2");
     }
     void logit(logger &L)
     {
-        L.logit(L_err, interr);
-        L.logit(L_CRB,diag(_R()));
-        L.logit(L_sec,secder);
+        L.log_vector(L_err, interr);
+        L.log_vector(L_CRB,diag(_R()));
+        L.log_vector(L_sec,secder);
     }
 
@@ -322 +322 @@
     void log_add(logger &L, const string &name="" )
     {
-        L_CRB=L.add(concat(rx,RV("Mz",1,0)),"crbz");
+        L_CRB=L.add_vector(concat(rx,RV("Mz",1,0)),"crbz");
     }
     void logit(logger &L)
     {
-        L.logit(L_CRB,diag(_R()));
+        L.log_vector(L_CRB,diag(_R()));
     }