Changeset 230 for pmsm

Show
Ignore:
Timestamp:
01/15/09 10:53:55 (16 years ago)
Author:
smidl
Message:

mpf_delta - estimation of covariance weight

Location:
pmsm
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • pmsm/mpf_test.cpp

    r229 r230  
    1818#include <stat/libFN.h> 
    1919 
     20#include <stat/loggers.h> 
     21 
    2022#include "pmsm.h" 
    2123#include "simulator.h" 
    … …  
    2931        double h = 1e-6; 
    3032        int Nsimstep = 125; 
    31         int Npart = 20; 
     33        int Npart = 200; 
    3234 
    3335        // internal model 
    … …  
    5658        MPF<EKFCh_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp ); 
    5759        // initialize 
    58         evolQ.set_parameters ( 0.1, Qdiag, 1.0); //sigma = 1/10 mu 
    59         evolQ.condition (Qdiag ); //Zdenek default 
    60         double xxx; 
    61         cout << Qdiag <<endl << "smp:"<< evolQ.samplecond(Qdiag,xxx) <<endl; 
    62         eigamma* pfinit=dynamic_cast<eigamma*>(evolQ._e()); 
    63         cout << pfinit->mean()<<endl; 
    64         M.set_est ( *pfinit ); 
    65         evolQ.set_parameters ( 0.10, Qdiag,0.995); //sigma = 1/10 mu 
    66         cout << Qdiag <<endl << "smp:"<< evolQ.samplecond(Qdiag,xxx) <<endl; 
    67          
     60        evolQ.set_parameters ( 0.1, 10*Qdiag, 1.0); //sigma = 1/10 mu 
     61        evolQ.condition (10*Qdiag ); //Zdenek default 
     62        M.set_est ( *evolQ._e() ); 
     63        evolQ.set_parameters ( 0.10, 10*Qdiag,0.9999); //sigma = 1/10 mu 
    6864        // 
    6965 
    … …  
    7167        const epdf& Mep = M._epdf(); 
    7268 
    73         mat Xt=zeros ( Ndat ,4 ); //true state from simulator 
    74         mat Dt=zeros ( Ndat,2+2 ); //observation 
    75         mat XtE=zeros ( Ndat, 4 ); 
    76         mat VarE=zeros ( Ndat, 4 ); 
    77         mat Qtr=zeros ( Ndat, 4 ); 
    78         mat XtM=zeros ( Ndat,4+4 ); //Q + x 
    79         mat VarM=zeros ( Ndat,4+4 ); //Q + x 
    80  
     69        dirfilelog L("exp/mpf_test",100); 
     70        int l_X = L.add(rx, "xt"); 
     71        int l_D = L.add(concat(ry,ru), ""); 
     72        int l_XE= L.add(rx, "xtE"); 
     73        int l_XM= L.add(concat(rQ,rx), "xtM"); 
     74        int l_VE= L.add(rx, "VE"); 
     75        int l_VM= L.add(concat(rQ,rx), "VM"); 
     76        int l_Q= L.add(rQ, ""); 
     77        L.init(); 
     78         
    8179        // SET SIMULATOR 
    8280        pmsmsim_set_parameters ( 0.28,0.003465,0.1989,0.0,4,1.5,0.04, 200., 3e-6, h ); 
    … …  
    10199                xtm = xt; 
    102100                 
    103                  
    104101                //Variances  
    105102                if (tK==1000)  Qdiag(0)*=10;  
    … …  
    116113                M.bayes ( concat ( dt,ut ) ); 
    117114 
    118                 Xt.set_row ( tK, xt); //vec from C-array 
    119                 Dt.set_row ( tK, concat ( dt,ut)); 
    120                 Qtr.set_row ( tK, Qdiag); 
    121                 XtE.set_row ( tK,KFEep.mean() ); 
    122                 VarE.set_row ( tK,KFEep.variance() ); 
    123                 XtM.set_row ( tK,Mep.mean() ); 
    124                 VarM.set_row ( tK,Mep.variance() ); 
     115                L.logit(l_X,xt); 
     116                L.logit(l_D,concat(dt,ut)); 
     117                L.logit(l_XE,KFEep.mean()); 
     118                L.logit(l_XM,Mep.mean()); 
     119                L.logit(l_VE,KFEep.variance()); 
     120                L.logit(l_VM,Mep.variance()); 
     121                L.logit(l_Q,Qdiag); 
     122                L.step(); 
    125123        } 
    126  
    127         it_file fou ( "mpf_test.it" ); 
    128  
    129         fou << Name ( "xth" ) << Xt; 
    130         fou << Name ( "Dt" ) << Dt; 
    131         fou << Name ( "Qtr" ) << Qtr; 
    132         fou << Name ( "xthE" ) << XtE; 
    133         fou << Name ( "xthM" ) << XtM; 
    134         fou << Name ( "VarE" ) << VarE; 
    135         fou << Name ( "VarM" ) << VarM; 
     124        L.finalize(); 
    136125        //Exit program: 
    137126 

  • pmsm/mpf_u_delta.cpp

    r226 r230  
    2525using namespace itpp; 
    2626 
    27 //!Extended Kalman filter with unknown \c Q 
    28 class EKFCh_cond : public EKFCh , public BMcond { 
     27//!Extended Kalman filter with unknown \c Q and delta u 
     28class EKFCh_du_kQ : public EKFCh , public BMcond { 
     29        chmat Qref; 
    2930public: 
    3031        //! Default constructor 
    31         EKFCh_cond ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ) {}; 
     32        EKFCh_du_kQ ( RV rx, RV ry,RV ru,RV rC ) :EKFCh ( rx,ry,ru ),BMcond ( rC ),Qref(rx.count()) {}; 
     33        void set_ref(const chmat &Qref0){Qref=Qref0;} 
    3234        void condition ( const vec &val ) { 
    33                 pfxu->condition ( val ); 
     35                pfxu->condition ( val.left(2) ); 
     36                 
     37                Q = Qref*std::abs(val(2)); 
     38                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); 
    3439        }; 
    3540}; 
    … …  
    7479        double h = 1e-6; 
    7580        int Nsimstep = 125; 
    76         int Npart = 200; 
     81        int Npart = 20; 
    7782 
    7883        mat Rnoise = randn ( 2,Ndat ); 
    … …  
    8994        vec mu0= "0.0 0.0 0.0 0.0"; 
    9095        vec Qdiag ( "0.4 0.4 0.001 0.000001" ); //zdenek: 0.01 0.01 0.0001 0.0001 
    91         vec Rdiag ( "0.1 0.1" ); //var(diff(xth)) = "0.034 0.034" 
     96        vec Rdiag ( "0.2 0.2" ); //var(diff(xth)) = "0.034 0.034" 
    9297        chmat Q ( Qdiag ); 
    9398        chmat R ( Rdiag ); 
    … …  
    96101        KFE.set_est ( mu0, chmat ( ones ( 4 ) ) ); 
    97102 
    98         RV rUd ( "{ud }", "2" ); 
    99         EKFCh_cond KFEp ( rx,ry,ru,rUd ); 
     103        RV rUd ( "{ud k}", "2 1" ); 
     104        EKFCh_du_kQ KFEp ( rx,ry,ru,rUd ); 
    100105        KFEp.set_parameters ( &fxu,&hxu,Q,R ); 
     106        KFEp.set_ref(Q); 
    101107        KFEp.set_est ( mu0, chmat ( ones ( 4 ) ) ); 
    102108 
    103109        mlnorm<ldmat> evolUd ( rUd,rUd ); 
    104         MPF<EKFCh_cond> M ( rx,rUd,evolUd,evolUd,Npart,KFEp ); 
     110        MPF<EKFCh_du_kQ> M ( rx,rUd,evolUd,evolUd,Npart,KFEp ); 
    105111        // initialize 
    106         vec Ud0="0 0"; 
    107         evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 10.0*eye ( 2 ) ) ); 
     112        vec Ud0="0 0 1.0"; 
     113        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec( "1 1 0.0001" ))); 
    108114        evolUd.condition ( Ud0 ); 
    109115        epdf& pfinit=evolUd._epdf(); 
    110116        M.set_est ( pfinit ); 
    111         evolUd.set_parameters ( eye ( 2 ), vec_2 ( 0.0,0.0 ), ldmat ( 0.1*eye ( 2 ) ) ); 
     117        evolUd.set_parameters ( eye ( 3 ), zeros(3), ldmat ( vec(" 4e-3 4e-3 1e-3" ))); 
    112118 
    113119        mat Xt=zeros ( Ndat ,4 ); //true state from simulator 
    … …  
    115121        mat XtE=zeros ( Ndat, 4 ); 
    116122        mat Qtr=zeros ( Ndat, 4 ); 
    117         mat XtM=zeros ( Ndat,2+4 ); //W + x 
     123        mat XtM=zeros ( Ndat, 3+4 ); //W + x 
    118124        mat XtMTh=zeros ( Ndat,1 ); 
    119125