Changeset 240

Timestamp:

01/22/09 19:43:11 (15 years ago)

Author:

smidl

Message:

mpf_u_delta with Q proportional to x2o²

Location:

pmsm

Files:

• mpf_u_delta.cpp (modified) (5 diffs)
• pmsm.h (modified) (5 diffs)
• sim_var.cpp (modified) (4 diffs)
• simulator_zdenek/simulator.cpp (modified) (1 diff)

pmsm/mpf_u_delta.cpp

@@ -63 +63 @@
                 xk ( 1 ) = ( 1.0- Rs/Ls*dt ) * ibm - Ypm/Ls*dt*omm * cos ( thm ) + ( ubm+ud ( 1 ) ) *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 ) = thm + omm*dt; // <0..2pi>
@@ -93 +93 @@
 
         vec mu0= "0.0 0.0 0.0 0.0";
-        vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
+        vec Qdiag ( "0.4 0.4 0.1 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001
         vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
         mat Q2o=diag(Qdiag);
         chmat R ( Rdiag );
-        EKFCh KFE ( rx,ry,rU );
+        EKFCh KFE ( rx,ry,ru );
         KFE.set_parameters ( &fxu0,&hxu,Q,R );
         KFE.set_est ( mu0, chmat ( ones ( 4 ) ) );
@@ -111 +111 @@
         MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp );
         // initialize
-        vec Ud0="0 0 100.0";
-        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 10" )));
+        vec Ud0="0 0 1.0";
+        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1e-4 1e-4 10e-4" )));
         evolUd.condition ( Ud0 );
         epdf& pfinit=evolUd._epdf();
         M.set_est ( pfinit );
-        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 9e-8 9e-8 9e-7" )));
+        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 5e-4 5e-4 1e-8" )));
 
         mat Xt=zeros ( Ndat ,4 ); //true state from simulator
@@ -128 +128 @@
         pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
         vec dt ( 2 );
-        vec ut ( 4 );
-        vec utm ( 4 );
+        vec ut ( 2 );
+        vec utm ( 2 );
         vec xt ( 4 );
         vec xtm=zeros ( 4 );
@@ -145 +145 @@
                 ut ( 0 ) = KalmanObs[0];
                 ut ( 1 ) = KalmanObs[1];
-                ut ( 2 ) = utm(0)-ut(0);
-                ut ( 3 ) = utm(1)-ut(1);
                 dt ( 0 ) = KalmanObs[2]+0.3*Rnoise ( 0,t );
                 dt ( 1 ) = KalmanObs[3]+0.3*Rnoise ( 1,t );
                 xt = vec ( x,4 );
 
+                vec x2o = fxu0.eval2o(utm-ut);
                 utm =ut;
                 
-                Q2o*=0.9;
-                Q2o+=outer_product(x2o_dbg,x2o_dbg);
+                Q2o*=0.99;
+                Q2o+=outer_product(x2o,x2o);
                 //estimator
                 KFE.bayes ( concat ( dt,ut ) );
-                for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(Q2o));}
+                for (int ii=0; ii<Npart; ii++){dynamic_cast<EKFCh_du_kQ*>(M._BM(ii))->set_ref(chmat(elem_mult(Qdiag,Qdiag)));}
                 M.bayes ( concat ( dt,ut ) );
 

pmsm/pmsm.h

@@ -12 +12 @@
 RV ru ( "{ua ub }");
 RV ry ( "{oia oib }");
-RV rU = concat(ru, RV("{dua dub }"));
 
 // class uipmsm : public uibase{
 //      double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
 // };
-
-vec x2o_dbg(4);
 
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$
@@ -81 +78 @@
         protected:
                 double Rs, Ls, dt, Ypm, kp, p,  J, Mz;
-
+                //! store first derivatives for the use in second derivatives
+                double dia, dib, dom, dth;
+                //! d2t = dt^2/2, cth = cos(th), sth=sin(th)
+                double d2t, cth, sth;
+                double iam, ibm, omm, thm, uam, ubm;
         public:
-                IMpmsm2o() :diffbifn (rx.count(), rx, rU ) {};
+                IMpmsm2o() :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;}
+                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;}
 
                 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 );
-                        double dua = u0 ( 2 )/dt;
-                        double dub = u0 ( 3 )/dt;
-
-                        double cth = cos(thm);
-                        double sth = sin(thm);
-                        double d2t = dt*dt/2;
+                        iam = x0 ( 0 );
+                        ibm = x0 ( 1 );
+                        omm = x0 ( 2 );
+                        thm = x0 ( 3 );
+                        uam = u0 ( 0 );
+                        ubm = u0 ( 1 );
+
+                        cth = cos(thm);
+                        sth = sin(thm);
                         
