Changeset 62 for tests/pmsm_sim2.cpp

Timestamp:

04/06/08 20:14:56 (16 years ago)

Author:

smidl

Message:

nova simulace s EKFfixed a novy EKF na plnych maticich

Files:

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

tests/pmsm_sim2.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 );
-        }
-}
-
+#include "iopom.h"
 
 using namespace itpp;
@@ -54 +36 @@
 void set_simulator_t(double &Ww) {
 
-        if (t>0.0) x[8]=1.2;    // 1A //0.2ZP
+        if (t>0.2) x[8]=1.2;    // 1A //0.2ZP
         if (t>0.4) x[8]=10.8;   // 9A
         if (t>0.6) x[8]=25.2;  // 21A
@@ -99 +81 @@
         if (t>8.3) x[8]=10.8;   // 9A
         if (t>8.5) x[8]=25.2;  // 21A
+        
+//        x[8]=0.0;
 }
 
 int main() {
         // Kalman filter
-        int Ndat = 30000;
+        int Ndat = 90000;
         double h = 1e-6;
         int Nsimstep = 125;
@@ -113 +97 @@
         // internal model
         IMpmsm fxu;
-        //                  Rs    Ls        dt       Fmag(Ypm) kp   p    J     Bf(Mz)
+        //                  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 );
         // observation model
@@ -119 +103 @@
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        vec Qdiag ( "0.01 0.01 0.0001 0.0001" ); //zdenek: 0.01 0.01 0.0001 0.0001
-        vec Rdiag ( "0.005 0.005" ); //var(diff(xth)) = "0.034 0.034"
+//      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"
         chmat Q ( Qdiag );
         chmat R ( Rdiag );
         EKFCh KFE ( rx,ry,ru );
-        KFE.set_parameters ( &fxu,&hxu,Q,R );
-        KFE.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
+        KFE.set_est ( mu0, ( 1*eye ( 4 ) ) );
+        KFE.set_parameters ( &fxu,&hxu,Qdiag,Rdiag);
 
         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 );
-        KFEp.set_est ( mu0, chmat ( 1*ones ( 4 ) ) );
 
         mgamma_fix evolQ ( rQ,rQ );
@@ -139 +124 @@
         epdf& pfinit=evolQ._epdf();
         M.set_est ( pfinit );
-        evolQ.set_parameters ( 100000.0, Qdiag, 0.5 );
+        evolQ.set_parameters ( 5000.0, Qdiag, 0.9999 );
 
         //
@@ -190 +175 @@
         //Exit program:
 
+        char tmpstr[200];
+        sprintf(tmpstr,"%s/%s","here/","format");
+        ofstream  form(tmpstr);
+        form << "# Experimetal header file"<< endl;
+        dirfile_write(form,"here/",Xt,"X","{isa isb om th }" );
+        dirfile_write ( form,"here",XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
+        dirfile_write ( form,"here",XtE,"XE","{isa isb om th }" );
+        dirfile_write ( form,"here",Dt,"Dt","{isa isb ua ub }" );
+
         ////////////////
         // Just Testing
-        NcFile nc ( "pmsm_sim.nc", NcFile::Replace ); // Create and leave in define mode
+/*      NcFile nc ( "pmsm_sim2.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,XtM,"XtM","{q1 q2 q3 q4 isa isb om th }" );
         write_to_nc ( nc,XtE,"XE","{isa isb om th }" );
-        write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );
+        write_to_nc ( nc,Dt,"Dt","{isa isb ua ub }" );*/
         return 0;
 }