Show
Ignore:
Timestamp:
08/02/12 22:43:00 (12 years ago)
Author:
smidl
Message:

opraven test Ch

Location:
applications/pmsm/simulator_zdenek/ekf_example
Files:
4 modified

Legend:

Unmodified
Added
Removed

  • applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp

    r1464 r1466  
    11#include "ekf_mm.h" 
    22#include "qmath.h" 
     3#include "stdio.h" 
    34 
    45 
    … …  
    1213        //               q15              + q15*q13 
    1314        (E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15; 
     15        E->x_pred[1]=E->x_est[1]; 
     16        E->x_pred[0]=E->x_est[0]; 
    1417         
    1518        mmultACh(E->PSI,E->Chf,E->PSICh,2,2); 
    … …  
    3639        E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13 
    3740         
    38          
    39         int16 difz[2]; 
    40         difz[0]=(isx-(E->y_est[0])); // shift to q15!! 
    41         difz[1]=(isy-(E->y_est[1]));//<<2; 
     41        E->difz[0]=(isx-(E->y_est[0])); // shift to q15!! 
     42        E->difz[1]=(isy-(E->y_est[1]));//<<2; 
    4243         
    4344        E->y_old[0] = isx; 
    … …  
    4546         
    4647        *detS = 32767; *rem=0; 
    47         carlson_fastC(difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem); 
     48        carlson_fastC(E->difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem); 
    4849} 
    4950 

  • applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.h

    r1464 r1466  
    2525         
    2626        int16 x_est[2]; /* estimate and prediction */ 
     27        int16 x_pred[2]; /* estimate and prediction */ 
    2728        int16 y_est[2]; /* estimate and prediction */ 
    2829        int16 y_old[2]; /* estimate and prediction */ 
    … …  
    3435        int16 Chf[4]; // upper triangular of covariance (inplace) 
    3536         
     37        int16 difz[2]; 
    3638        int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
    3739}; 

  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

    r1464 r1466  
    1  
    21#include <estim/kalman.h> 
    3  
    42#include "ekf_obj.h" 
    53//#include "../simulator.h" 
    … …  
    1210                        *I++ = M(i,j); 
    1311                } 
     12        } 
     13} 
     14void int16_to_mat(int16 *I, imat &M, int rows, int cols){ 
     15        M.set_size(rows,cols); 
     16        for (int16 i=0;i<M.rows(); i++){ 
     17                for (int16 j=0;j<M.cols(); j++){ 
     18                         M(i,j) = *I++; 
     19                } 
     20        } 
     21} 
     22void int16_to_vec(int16 *I, ivec &v, int len){ 
     23        v.set_size(len); 
     24        for (int16 i=0;i<v.length(); i++){ 
     25                        v(i) = *I++; 
    1426        } 
    1527} 
    … …  
    10431055        } 
    10441056         
    1045          
    10461057        { 
    10471058                int Chi[4];     for (int i=0;i<4; i++) Chi[i]=(int)Chf[i]; 

  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

    r1464 r1466  
    2828using namespace bdm; 
    2929 
    30 double minQ(double Q); 
     30double minQ(double Q);  
    3131 
    3232void mat_to_int16(const imat &M, int16 *I); 
    3333void vec_to_int16(const ivec &v, int16 *I); 
     34void int16_to_mat(int16 *I, imat &M, int rows, int cols); 
     35void int16_to_vec(int16 *I, ivec &v, int len); 
    3436void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref); 
    3537 
    36 #ifdef XXX 
    37 /*! 
    38 \brief Extended Kalman Filter with full matrices in fixed point16 arithmetic 
    39  
    40 An approximation of the exact Bayesian filter with Gaussian noices and non-linear evolutions of their mean. 
    41 */ 
    42 class EKFfixed : public BM { 
    43 public: 
    44 void init_ekf(double Tv); 
    45 void ekf(double ux, double uy, double isxd, double isyd); 
    46  
    47 /* Declaration of local functions */ 
    48 void prediction(int16 *ux); 
    49 void correction(void); 
    50 void update_psi(void); 
    51  
    52 /* Constants - definovat jako konstanty ?? ?kde je vyhodnejsi aby v pameti byli?*/ 
    53  int16 Q[16]; /* matrix [4,4] */ 
    54  int16 R[4]; /* matrix [2,2] */ 
    55  
    56  int16 x_est[4]; 
    57  int16 x_pred[4]; 
    58  int16 P_pred[16]; /* matrix [4,4] */ 
    59  int16 P_est[16]; /* matrix [4,4] */ 
    60  int16 Y_mes[2]; 
    61  int16 ukalm[2]; 
    62  int16 Kalm[8]; /* matrix [5,2] */ 
    63  
    64  int16 PSI[16]; /* matrix [4,4] */ 
    65  
    66  int16 temp15a[16]; 
    67  
    68  int16 cA, cB, cC, cG, cH;  // cD, cE, cF, cI ... nepouzivane 
    69  
    70  int32 temp30a[4]; /* matrix [2,2] - temporary matrix for inversion */ 
    71  enorm<fsqmat> E; 
    72  mat Ry; 
    73    
    74 public: 
    75         //! Default constructor 
    76         EKFfixed ():BM(),E(),Ry(2,2){ 
    77                 int16 i; 
    78                 for(i=0;i<16;i++){Q[i]=0;} 
    79                 for(i=0;i<4;i++){R[i]=0;} 
    80  
    81                 for(i=0;i<4;i++){x_est[i]=0;} 
    82                 for(i=0;i<4;i++){x_pred[i]=0;} 
    83                 for(i=0;i<16;i++){P_pred[i]=0;} 
    84                 for(i=0;i<16;i++){P_est[i]=0;} 
    85                 P_est[0]=0x7FFF; 
    86                 P_est[5]=0x7FFF; 
    87                 P_est[10]=0x7FFF; 
    88                 P_est[15]=0x7FFF; 
    89                 for(i=0;i<2;i++){Y_mes[i]=0;} 
    90                 for(i=0;i<2;i++){ukalm[i]=0;} 
    91                 for(i=0;i<8;i++){Kalm[i]=0;} 
    92  
    93                 for(i=0;i<16;i++){PSI[i]=0;} 
    94  
    95                 set_dim(4); 
    96                 E._mu()=zeros(4); 
    97                 E._R()=zeros(4,4); 
    98                 init_ekf(0.000125); 
    99         }; 
    100         //! Here dt = [yt;ut] of appropriate dimensions 
    101         void bayes ( const vec &yt, const vec &ut ); 
    102         //!dummy! 
    103         const epdf& posterior() const {return E;}; 
    104          
    105 }; 
    106  
    107 UIREGISTER(EKFfixed); 
    108  
    109 #endif 
    11038 
    11139//! EKF for testing q44 
    … …  
    12149}; 
    12250UIREGISTER(EKFtest); 
     51 
     52 
     53class EKFscale: public EKFCh{ 
     54        LOG_LEVEL(EKFscale, logCh, logA, logC); 
     55public: 
     56        mat Tx; 
     57        mat Ty; 
     58         
     59        void from_setting ( const Setting &set ){ 
     60                EKFCh::from_setting(set); 
     61                vec v; 
     62                UI::get(v, set, "xmax", UI::compulsory); 
     63                Tx = diag(1./v); 
     64                UI::get(v, set, "ymax", UI::compulsory); 
     65                Ty = diag(1./v); 
     66                 
     67                UI::get(log_level, set, "log_level", UI::optional); 
     68        }; 
     69         
     70        void log_register(logger &L, const string &prefix){ 
     71                BM::log_register ( L, prefix ); 
     72                 
     73                L.add_vector ( log_level, logCh, RV ("Ch", dimension()*dimension() ), prefix ); 
     74                L.add_vector ( log_level, logA, RV ("A", dimension()*dimension() ), prefix ); 
     75                L.add_vector ( log_level, logC, RV ("C", dimensiony()*dimension() ), prefix ); 
     76        }; 
     77         
     78        void log_write() const{ 
     79                BM::log_write(); 
     80            if ( log_level[logCh] ) { 
     81                        mat Chsc=Tx*(est._R()._Ch()); 
     82                        vec v(Chsc._data(), dimension()*dimension()); 
     83                        if (v(0)<0) 
     84                                v= -v; 
     85                        log_level.store( logCh, round(v*32767)); 
     86                } 
     87                if (log_level[logA]){ 
     88                        mat Asc = Tx*A*inv(Tx); 
     89                        vec v(Asc._data(), dimension()*dimension()); 
     90                        log_level.store( logA, round(v*32767)); 
     91                } 
     92                if (log_level[logC]){ 
     93                        mat Csc = Ty*C*inv(Tx); 
     94                        vec v(Csc._data(), dimensiony()*dimension()); 
     95                        log_level.store( logC, round(v*32767)); 
     96                } 
     97        } 
     98         
     99}; 
     100UIREGISTER(EKFscale); 
    123101 
    124102/*! 
    … …  
    400378                L.add_vector ( log_level, logCh, RV ("Ch", 16 ), prefix ); 
    401379                L.add_vector ( log_level, logA, RV ("A", 16 ), prefix ); 
    402                 L.add_vector ( log_level, logP, RV ("P", 16 ), prefix ); 
    403                  
     380                L.add_vector ( log_level, logP, RV ("P", 16 ), prefix );                 
    404381        }; 
    405382        //void from_setting(); 
    … …  
    656633//! EKF with covariance R estimated by the VB method 
    657634class EKFvbR: public EKFfull{    
    658     LOG_LEVEL(EKFvbR,logR,logQ); 
     635    LOG_LEVEL(EKFvbR,logR,logQ,logP); 
    659636 
    660637        //! Statistics of the R estimator 
    … …  
    691668                L.add_vector(log_level, logR, RV("{R }",vec_1<int>(4)), prefix); 
    692669                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(4)), prefix); 
     670                L.add_vector(log_level, logP, RV("{P }",vec_1<int>(4)), prefix); 
    693671        } 
    694672        void bayes ( const vec &val, const vec &cond ) { 
    … …  
    709687                        EKFfull::bayes(val,cond); 
    710688 
    711                         diffy = val - fy._mu(); 
    712                         Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/; 
    713                         R = Psi_vbR/(nu-2); 
     689//                      diffy = val - fy._mu(); 
     690//                      Psi_vbR = phi*PsiR + (1-phi)*PsiR0+ outer_product(diffy,diffy)/*+C*mat(est._R())*C.T()*/; 
     691//                      R = Psi_vbR/(nu-2); 
    714692         
    715693                        diffx = est._mu() - xpred; 
    716                         Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx) /*+mat(est._R()) /*+ A*mat(P0)*A.T()*/; 
     694                        Psi_vbQ = phi*PsiQ + (1-phi)*PsiQ0+ outer_product(diffx,diffx);/*mat(est._R());  A*mat(P0)*A.T();*/ 
     695                        // 
     696                        Psi_vbQ(0,1) = 0.0; 
     697                        Psi_vbQ(1,0) = 0.0; 
    717698                        Q = Psi_vbQ/(nu-2); 
    718699                } 
    … …  
    722703                log_level.store(logQ, vec(Q._M()._data(),4)); 
    723704                log_level.store(logR, vec(R._M()._data(),4)); 
    724                  
     705                { 
     706                        mat Ch(2,2); 
     707                        Ch=chol(est._R()._M()); 
     708                        log_level.store(logP, vec(Ch._data(),4)); 
     709                } 
    725710        } 
    726711}; 
    … …  
    729714 
    730715class ekfChfix: public BM{ 
    731          
    732716        ekf_data E; 
    733717public: 
    … …  
    758742                vec dQ,dR; 
    759743                UI::get ( dQ, set, "dQ", UI::optional ); 
    760                 UI::get ( dQ, set, "dR", UI::optional ); 
     744                UI::get ( dR, set, "dR", UI::optional ); 
    761745                if (dQ.length()==2){ 
    762746                        E.Q[0]=prevod(dQ[0],15);      // 1e-3 
    … …  
    774758UIREGISTER(ekfChfix); 
    775759 
     760 
     761 
     762class ekfChfixQ: public BM{ 
     763        LOG_LEVEL(ekfChfixQ,logQ,logCh,logC,logRes) 
     764        ekf_data E; 
     765        int64 PSI_Q0, PSI_Q1, PSI_Q0_reg, PSI_Q1_reg; 
     766        int Q_ni; 
     767        int phi_Q; 
     768public: 
     769        ekfChfixQ(){ 
     770                init_ekfCh2(&E,0.000125);set_dim(2);    dimc = 2; 
     771                dimy = 2; 
     772                Q_ni = 7; 
     773                phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni); 
     774 
     775                PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni; 
     776                PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni; 
     777                PSI_Q0 = ((int64)E.Q[0])<<Q_ni; 
     778                PSI_Q1 = ((int64)E.Q[3])<<Q_ni; 
     779        }  
     780        void bayes ( const vec &val, const vec &cond ) { 
     781                int16 ux,uy; 
     782                ux=prevod(cond[0]/Uref,15); 
     783                uy=prevod(cond[1]/Uref,15); 
     784                 
     785                int16 yx,yy; 
     786                // zadani mereni 
     787                yx=prevod(val[0]/Iref,15); 
     788                yy=prevod(val[1]/Iref,15); 
     789                 
     790                int16 detS, rem; 
     791                ekfCh2(&E, ux,uy,yx,yy, &detS, &rem); 
     792                 
     793                Est._mu()=vec_2(E.x_est[0]*Uref/32768., E.x_est[1]*Uref/32768.); 
     794                 
     795                int16 xerr0 = E.x_est[0]-E.x_pred[0]; 
     796                PSI_Q0 = ((int64)(phi_Q*PSI_Q0) + (((int64)xerr0*xerr0)<<15) + PSI_Q0_reg)>>15;          
     797                E.Q[0] = PSI_Q0>>Q_ni; 
     798 
     799                xerr0 = E.x_est[1]-E.x_pred[1]; 
     800                PSI_Q1 = ((int64)(phi_Q*PSI_Q1) + (((int64)xerr0*xerr0)<<15) + PSI_Q1_reg)>>15;          
     801                E.Q[3] = PSI_Q1>>Q_ni; 
     802                 
     803                cout << E.Q[0] << "," << E.Q[3] <<endl; 
     804                ll = -0.5*qlog(detS)-0.5*rem; 
     805        } 
     806        const epdf& posterior() const {return Est;}; 
     807        void from_setting ( const Setting &set ){ 
     808                BM::from_setting(set); 
     809                vec dQ,dR; 
     810                UI::get ( dQ, set, "dQ", UI::optional ); 
     811                UI::get ( dR, set, "dR", UI::optional ); 
     812                if (dQ.length()==2){ 
     813                        E.Q[0]=prevod(dQ[0],15);      // 1e-3 
     814                        E.Q[3]=prevod(dQ[1],15);      // 1e-3 
     815                } 
     816                if (dR.length()==2){ 
     817                        E.dR[0]=prevod(dR[0],15);      // 1e-3 
     818                        E.dR[1]=prevod(dR[1],15);      // 1e-3 
     819                } 
     820 
     821                UI::get(Q_ni, set, "Q_ni", UI::optional); 
     822                { // zmena Q!! 
     823                        phi_Q = ((1<<Q_ni) -1)<<(15-Q_ni); 
     824 
     825                        PSI_Q0_reg = ((int64)((1<<15)-phi_Q)*E.Q[0])<<Q_ni; 
     826                        PSI_Q1_reg = ((int64)((1<<15)-phi_Q)*E.Q[3])<<Q_ni; 
     827                        PSI_Q0 = ((int64)E.Q[0])<<Q_ni; 
     828                        PSI_Q1 = ((int64)E.Q[3])<<Q_ni; 
     829 
     830                } 
     831                UI::get(log_level, set, "log_level", UI::optional); 
     832 
     833        } 
     834        void log_register ( logger &L, const string &prefix ) { 
     835                BM::log_register(L,prefix); 
     836                L.add_vector(log_level, logQ, RV("{Q }",vec_1<int>(2)), prefix); 
     837                L.add_vector(log_level, logCh, RV("{Ch }",vec_1<int>(3)), prefix); 
     838                L.add_vector(log_level, logC, RV("{C }",vec_1<int>(4)), prefix); 
     839                L.add_vector(log_level, logRes, RV("{dy }",vec_1<int>(2)), prefix); 
     840        } 
     841        void log_write() const { 
     842                BM::log_write(); 
     843                if ( log_level[logQ] ) { 
     844                        log_level.store( logQ, vec_2((double)E.Q[0],(double)E.Q[3] )); 
     845                } 
     846                if ( log_level[logCh] ) { 
     847                        log_level.store( logCh, vec_3((double)E.Chf[0],(double)E.Chf[1], (double)E.Chf[3])); 
     848                } 
     849                if ( log_level[logRes] ) { 
     850                        log_level.store( logRes, vec_2((double)E.difz[0],(double)E.difz[1])); 
     851                } 
     852                if ( log_level[logC] ) { 
     853                        vec v(4); 
     854                        for (int i=0;i<4;i++) v(i)=E.C[i]; 
     855                        log_level.store( logC, v); 
     856                } 
     857        } 
     858         
     859        enorm<fsqmat> Est; 
     860        mat Ry; 
     861}; 
     862UIREGISTER(ekfChfixQ); 
     863 
    776864#endif // KF_H 
    777865