-                        double dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
-                        double dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
-                        double dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
-                        double dth = omm;
-                        
-                        double d2ia = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
-                        double d2ib = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
-                        double d2om = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
-                        double d2th = dom;
-                        
+                        dia = (- Rs/Ls*iam +  Ypm/Ls*omm * sth + uam/Ls);
+                        dib = (- Rs/Ls*ibm -  Ypm/Ls*omm * cth + ubm/Ls);
+                        dom = kp*p*p * Ypm/J *( ibm * cth-iam * sth ) - p/J*Mz;
+                        dth = omm;
+                                                
                         vec xk=zeros ( 4 );
                         xk ( 0 ) =  iam + dt*dia;// +d2t*d2ia;
@@ -118 +111 @@
                         xk ( 3 ) = thm + dt*dth;// +d2t*d2th; // <0..2pi>
                         
-                        x2o_dbg(0)=d2t*d2ia;
-                        x2o_dbg(1)=d2t*d2ib;
-                        x2o_dbg(2)=d2t*d2om;
-                        x2o_dbg(3)=d2t*d2th;
-                        
                         if ( xk ( 3 ) >pi ) xk ( 3 )-=2*pi;
                         if ( xk ( 3 ) <-pi ) xk ( 3 ) +=2*pi;
@@ -128 +116 @@
                 }
 
+                //! eval 2nd order Taylor expansion, MUST be used only as a follow up AFTER eval()!!
+                vec eval2o(const vec &du){
+                        double dua = du ( 0 )/dt;
+                        double dub = du ( 1 )/dt;
+                        
+                        vec xth2o(4);
+                        xth2o(0) = (- Rs/Ls*dia +  Ypm/Ls*(dom * sth + omm*cth) + dua/Ls);
+                        xth2o(1) = (- Rs/Ls*dib -  Ypm/Ls*(dom * cth - omm*sth) + dub/Ls);
+                        xth2o(2) = kp*p*p * Ypm/J *( dib * cth-ibm*sth - (dia * sth + iam *cth));
+                        xth2o(3) = dom;
+                        return xth2o;
+                }
                 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 );
+                         iam = x0 ( 0 );
+                         ibm = x0 ( 1 );
+                         omm = x0 ( 2 );
+                         thm = x0 ( 3 );
                 // d ia
                         A ( 0,0 ) = ( 1.0- Rs/Ls*dt ); A ( 0,1 ) = 0.0;
@@ -154 +154 @@
 //! State evolution model for a PMSM drive and its derivative with respect to \f$x\f$, equation for \f$\omega\f$ is omitted.$
 class IMpmsmStat : public IMpmsm {
+        public:
         IMpmsmStat() :IMpmsm() {};
         //! Set mechanical and electrical variables

pmsm/sim_var.cpp

@@ -40 +40 @@
         double Ww = 0.0;
         vec dt ( 2 );
-        vec ut ( 4 );
-        vec utm ( 4 );
+        vec ut ( 2 );
+        vec utm ( 2 );
         vec dut ( 2 );
         vec dit (2);
+        vec x2o(2);
         vec xtm=zeros ( 4 );
         vec xte=zeros ( 4 );
@@ -62 +63 @@
         mat Q =diag( Qdiag );
         mat R =diag ( Rdiag );
-        EKFfull Efix ( rx,ry,rU );
+        EKFfull Efix ( rx,ry,ru );
         Efix.set_est ( mu0, 1*eye ( 4 )  );
         Efix.set_parameters ( &fxu,&hxu,Q,R);
 
-        EKFfull Eop ( rx,ry,rU );
+        EKFfull Eop ( rx,ry,ru );
         Eop.set_est ( mu0, 1*eye ( 4 ) );
         Eop.set_parameters ( &fxu,&hxu,Q,R);
 
-        EKFfull Edi ( rx,ry,rU );
+        EKFfull Edi ( rx,ry,ru );
         Edi.set_est ( mu0, 1*eye ( 4 ) );
         Edi.set_parameters ( &fxu,&hxu,Q,R);
@@ -106 +107 @@
                 ut ( 0 ) = KalmanObs[4];
                 ut ( 1 ) = KalmanObs[5];
-                ut ( 2 ) = utm(0)-ut(0);
-                ut ( 3 ) = utm(1)-ut(1);
                 
+                x2o = fxu.eval2o(utm-ut);
                 utm = ut;
                 
@@ -156 +156 @@
                 //LOG
                 L.logit(X_log,  vec(x,4)); //vec from C-array
-                L.logit(X2o_log, x2o_dbg); 
+                L.logit(X2o_log, x2o); 
                 L.logit(Xdf_log, xdif); 
                 L.logit(Efix_log, Efix_ep.mean() ); 

pmsm/simulator_zdenek/simulator.cpp

@@ -394 +394 @@
 
   // uprava velikosti vzhledem k Uc!=Ucn
-/*  kor_Uc=Ucn/230.;
+  kor_Uc=Ucn/230.;
   *u*=kor_Uc; /**/
 
   pwm(modulace);
 
-  *us=*u*cos(*(u+1));
-  *(us+1)=*u*sin(*(u+1));;
+//  *us=*u*cos(*(u+1));
+//  *(us+1)=*u*sin(*(u+1));;
 
   pmsm_model(5);