Changeset 63

Timestamp:

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

Author:

smidl

Message:

odstraneni net a drobne upravy

Location:

tests

Files:

• pmsm.h (modified) (1 diff)
• pmsm_sim.cpp (modified) (2 diffs)
• pmsm_sim2.cpp (modified) (7 diffs)
• pmsm_sim3.cpp (modified) (8 diffs)

tests/pmsm.h

@@ -12 +12 @@
 //! State evolution model for a PMSM drive and its derivative with respect to \$x\$
 class IMpmsm : public diffbifn {
+protected:
         double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 
 public:
         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;}
+
+        vec eval ( const vec &x0, const vec &u0 ) {
+                // last state
+                double iam = x0 ( 0 );
+                double ibm = x0 ( 1 );
+                double omm = x0 ( 2 );
+                double thm = x0 ( 3 );
+                double uam = u0 ( 0 );
+                double ubm = u0 ( 1 );
+
+                vec xk=zeros ( 4 );
+                //ia
+                xk ( 0 ) = ( 1.0- Rs/Ls*dt ) * iam + Ypm/Ls*dt*omm * sin ( thm ) + uam*dt/Ls;
+                //ib
+                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;
+                //th
+                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi>
+                return xk;
+        }
+
+        void dfdx_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {
+                double iam = x0 ( 0 );
+                double ibm = x0 ( 1 );
+                double omm = x0 ( 2 );
+                double thm = x0 ( 3 );
+                // d ia
+                A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
+                A ( 0,2 ) = Ypm/Ls*dt* sin ( thm ); A ( 0,3 ) = Ypm/Ls*dt*omm * ( cos ( thm ) );
+                // d ib
+                A ( 1,0 ) = 0.0 ; A ( 1,1 ) = ( 1.0- Rs/Ls*dt );
+                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,2 ) = 1.0;
+                A ( 2,3 ) = 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;
+        }
+
+        void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full=true ) {it_error ( "not needed" );};
+
+};
+
+class IMpmsmStat : public IMpmsm {
+        IMpmsmStat() :IMpmsm() {};
         //! 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;}

tests/pmsm_sim.cpp

@@ -18 +18 @@
 #include "pmsm.h"
 #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;
@@ -146 +126 @@
         //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;
 }

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:
 

tests/pmsm_sim3.cpp

@@ -27 +27 @@
 void set_simulator_t(double &Ww) {
 
-        if (t>0.2) x[8]=1.2;    // 1A //0.2ZP
+        if (t>0.0002) 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
@@ -73 +73 @@
         if (t>8.5) x[8]=25.2;  // 21A
         
-//        x[8]=0.0;
+        x[8]=0.0;
 }
 
@@ -81 +81 @@
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 500;
+        int Npart = 100;
         
 
         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 Qdiag ( "0.05 0.05 0.002 0.001" ); //zdenek: 0.01 0.01 0.0001 0.0001
         vec Rdiag ( "0.05 0.05" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
@@ -99 +99 @@
         MPF<EKFfixed> M ( rx,rQ,evolQ,evolQ,Npart,KFE );
         // initialize
-        evolQ.set_parameters ( 1000.0 ,Qdiag, 0.5); //sigma = 1/10 mu
+        evolQ.set_parameters ( 10.0 ,Qdiag, 1.0); //sigma = 1/10 mu
         evolQ.condition ( Qdiag ); //Zdenek default
         epdf& pfinit=evolQ._epdf();
         M.set_est ( pfinit );
-        evolQ.set_parameters ( 100000.0, Qdiag, 0.99999 );
+        evolQ.set_parameters ( 500000.0, Qdiag, 0.9999 );
 
 
@@ -111 +111 @@
 
         mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-        mat Dt=zeros ( Ndat,4+2 ); //observation
+        mat Dt=zeros ( Ndat,4 ); //observation
         mat XtE=zeros ( Ndat, 4 );
         mat XtM=zeros ( Ndat,4+4 ); //Q + x
@@ -118 +118 @@
         pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
         double Ww=0.0;
+        static int k_rampa=1;
+        static long k_rampa_tmp=0;
+
         vec dt ( 2 );
         vec ut ( 2 );
@@ -125 +128 @@
                 for ( int ii=0; ii<Nsimstep;ii++ ) {
                         //simulator
-                        set_simulator_t(Ww);
+                        Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
+                        if ( Ww>2.*M_PI*150. ) {
+                                Ww=2.*M_PI*150.;
+                                if ( k_rampa_tmp<500000 ) k_rampa_tmp++;
+                                else {k_rampa=-1;k_rampa_tmp=0;}
+                        };
+                        if ( Ww<-2.*M_PI*150. ) Ww=-2.*M_PI*150.; /* */
+//                      set_simulator_t(Ww);
                         pmsmsim_step ( Ww );
                 };
@@ -138 +148 @@
                 
                 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))) ) );
+                Dt.set_row ( tK, concat ( dt,ut));
                 XtE.set_row ( tK,KFEep.mean() );
+                double qNaN = std::numeric_limits<double>::quiet_NaN();
                 XtM.set_row ( tK,Mep.mean() );
         }