Changeset 48 for tests

Show
Ignore:
Timestamp:
03/20/08 21:41:51 (17 years ago)
Author:
smidl
Message:

meziverze...

Location:
tests
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • tests/pmsm.h

    r42 r48  
    22#define PMSM_H 
    33 
     4//TODO hardcoded RVs!!! 
    45RV rx ( "1 2 3 4", "{ia, ib, om, th}", ones_i ( 4 ), zeros_i ( 4 )); 
    56RV ru ( "5 6", "{ua, ub}", ones_i ( 2 ) ,zeros_i ( 2 )); 
    … …  
    1011        double Rs, Ls, dt, Ypm, kp, p,  J, Mz; 
    1112 
    12 //TODO hardcoded RVs!!! 
    1313public: 
    1414        IMpmsm() :diffbifn ( rx, ru ) {}; 
    … …  
    3333                xk ( 2 ) = omm + kp*p*p * Ypm/J*dt* ( ibm * cos ( thm )-iam * sin ( thm ) ) - p/J*dt*Mz; 
    3434                //th 
    35                 xk ( 3 ) = rem(thm + omm*dt,pi); //or 2*pi? 
     35                xk ( 3 ) = rem(thm + omm*dt,2*pi); // <0..2pi> 
    3636                return xk; 
    3737        } 

  • tests/pmsm_sim.cpp

    r46 r48  
    2020 
    2121using namespace itpp; 
    22 /* 
    23 // PMSM with Q on Ia and Ib given externally 
    24 class EKF_unQ : public EKF<chmat> , public BMcond { 
     22//!Extended Kalman filter with unknown \c Q 
     23class EKF_unQ : public EKFCh , public BMcond { 
    2524public: 
    26         EKF_unQ( rx,ry,ru,rQ):EKF<chmat>(rx,ry,ru),BMcond(rQ){}; 
    27         void condition(const vec &Q0){}; 
    28 };*/ 
     25        //! Default constructor 
     26        EKF_unQ ( RV rx, RV ry,RV ru,RV rQ ) :EKFCh ( rx,ry,ru ),BMcond ( rQ ) {}; 
     27        void condition ( const vec &Q0 ) { 
     28                Q.setD ( Q0,0 ); 
     29                //from EKF 
     30                preA.set_submatrix ( dimy+dimx,dimy,Q._Ch() ); 
     31        };  
     32}; 
    2933 
    3034 
    … …  
    3337        int Ndat = 10000; 
    3438        double h = 1e-6; 
    35         int Nsimstep = 20; 
     39        int Nsimstep = 125; 
     40        int Npart = 100; 
    3641         
    37         const int Nll = 10; 
    38  
    39 //      cout << KF; 
    4042        // internal model 
    4143        IMpmsm fxu; 
    … …  
    4648 
    4749        vec mu0= "0.0 0.0 0.0 0.0"; 
    48         vec Qdiag ( "0.03 0.03 0.001 0.00001" ); 
    49         vec Rdiag ( "0.000001 0.000001" ); //var(diff(xth)) = "0.034 0.034" 
    50         vec vQ = "0.01:0.01:0.1"; 
     50        vec Qdiag ( "0.05 0.05 0.001 0.001" ); //zdenek: 0.05 0.05 0.001 0.001 
     51        vec Rdiag ( "0.03 0.03" ); //var(diff(xth)) = "0.034 0.034" 
    5152        chmat Q ( Qdiag ); 
    5253        chmat R ( Rdiag ); 
    5354        EKFCh KFE ( rx,ry,ru ); 
     55        KFE.set_parameters ( &fxu,&hxu,Q,R ); 
    5456        KFE.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); 
    55         KFE.set_parameters ( &fxu,&hxu,Q,R ); 
    5657 
    57         mat ll ( Nll,Ndat ); 
     58        RV rQ ( "100","{Q}","4","0" ); 
     59        EKF_unQ KFEp ( rx,ry,ru,rQ ); 
     60        KFEp.set_parameters ( &fxu,&hxu,Q,R ); 
     61        KFEp.set_est ( mu0, chmat ( 1000*ones ( 4 ) ) ); 
    5862 
    59         EKFCh* kfArray[Nll]; 
     63        mgamma evolQ ( rQ,rQ ); 
     64        MPF<EKF_unQ> M ( rx,rQ,evolQ,evolQ,Npart,KFEp ); 
     65        // initialize 
     66        evolQ.set_parameters ( 100.0 ); //sigma = 1/10 mu 
     67        evolQ.condition ( "0.05 0.05 0.001 0.001" ); //Zdenek default 
     68        epdf& pfinit=evolQ._epdf(); 
     69        M.set_est ( pfinit ); 
     70        evolQ.set_parameters ( 1000.0 );  
    6071 
    61  
    62         for ( int i=0;i<Nll;i++ ) { 
    63                 vec Qid ( Qdiag ); 
    64                 Qid ( 0 ) = vQ ( i ); Qid ( 1 ) = vQ ( i ); 
    65                 kfArray[i]= new EKFCh ( rx,ry,ru ); 
    66                 kfArray[i]->set_est ( mu0, chmat ( 100*ones ( 4 ) ) ); 
    67                 kfArray[i]->set_parameters ( &fxu,&hxu,chmat ( Qid ),R ); 
    68         } 
     72        // 
    6973 
    7074        epdf& KFEep = KFE._epdf(); 
     75        epdf& Mep = M._epdf(); 
    7176 
    7277        mat Xt=zeros ( 9,Ndat ); //true state from simulator 
    7378        mat Dt=zeros ( 4,Ndat ); //observation 
    7479        mat XtE=zeros ( 4,Ndat ); 
     80        mat XtM=zeros ( 8,Ndat ); //Q + x 
    7581 
    7682        // SET SIMULATOR 
    … …  
    8086        static long k_rampa_tmp=0; 
    8187        vec dt ( 2 ); 
     88        vec dtVS =zeros( 2 ); 
     89        vec xtVS =zeros(4); 
     90        vec et ( 4 ); 
     91        vec wt ( 2 ); 
    8292        vec ut ( 2 ); 
     93        mat XtV=zeros ( 4,Ndat ); 
    8394 
    8495        for ( int tK=1;tK<Ndat;tK++ ) { 
    … …  
    101112                dt ( 0 ) = KalmanObs[2]; 
    102113                dt ( 1 ) = KalmanObs[3]; 
     114                 
     115                // My own simulator for testing : Asuming ut is OK 
     116                NorRNG.sample_vector ( 4,et ); 
     117                NorRNG.sample_vector ( 2,wt ); 
     118                xtVS = fxu.eval ( xtVS,ut ) + Q.sqrt_mult ( et ); 
     119                dtVS = hxu.eval ( xtVS,ut ) + R.sqrt_mult ( wt ); 
     120 
    103121                //estimator 
    104122                KFE.bayes ( concat ( dt,ut ) ); 
    105                 for ( int i=0;i<Nll;i++ ) { 
    106                         kfArray[i]->bayes ( concat ( dt,ut ) ); 
    107                         ll ( i,tK ) =ll ( i,tK-1 ) + kfArray[i]->_ll(); 
    108                 } 
     123                M.bayes ( concat ( dt,ut ) ); 
    109124 
    110                 Xt.set_col ( tK,vec ( x,9 ) ); 
     125                Xt.set_col ( tK,vec ( x,9 ) ); //vec from C-array 
    111126                Dt.set_col ( tK, concat ( dt,ut ) ); 
    112127                XtE.set_col ( tK,KFEep.mean() ); 
     128                XtM.set_col ( tK,Mep.mean() ); 
     129                XtV.set_col ( tK,xtVS ); 
    113130        } 
    114131 
    … …  
    118135        fou << Name ( "Dt" ) << Dt; 
    119136        fou << Name ( "xthE" ) << XtE; 
    120         fou << Name ( "ll" ) << ll; 
    121         fou << Name ( "llgrid" ) << vQ; 
     137        fou << Name ( "xthM" ) << XtM; 
     138        fou << Name ( "xthV" ) << XtV; 
    122139        //Exit program: 
    123140        return 0;