Changeset 230

Timestamp:

01/15/09 10:53:55 (15 years ago)

Author:

smidl

Message:

mpf_delta - estimation of covariance weight

Files:

• bdm/math/chmat.h (modified) (1 diff)
• bdm/stat/loggers.cpp (modified) (1 diff)
• matlab/pmsm/mpf_u_delta_disp.m (modified) (2 diffs)
• pmsm/mpf_test.cpp (modified) (6 diffs)
• pmsm/mpf_u_delta.cpp (modified) (5 diffs)

bdm/math/chmat.h

@@ -75 +75 @@
         chmat& operator += ( const chmat &A2 );
         chmat& operator -= ( const chmat &A2 ) ;
+        chmat& operator * ( const chmat &A2 );
+        chmat& operator * ( const double &d ){Ch*d; return *this;};
+        chmat& operator = ( const chmat &A2 ){Ch=A2.Ch;dim=A2.dim;return *this;}
 };
 

bdm/stat/loggers.cpp

@@ -44 +44 @@
                 for ( j=0;j<entries ( i ).length();j++ ) { //for RVs in entries
                         int rvsize = entries ( i ).size ( j );
-                        if ( rvsize ==1 ) {
-                                scalarnames ( ii ) = names ( i ) + "_" + entries ( i ).name ( j );
-                                ii++;
-                        }
-                        else {
+                        // for non-empty names
+                        if (names(i).length()>0) {scalarnames(ii)=names(i) + "_";}
+                        // add name
+                        scalarnames ( ii ) +=  entries ( i ).name ( j );
+                        // add number when needed
+                        if ( rvsize >1 ) {
+                                // copy the same name for th whole RV
+                                for ( k=1;k<rvsize;k++ ) {scalarnames(ii+k)=scalarnames(ii);}
+                                // add numbers
                                 for ( k=0;k<rvsize;k++ ) { //for all scalars in given RV
                                         sprintf ( num,"%d",k );
-                                        scalarnames ( ii ) = names ( i ) + "_" + entries ( i ).name ( j ) + "_" + num;
+                                        scalarnames ( ii ) += (std::string)"_" + num;
                                         ii++;
                                 }
-                        }
+                        } else {
+                        ii++;}
                 }
         }

matlab/pmsm/mpf_u_delta_disp.m

