Changeset 62 for tests

Timestamp:

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

Author:

smidl

Message:

nova simulace s EKFfixed a novy EKF na plnych maticich

Location:

tests

Files:

• CMakeLists.txt (modified) (1 diff)
• pmsm.h (modified) (4 diffs)
• pmsm_sim2.cpp (modified) (7 diffs)
• pmsm_sim3.cpp (added)

tests/CMakeLists.txt

@@ -45 +45 @@
 include_directories (${BDM_SOURCE_DIR}/simulator_zdenek)
 link_directories (${BDM_BINARY_DIR}/simulator_zdenek)
+link_directories (${BDM_BINARY_DIR}/simulator_zdenek/ekf_example)
 add_executable (pmsm_sim pmsm_sim.cpp)
 add_executable (pmsm_sim2 pmsm_sim2.cpp)
+add_executable (pmsm_sim3 pmsm_sim3.cpp)
 target_link_libraries (pmsm_sim ${BdmLibs} pmsmsim netcdf_c++)
 target_link_libraries (pmsm_sim2 ${BdmLibs} pmsmsim netcdf_c++)
+target_link_libraries (pmsm_sim3 ${BdmLibs} pmsmsim ekf_obj)

tests/pmsm.h

@@ -10 +10 @@
 RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 ));
 
-//! State evolution model for a PMSM drive and its derivative with respect to $x$
+//! State evolution model for a PMSM drive and its derivative with respect to \$x\$
 class IMpmsm : public diffbifn {
         double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 
 public:
-        IMpmsm() :diffbifn ( rx, ru ) {};
+        IMpmsm() :diffbifn (rx.count(), rx, ru ) {};
         //! Set mechanical and electrical variables
         void set_parameters ( double Rs0, double Ls0, double dt0, double Ypm0, double kp0, double p0, double J0, double Mz0 ) {Rs=Rs0; Ls=Ls0; dt=dt0; Ypm=Ypm0; kp=kp0; p=p0; J=J0; Mz=Mz0;}
@@ -34 +34 @@
                 xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ubm*dt/Ls;
                 //om
-                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz;
+                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,2*pi); // <0..2pi>
@@ -52 +52 @@
                 A ( 1,2 ) = -Ypm/Ls*dt* cos ( thm ); A ( 1,3 ) = Ypm/Ls*dt*omm * ( sin ( thm ) );
                 // d om
-                A ( 2,0 ) = kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
-                A ( 2,1 ) = kp*p*p * Ypm/J*dt* ( cos ( thm ) );
+                A ( 2,0 ) = 0.0;//kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
+                A ( 2,1 ) = 0.0;//kp*p*p * Ypm/J*dt* ( cos ( thm ) );
                 A ( 2,2 ) = 1.0;
-                A ( 2,3 ) = kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
+                A ( 2,3 ) = 0.0;//kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );
                 // d th
                 A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
@@ -64 +64 @@
 };
 
-//! Observation model for PMSM drive and its derivative with respect to $x$
+//! Observation model for PMSM drive and its derivative with respect to \$x\$
 class OMpmsm: public diffbifn {
 public:
-        OMpmsm() :diffbifn ( rx,ru ) {};
+        OMpmsm() :diffbifn (2, rx,ru ) {};
 
         vec eval ( const vec &x0, const vec &u0 ) {

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;
 }