Changeset 54 for tests/pmsm_sim.cpp

Timestamp:

03/25/08 13:40:27 (17 years ago)

Author:

smidl

Message:

funkcni verze testu + pmsm

Files:

• tests/pmsm_sim.cpp (modified) (10 diffs)

tests/pmsm_sim.cpp

@@ -19 +19 @@
 #include "simulator.h"
 
+#include <netcdfcpp.h>
+void write_to_nc ( NcFile &nc, mat &X, std::string Xn, Array<std::string> A ) {
+        char tmpstr[200];
+        int Len = X.rows();
+
+        sprintf ( tmpstr,"%s.length",Xn.c_str() );
+        NcDim* lengt = nc.add_dim ( tmpstr, ( long ) Len );
+        for ( int j=0; j<X.cols(); j++ ) {
+                if ( j<A.length() )
+                        sprintf ( tmpstr,"%s_%s",Xn.c_str(), ( A ( j ) ).c_str() );
+                else
+                        sprintf ( tmpstr,"%s_%d",Xn.c_str(),j );
+                // Create variables and their attributes
+                NcVar* P = nc.add_var ( tmpstr, ncDouble, lengt );
+                const double* Dp = X._data();
+                P->put ( &Dp[j*Len],Len );
+        }
+}
+
+
 using namespace itpp;
 //!Extended Kalman filter with unknown \c Q
@@ -29 +49 @@
                 //from EKF
                 preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
-        }; 
+        };
 };
 
@@ -35 +55 @@
 int main() {
         // Kalman filter
-        int Ndat = 10000;
+        int Ndat = 30000;
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 100;
-        
+        int Npart = 1000;
+
         // internal model
         IMpmsm fxu;
@@ -48 +68 @@
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        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"
+        vec Qdiag ( "0.01 0.01 0.00001 0.00001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+        vec Rdiag ( "0.0005 0.0005" ); //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_est ( mu0, chmat ( 1*ones ( 4 ) ) );
 
-        RV rQ ( "100","{Q}","4","0" );
+        RV rQ ( "100","{Q}","2","0" );
         EKF_unQ KFEp ( rx,ry,ru,rQ );
         KFEp.set_parameters ( &fxu,&hxu,Q,R );
-        KFEp.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) );
+        KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
 
         mgamma evolQ ( rQ,rQ );
@@ -65 +85 @@
         // initialize
         evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu
-        evolQ.condition ( "0.05 0.05 0.001 0.001" ); //Zdenek default
+        evolQ.condition ( "0.01 0.01" ); //Zdenek default
         epdf& pfinit=evolQ._epdf();
         M.set_est ( pfinit );
-        evolQ.set_parameters ( 1000.0 ); 
+        evolQ.set_parameters ( 1000.0 );
 
         //
@@ -75 +95 @@
         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
+        mat Xt=zeros ( Ndat ,9 ); //true state from simulator
+        mat Dt=zeros ( Ndat,4+2 ); //observation
+        mat XtE=zeros ( Ndat, 4 );
+        mat XtM=zeros ( Ndat,6 ); //Q + x
 
         // SET SIMULATOR
@@ -86 +106 @@
         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++ ) {
@@ -112 +127 @@
                 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
@@ -123 +132 @@
                 M.bayes ( concat ( dt,ut ) );
 
-                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 );
+                Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
+                Dt.set_row ( tK, concat ( dt,ut,vec_1(sqrt(pow(ut(0),2)+pow(ut(1),2))), vec_1(sqrt(pow(dt(0),2)+pow(dt(1),2))) ) );
+                XtE.set_row ( tK,KFEep.mean() );
+                XtM.set_row ( tK,Mep.mean() );
         }
 
@@ -136 +144 @@
         fou << Name ( "xthE" ) << XtE;
         fou << Name ( "xthM" ) << XtM;
-        fou << Name ( "xthV" ) << XtV;
         //Exit program:
+
+        ////////////////
+        // Just Testing
+        NcFile nc ( "pmsm_sim.nc", NcFile::Replace ); // Create and leave in define mode
+        if ( ! nc.is_valid() ) {        std::cerr << "creation of NCFile failed."<<endl;}
+
+        write_to_nc ( nc,Xt,"X","{isa isb om th }" );
+        write_to_nc ( nc,XtM,"XtM","{q1 q2 isa isb om th }" );
+        write_to_nc ( nc,XtE,"XE","{isa isb om th }" );
+        write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );
         return 0;
+}
 
-}