Changeset 318

Timestamp:

04/23/09 13:19:04 (15 years ago)

Author:

smidl

Message:

pmsm CRB model

Files:

• doprava/k1.cpp (modified) (2 diffs)
• pmsm/pmsm.h (modified) (5 diffs)
• pmsm/pmsmDS.h (modified) (6 diffs)
• pmsm/pmsm_estim.cpp (modified) (2 diffs)
• pmsm/pmsm_ui.h (modified) (3 diffs)
• pmsm/simulator_zdenek/simulator.cpp (modified) (2 diffs)
• pmsm/simulator_zdenek/simulator.h (modified) (1 diff)

doprava/k1.cpp

@@ -49 +49 @@
 
         // Definovat co se bude logovat
-        Efix.log_add(&L,"E");
+        Efix.log_add(L,"E");
         L.init(); // <<==== allocate memory for results
         
@@ -58 +58 @@
 
                 //LOG results
-                Efix.logit(&L); 
+                Efix.logit(L); 
                 
                 L.step();

pmsm/pmsm.h

@@ -27 +27 @@
         IMpmsm() :diffbifn ( ) {dimy=4; dimx = 4; dimu=2;};
         //! 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;}
+        virtual 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 ) {
@@ -77 +77 @@
 
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
-class IMpmsm2o : public diffbifn {
+class IMpmsm2o : public IMpmsm {
         protected:
-                double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
+//              double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
                 //! store first derivatives for the use in second derivatives
                 double dia, dib, dom, dth;
@@ -86 +86 @@
                 double iam, ibm, omm, thm, uam, ubm;
         public:
-                IMpmsm2o() :diffbifn () {dimy=4;dimx=4;dimu=2;};
+                IMpmsm2o() :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; d2t=dt*dt/2;}
@@ -111 +111 @@
                         xk ( 1 ) = ibm + dt*dib;// +d2t*d2ib;
                         xk ( 2 ) = omm +dt*dom;// +d2t*d2om;
-                        xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi>
+                        xk ( 3 ) = thm + dt*dth;// +d2t*dom; // <0..2pi>
                         
                         if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
@@ -148 +148 @@
                 // d th
                         A ( 3,0 ) = 0.0; A ( 3,1 ) = 0.0; A ( 3,2 ) = dt; A ( 3,3 ) = 1.0;
+                        // FOR d2t*dom!!!!!!!!!
+/*                      A ( 3,0 ) = dt* kp*p*p * Ypm/J*dt* ( - sin ( thm ) );
+                        A ( 3,1 ) = dt* kp*p*p * Ypm/J*dt* ( cos ( thm ) );
+                        A ( 3,2 ) = dt;
+                        A ( 3,3 ) = 1.0 + dt* kp*p*p * Ypm/J*dt* ( -ibm * sin ( thm )-iam * cos ( thm ) );*/
                 }
 

pmsm/pmsmDS.h

@@ -12 +12 @@
 
 #include <stat/loggers.h>
+#include <estim/libKF.h>
 #include "simulator.h"
 #include "pmsm.h"
@@ -23 +24 @@
         //! Setpoints of omega in timespans given by dt_prof
         vec profileWw;
+        //! Setpoints of Mz in timespans given by dt_prof
+        vec profileMz;
         //! time-step for profiles
         double dt_prof;
@@ -29 +32 @@
         //! options for logging, - log predictions of 'true' voltage
         bool opt_modu;
+        //! options for logging, - 
 public:
         //! Constructor with fixed sampling period
@@ -46 +50 @@
                 static double dW; // increase of W
                 static double Ww; // W
+                static double Mz; // W
                 if ( t>=dt_prof*ind ) {
                         ind++;
+                        // check omega profile and set dW
                         if ( ind<profileWw.length() ) {
                                 //linear increase
@@ -61 +67 @@
                                 dW = 0;
                         }
+                        // Check Mz profile and set Mz
+                        if ( ind<profileMz.length() ) {
+                                //sudden increase
+                                Mz = profileMz(ind);
+                        }
+                        else {
+                                Mz = 0;
+                        }
                 }
                 Ww += dW;
                 //Simulate Dt seconds!
-                for ( int i=0;i<Dt;i++ ) {      pmsmsim_step ( Ww );}
+                for ( int i=0;i<Dt;i++ ) {      pmsmsim_step ( Ww , Mz);}
         };
 
@@ -100 +114 @@
                         L.logit ( L_optu , vec_2 ( ua,ub ) );
                 }
+                
         }
 
-        void set_profile ( double dt, const vec &Ww ) {dt_prof=dt; profileWw=Ww;}
+        void set_profile ( double dt, const vec &Ww, const vec &Mz ) {dt_prof=dt; profileWw=Ww; profileMz=Mz;}
 };
