Changeset 94 for pmsm

Timestamp:

05/07/08 13:59:56 (17 years ago)

Author:

smidl

Message:

prevod sim_var na logger

Location:

pmsm

Files:

• pmsm.h (modified) (1 diff)
• sim_var.cpp (modified) (6 diffs)

pmsm/pmsm.h

@@ -7 +7 @@
 
 //TODO hardcoded RVs!!!
-RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 ));
-RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 ));
-RV ry ( "7 8", "{oia, oib}", ones_i ( 2 ) ,zeros_i ( 2 ));
+RV rx ( "1 2 3 4", "{ia ib om th }", ones_i ( 4 ), zeros_i ( 4 ));
+RV ru ( "5 6", "{ua ub }", ones_i ( 2 ) ,zeros_i ( 2 ));
+RV ry ( "7 8", "{oia oib }", ones_i ( 2 ) ,zeros_i ( 2 ));
 
 class uipmsm : public uibase{

pmsm/sim_var.cpp

@@ -19 +19 @@
 #include "simulator.h"
 
-#include "iopom.h"
+#include <stat/loggers.h>
 
 using namespace itpp;
@@ -81 +81 @@
         int Nsimstep = 125;
 
-        mat Xt=zeros ( Ndat ,9 ); //true state from simulator
-        mat XtM=zeros ( Ndat ,4 ); //true state from simulator
-        mat XtF=zeros ( Ndat ,4 ); //true state from simulator
-        mat XtO=zeros ( Ndat ,4 ); //true state from simulator
-        mat Dt=zeros ( Ndat,4 ); //observation
-        mat Qrec=zeros ( Ndat, 4*4 ); //full covariance matrix
-
+        dirfilelog L("exp/sim_var",1000);
+        
         // SET SIMULATOR
         pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
@@ -96 +91 @@
         vec xdif=zeros ( 4 );
         vec xt ( 4 );
+        vec ddif=zeros(2);
         IMpmsm fxu;
         //                  Rs    Ls        dt           Fmag(Ypm) kp   p    J     Bf(Mz)
@@ -101 +97 @@
         OMpmsm hxu;
         mat Qt=zeros ( 4,4 );
+        mat Rt=zeros ( 2,2 );
 
         // ESTIMATORS
@@ -116 +113 @@
         Eop.set_parameters ( &fxu,&hxu,Q,R);
 
+        EKFfull Edi ( rx,ry,ru );
+        Edi.set_est ( mu0, 1*eye ( 4 ) );
+        Edi.set_parameters ( &fxu,&hxu,Q,R);
+
         epdf& Efix_ep = Efix._epdf();
         epdf& Eop_ep = Eop._epdf();
+        epdf& Edi_ep = Edi._epdf();
 
+        //LOG
+        RV rQ("10", "{Q }", "16","0");
+        RV rR("11", "{R }", "4","0");
+        int X_log = L.add(rx,"X");
+        int Efix_log = L.add(rx,"XF");
+        int Eop_log = L.add(rx,"XO");
+        int Edi_log = L.add(rx,"XD");
+        int Q_log = L.add(rQ,"");
+        int R_log = L.add(rR,"");
+        L.init();
 
         for ( int tK=1;tK<Ndat;tK++ ) {
@@ -133 +145 @@
 
                 xt = fxu.eval ( xtm,ut );
-
                 //Results:
                 // in xt we have simulaton according to the model
                 // in x we have "reality"
-
                 xtm ( 0 ) =x[0];xtm ( 1 ) =x[1];xtm ( 2 ) =x[2];xtm ( 3 ) =x[3];
-                xdif=xtm-xt;
+                xdif = xtm-xt;
                 if ( xdif ( 3 ) >pi ) xdif ( 3 )-=2*pi;
                 if ( xdif ( 3 ) <-pi ) xdif ( 3 ) +=2*pi;
+                
+                ddif = hxu.eval(xtm,ut) - dt;
 
                 //Rt = 0.9*Rt + xdif^2
-                Qt*=0.0;
+                Qt*=0.01;
                 Qt += outer_product ( xdif,xdif ); //(x-xt)^2
-                Qrec.set_row ( tK, vec ( Qt._data(),16 ) );
+                Rt*=0.01;
+                Rt += outer_product ( ddif,ddif ); //(x-xt)^2
 
                 //ESTIMATE
                 Efix.bayes(concat(dt,ut));
                 //
-                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-16*eye(4)),(1*eye(2)));
+                Eop.set_parameters ( &fxu,&hxu,(Qt+1e-16*eye(4)),(Rt+1e-3*eye(2)));
                 Eop.bayes(concat(dt,ut));
+                //
+                Edi.set_parameters ( &fxu,&hxu,(diag(diag(Qt))+1e-16*eye(4)),(diag(diag(Rt))+1e-3*eye(2)));
+                Edi.bayes(concat(dt,ut));
                 
-                Xt.set_row ( tK,vec ( x,9 ) ); //vec from C-array
-                XtM.set_row ( tK,xt ); //vec from C-array
-                XtF.set_row ( tK,Efix_ep.mean() ); 
-                XtO.set_row ( tK,Eop_ep.mean() ); 
-                Dt.set_row ( tK, concat ( dt,ut ) );
+                //LOG
+                L.logit(X_log, vec(x,4)); //vec from C-array
+                L.logit(Efix_log, Efix_ep.mean() ); 
+                L.logit(Eop_log, Eop_ep.mean() ); 
+                L.logit(Edi_log, Edi_ep.mean() ); 
+                L.logit(Q_log, vec(Qt._data(),16) ); 
+                L.logit(R_log, vec(Rt._data(),4) ); 
+                
+                L.step(false);
         }
 
-        char tmpstr[200];
-        sprintf ( tmpstr,"%s/%s","variance/","format" );
-        ofstream  form ( tmpstr );
-        form << "# Experimetal header file"<< endl;
-        dirfile_write ( form,"variance/",Xt,"X","{isa isb om th }" );
-        dirfile_write ( form,"variance/",XtM,"Xsim","{isa isb om th }" );
-        dirfile_write ( form,"variance/",XtO,"XO","{isa isb om th }" );
-        dirfile_write ( form,"variance/",XtF,"XF","{isa isb om th }" );
-        dirfile_write ( form,"variance/",Dt,"D","{isa isb usa usb }" );
-        dirfile_write ( form,"variance",Qrec,"Q","{ }" );
+        L.step(true);
 
         return 0;