@@ -15 +15 @@
         hold on;
         plot(xthE(:,i)','r')
-        plot(xthM(:,i+2)','g')
+        plot(xthM(:,i+3)','g')
 %       plot(xthV(i,:)','m');
     grid on
 end
 
+figure(7); plot(abs(xthM(:,3)));
 
 figure(3)
@@ -55 +56 @@
 
 hold on;
-di = xthM(:,1+4)'-xth(:,1+2)';
+di = xthM(:,3+3)'-xth(:,1+2)';
 plot(tm,di,'-')
 MSE_M=di*di'/9000

pmsm/mpf_test.cpp

@@ -18 +18 @@
 #include <stat/libFN.h>
 
+#include <stat/loggers.h>
+
 #include "pmsm.h"
 #include "simulator.h"
@@ -29 +31 @@
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 20;
+        int Npart = 200;
 
         // internal model
@@ -56 +58 @@
         MPF<EKFCh_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp );
         // initialize
-        evolQ.set_parameters ( 0.1, Qdiag, 1.0); //sigma = 1/10 mu
-        evolQ.condition (Qdiag ); //Zdenek default
-        double xxx;
-        cout << Qdiag <<endl << "smp:"<< evolQ.samplecond(Qdiag,xxx) <<endl;
-        eigamma* pfinit=dynamic_cast<eigamma*>(evolQ._e());
-        cout << pfinit->mean()<<endl;
-        M.set_est ( *pfinit );
-        evolQ.set_parameters ( 0.10, Qdiag,0.995); //sigma = 1/10 mu
-        cout << Qdiag <<endl << "smp:"<< evolQ.samplecond(Qdiag,xxx) <<endl;
-        
+        evolQ.set_parameters ( 0.1, 10*Qdiag, 1.0); //sigma = 1/10 mu
+        evolQ.condition (10*Qdiag ); //Zdenek default
+        M.set_est ( *evolQ._e() );
+        evolQ.set_parameters ( 0.10, 10*Qdiag,0.9999); //sigma = 1/10 mu
         //
 
@@ -71 +67 @@
         const epdf& Mep = M._epdf();
 
-        mat Xt=zeros ( Ndat ,4 ); //true state from simulator
-        mat Dt=zeros ( Ndat,2+2 ); //observation
-        mat XtE=zeros ( Ndat, 4 );
-        mat VarE=zeros ( Ndat, 4 );
-        mat Qtr=zeros ( Ndat, 4 );
-        mat XtM=zeros ( Ndat,4+4 ); //Q + x
-        mat VarM=zeros ( Ndat,4+4 ); //Q + x
-
+        dirfilelog L("exp/mpf_test",100);
+        int l_X = L.add(rx, "xt");
+        int l_D = L.add(concat(ry,ru), "");
+        int l_XE= L.add(rx, "xtE");
+        int l_XM= L.add(concat(rQ,rx), "xtM");
+        int l_VE= L.add(rx, "VE");
+        int l_VM= L.add(concat(rQ,rx), "VM");
+        int l_Q= L.add(rQ, "");
+        L.init();
+        
         // SET SIMULATOR
         pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h );
@@ -101 +99 @@
                 xtm = xt;
                 
-                
                 //Variances 
                 if (tK==1000)  Qdiag(0)*=10; 
@@ -116 +113 @@
                 M.bayes ( concat ( dt,ut ) );
 
-                Xt.set_row ( tK, xt); //vec from C-array
-                Dt.set_row ( tK, concat ( dt,ut));
-                Qtr.set_row ( tK, Qdiag);
-                XtE.set_row ( tK,KFEep.mean() );
-                VarE.set_row ( tK,KFEep.variance() );
-                XtM.set_row ( tK,Mep.mean() );
-                VarM.set_row ( tK,Mep.variance() );
+                L.logit(l_X,xt);
+                L.logit(l_D,concat(dt,ut));
+                L.logit(l_XE,KFEep.mean());
+                L.logit(l_XM,Mep.mean());
+                L.logit(l_VE,KFEep.variance());
+                L.logit(l_VM,Mep.variance());
+                L.logit(l_Q,Qdiag);
+                L.step();
         }
-
-        it_file fou ( "mpf_test.it" );
-
-        fou << Name ( "xth" ) << Xt;
-        fou << Name ( "Dt" ) << Dt;
-        fou << Name ( "Qtr" ) << Qtr;
-        fou << Name ( "xthE" ) << XtE;
-        fou << Name ( "xthM" ) << XtM;
-        fou << Name ( "VarE" ) << VarE;
-        fou << Name ( "VarM" ) << VarM;
+        L.finalize();
         //Exit program:
 

pmsm/mpf_u_delta.cpp

@@ -25 +25 @@
 using namespace itpp;
 
-//!Extended Kalman filter with unknown \c Q
-class EKFCh_cond : public EKFCh , public BMcond {
+//!Extended Kalman filter with unknown \c Q and delta u
+class EKFCh_du_kQ : public EKFCh , public BMcond {
+        chmat Qref;
 public:
         //! Default constructor
-        EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {};
+        EKFCh_du_kQ ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ),Qref(rx.count()) {};
+        void set_ref(const chmat &Qref0){Qref=Qref0;}
         void condition ( const vec &val ) {
-                pfxu->condition ( val );
+                pfxu->condition ( val.left(2) );
+                
+                Q = Qref*std::abs(val(2));
+                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() );
         };
 };
@@ -74 +79 @@
         double h = 1e-6;
         int Nsimstep = 125;
-        int Npart = 200;
+        int Npart = 20;
 
         mat Rnoise = randn ( 2,Ndat );
@@ -89 +94 @@
         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 Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034"
+        vec Rdiag ( "0.2 0.2" ); //var(diff(xth)) = "0.034 0.034"
         chmat Q ( Qdiag );
         chmat R ( Rdiag );
@@ -96 +101 @@
         KFE.set_est ( mu0, chmat ( ones ( 4 ) ) );
 
-        RV rUd ( "{ud }", "2" );
-        EKFCh_cond KFEp ( rx,ry,ru,rUd );
+        RV rUd ( "{ud k}", "2 1" );
+        EKFCh_du_kQ KFEp ( rx,ry,ru,rUd );
         KFEp.set_parameters ( &fxu,&hxu,Q,R );
+        KFEp.set_ref(Q);
         KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) );
 
         mlnorm<ldmat> evolUd ( rUd,rUd );
-        MPF<EKFCh_cond> M ( rx,rUd,evolUd,evolUd,Npart,KFEp );
+        MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp );
         // initialize
-        vec Ud0="0 0";
-        evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 10.0*eye ( 2 ) ) );
+        vec Ud0="0 0 1.0";
+        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 0.0001" )));
         evolUd.condition ( Ud0 );
         epdf& pfinit=evolUd._epdf();
         M.set_est ( pfinit );
-        evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 0.1*eye ( 2 ) ) );
+        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 4e-3 4e-3 1e-3" )));
 
         mat Xt=zeros ( Ndat ,4 ); //true state from simulator
@@ -115 +121 @@
         mat XtE=zeros ( Ndat, 4 );
         mat Qtr=zeros ( Ndat, 4 );
-        mat XtM=zeros ( Ndat,2+4 ); //W + x
+        mat XtM=zeros ( Ndat, 3+4 ); //W + x
         mat XtMTh=zeros ( Ndat,1 );