+
+//! This class behaves like BM but it is evaluating EKF
+class pmsmCRB : public EKFCh{
+        protected:
+                vec interr;
+                vec old_true;
+                vec secder;
+                int L_CRB;
+                int L_err;
+                int L_sec;
+        public:
+        //! constructor
+        pmsmCRB():EKFCh(){old_true=zeros(6);}
+
+        void bayes(const vec &dt){
+                //assume we know state exactly:
+                vec true_state=vec(x,4); // read from pmsm
+                est.set_mu(true_state);
+                
+                //integration error
+                old_true(4)=KalmanObs[4];
+                old_true(5)=KalmanObs[5];// add U
+                interr = (true_state - pfxu->eval(old_true));
+                
+                //second derivative
+                IMpmsm2o* pf = dynamic_cast<IMpmsm2o*>(pfxu);
+                if (pf) {secder=pf->eval2o(vec_2(KalmanObs[4],KalmanObs[5]));}
+                                
+                EKFCh::bayes(dt);
+                old_true.set_subvector(0,true_state);
+        }
+        
+        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");
+        }
+        void logit(logger &L){
+                L.logit(L_err, interr);
+                L.logit(L_CRB,diag(est._R().to_mat()));
+                L.logit(L_sec,secder);
+        }
+};

pmsm/pmsm_estim.cpp

@@ -38 +38 @@
 
         DS->log_add ( *L );
-        ivec L_est(nE);
+        string nic="";
         for (int i=0; i<nE; i++){
-                L_est(i)= L->add ( Es(i)->posterior()._rv(), "" ); // estimate
+                Es(i)->log_add(*L,nic); // estimate
         }
         L->init();
@@ -61 +61 @@
                         Es(i)->bayes ( Dls(i)->pushdown ( dt ) );               // update estimates
 
-                        L->logit ( L_est(i), Es(i)->posterior().mean() );
+                        Es(i)->logit (*L);
                 }
                 // Regulators

pmsm/pmsm_ui.h

@@ -29 +29 @@
                         string var=S["variant"];
                         if (var=="Stat"){tmp=new IMpmsmStat;}
-                        if (var=="Mf"){tmp=new IMpmsmStat;}
+                        if (var=="2o"){tmp=new IMpmsm2o;}
                 } else {
                         tmp= new IMpmsm;
@@ -65 +65 @@
                 //Calling function tmp->tmp_set
                 UIcall<pmsmDS*> ( S["params"], &tmp_set , tmp );
-                //
-                if ( S.exists ( "profile" ) ) {
-                        tmp->set_profile ( S["tstep"],getvec ( S["profile"] ) );
-                }
-                else {
-                        tmp->set_profile ( 1.0, vec ( "1" ) );
-                }
+                
+                // Default values of profiles for omega and Mz
+                vec profW=vec("1.0");
+                vec profM=vec("0.0");
+                double tstep=1.0;
+                
+                if ( S.exists ( "tstep" ) ) {tstep=S["tstep"];}
+                if ( S.exists ( "profileW" ) ) {profW=getvec ( S["profileW"] ) ;}
+                if ( S.exists ( "profileM" ) ) {profM=getvec ( S["profileM"] ) ;}
+                
+                tmp->set_profile (tstep , profW, profM);
+                
+                string opts="";
+                if ( S.exists ( "options" ) ) {opts=(const char*)S["options"];}
+                tmp->set_options(opts);
+                
                 return tmp;
         };
@@ -77 +86 @@
 };
 UIREGISTER ( UIpmsmDS );
+
+class UIpmsmCRB: public UIbuilder {
+        public:
+                UIpmsmCRB():UIbuilder("pmsmCRB"){};
+                bdmroot* build ( Setting &S ) const {
+                        diffbifn* IM; UIbuild(S["IM"],IM);
+                        diffbifn* OM; UIbuild(S["OM"],OM);
+                
+                //parameters
+                        pmsmCRB* E; E=new pmsmCRB;
+                        E->set_parameters(IM, OM, diag(getvec(S["dQ"])), diag(getvec(S["dR"])));
+                
+                //statistics
+                        int dim=IM->dimension();
+                        vec mu0;
+                        mat P0;
+                        if (S.exists("mu0")){mu0=getvec(S["mu0"]);}else{mu0=zeros(dim);};
+                        if (S.exists("P0")){mu0=getmat(S["P0"],dim);}else{P0=eye(dim);};
+                        E->set_statistics(mu0,P0);
+                
+                //connect
+                        RV* drv; UIbuild(S["drv"],drv);
+                        E->set_drv(*drv);
+                        RV* rv; UIbuild(S["rv"],rv);
+                        E->set_rv(*rv);
+                        return E;
+                }       
+};
+UIREGISTER ( UIpmsmCRB );

pmsm/simulator_zdenek/simulator.cpp

@@ -329 +329 @@
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////
-void pmsmsim_step(double Ww)            // you must link array KalmanObs[] to EKF modul
+void pmsmsim_step(double Ww, double Mz)            // you must link array KalmanObs[] to EKF modul
 {
   double Umk, ub, uc;
@@ -338 +338 @@
 //    *us=KalmanObs[0]; *(us+1)=KalmanObs[1];
 //     *us=ualfa; *(us+1)=ubeta;
+        //Mz
+        x[8]= Mz;
+        
     pmsm_model(5);
 

pmsm/simulator_zdenek/simulator.h

@@ -18 +18 @@
 
 extern void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0);
-extern void pmsmsim_step(double Ww);
+extern void pmsmsim_step(double Ww, double Mz=0.0);
 extern void pmsmsim_noreg_step(double ua, double ub);