Changeset 1174 for applications/pmsm

Show
Ignore:
Timestamp:
09/01/10 10:12:19 (14 years ago)
Author:
smidl
Message:

development of fixed Bierman code

Location:
applications/pmsm
Files:
2 added
4 modified

Legend:

Unmodified
Added
Removed
  • applications/pmsm/pmsm_estim.cpp

    r1168 r1174  
    33\file 
    44\brief Multi-Estimator (developped for PMSM) 
    5  
    6  
     5  
    76 */ 
    87 
  • applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt

    r66 r1174  
    22 
    33include_directories(../../bdm) 
    4 add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp) 
     4add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp) 
  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

    r1168 r1174  
    66 
    77double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};}; 
     8 
     9void mat_to_int(const imat &M, int *I){ 
     10        for (int i=0;i<M.rows(); i++){ 
     11                for (int j=0;j<M.cols(); j++){ 
     12                        *I++ = M(i,j); 
     13                } 
     14        } 
     15} 
     16void vec_to_int(const ivec &v, int *I){ 
     17        for (int i=0;i<v.length(); i++){ 
     18                        *I++ = v(i); 
     19        } 
     20} 
    821 
    922/////////////// 
     
    167180  PSI[15]=0x7FFF; 
    168181} 
     182 
     183 
     184void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) { 
     185        pfxu = pfxu0; 
     186        phxu = phxu0; 
     187         
     188        set_dim ( pfxu0->_dimx() ); 
     189        dimy = phxu0->dimension(); 
     190        dimc = pfxu0->_dimu(); 
     191         
     192        vec &_mu = est._mu(); 
     193        // if mu is not set, set it to zeros, just for constant terms of A and C 
     194        if ( _mu.length() != dimension() ) _mu = zeros ( dimension() ); 
     195        A = zeros ( dimension(), dimension() ); 
     196        C = zeros ( dimy, dimension() ); 
     197         
     198        //initialize matrices A C, later, these will be only updated! 
     199        pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true ); 
     200        //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true ); 
     201        phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true ); 
     202        //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true ); 
     203         
     204        R = R0; 
     205        Q = Q0; 
     206         
     207        //  
     208} 
     209// aux fnc  
     210void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref){ 
     211        mat P= U*diag(D)*U.T(); 
     212        mat T = diag(1.0/(xref)); 
     213        mat Pf = T*P*T; 
     214         
     215        ldmat Pld(Pf); 
     216         
     217        mat Ut=Pld._L().T()*(1<<15); // U is in q15 -- diagonal is 0!!! 
     218        Uf=round_i(Ut); 
     219        Df=round_i(Pld._D()*(1<<15)); 
     220        ivec zer=find(Df==0); 
     221        for(int i=0; i<zer.length(); i++) Df(zer(i))=1; 
     222} 
     223 
     224 
     225void EKF_UDfix::bayes ( const vec &yt, const vec &cond ) { 
     226        //preparatory 
     227        vec &_mu=est._mu(); 
     228        const vec &u=cond; 
     229        int dim = dimension(); 
     230         
     231        U = est._R()._L().T(); 
     232        D =  est._R()._D(); 
     233         
     234        //////////// 
     235         
     236        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx 
     237        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx 
     238         
     239        mat PhiU = A*U; 
     240 
     241        ////// 
     242        vec xref(5); 
     243        xref(0)= 30.0*1.4142; 
     244        xref(1)= 30.0*1.4142; 
     245        xref(2)= 6.283185*200.; 
     246        xref(3) = 3.141593; 
     247        xref(4) = 34.0; 
     248         
     249        imat Utf; 
     250        ivec Dtf; 
     251        UDtof(U,D,Utf,Dtf,xref); 
     252        mat invxref=diag(1.0/xref); 
     253        imat Af=round_i(invxref*A*diag(xref)*(1<<15)); 
     254        mat_to_int(Af, PSI); 
     255        mat_to_int(Utf,Uf);              
     256        vec_to_int(Dtf, Df);  
     257         
     258        // A*U == PSI*U 
     259        mmultAU(PSI,Uf,PSIU,5,5);        
     260/*      cout << ivec(PSIU,25) << endl; 
     261        cout << (Af*Utf)/(1<<15) <<endl;*/ 
     262         
     263        vec Din = D; 
     264        int i,j,k; 
     265        double sigma; 
     266        mat G = eye(dim); 
     267        //////// thorton 
     268         
     269        //move mean; 
     270        _mu = pfxu->eval(_mu,u); 
     271         
     272        for (i=dim-1; i>=0;i--){ 
     273                sigma = 0.0;  
     274                for (j=0; j<dim; j++) { 
     275                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j);  
     276                        sigma += G(i,j)*G(i,j) * Q(j,j); 
     277                } 
     278                 
     279/*              double sigma2= 0.0; 
     280                for (j=0; j<dim; j++) { 
     281                        sigma2 += PhiU(i,j)*PhiU(i,j) *Din(j);  
     282                } 
     283                sigma2 +=Q(i,i);//*G(i,i)=1.0 
     284                for (j=i+1; j<dim; j++) { 
     285                        sigma2 += G(i,j)*G(i,j) * Q(j,j); 
     286                }*/ 
     287                 
     288/*              UDtof(U,D,Utf,Dtf,xref); 
     289                cout << "d=sig"<<endl; 
     290                cout << Utf << endl << Dtf << endl; 
     291                cout << G << endl << Din << endl<<endl;*/ 
     292                 
     293                D(i) = sigma;  
     294                for (j=0;j<i;j++){  
     295//                      cout << i << "," << j << endl; 
     296                        sigma = 0.0;  
     297                        for (k=0;k<dim;k++){  
     298                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k);  
     299                        } 
     300                        for (k=0;k<dim;k++){  
     301                                sigma += G(i,k)*Q(k,k)*G(j,k);  
     302                        } 
     303                        // 
     304                        U(j,i) = sigma/D(i);  
     305                         
     306/*                      cout << "U=sig/D"<<endl; 
     307                        UDtof(U,D,Utf,Dtf,xref); 
     308                        cout << Utf << endl << Dtf << endl; 
     309                        cout << G << endl << Din << endl<<endl;*/ 
     310                         
     311                        for (k=0;k<dim;k++){  
     312                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k);  
     313                        } 
     314                        for (k=0;k<dim;k++){  
     315                                G(j,k) = G(j,k) - U(j,i)*G(i,k);  
     316                        } 
     317                 
     318/*                      cout << "end"<<endl; 
     319                        UDtof(U,D,Utf,Dtf,xref); 
     320                        cout << G << endl << Din << endl; 
     321                        cout << Utf << endl << Dtf << endl<<endl;*/ 
     322                } 
     323        } 
     324 
     325        int Qf[25]; 
     326        imat Qrnd=round_i(diag(elem_div(diag(Q),pow(xref,2)))*(1<<15)); 
     327//      cout << "Qr" << Qrnd<<endl; 
     328        mat_to_int(Qrnd,Qf); 
     329 
     330        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5); 
     331         
     332        cout << "bierman double I" <<endl; 
     333        UDtof(U,D,Utf,Dtf,xref); 
     334        cout << "Uf: " << Utf << endl; 
     335        cout << "Df: " << Dtf << endl; 
     336        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl; 
     337        mat_to_int(Utf,Uf);              
     338        vec_to_int(Dtf, Df);  
     339         
     340        // bierman 
     341         
     342        double dz,alpha,gamma,beta,lambda; 
     343        vec a; 
     344        vec b; 
     345        vec yp = phxu->eval(_mu,u); 
     346        vec xp=_mu; // used in bierman 
     347         
     348        for (int iy=0; iy<dimy; iy++){ 
     349                a     = U.T()*C.get_row(iy);    // a is not modified, but  
     350                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain.  
     351                dz    = yt(iy) - yp(iy);  
     352                 
     353                alpha = R(iy);  
     354                gamma = 1/alpha;  
     355                for (j=0;j<dim;j++){ 
     356                        beta   = alpha;  
     357                        alpha  = alpha + a(j)*b(j);  
     358                        lambda = -a(j)*gamma;  
     359                        gamma  = 1.0/alpha;  
     360                        D(j) = beta*gamma*D(j);  
     361                         
     362                        //                      cout << "a: " << alpha << "g: " << gamma << endl; 
     363                        for (i=0;i<j;i++){ 
     364                                beta   = U(i,j);  
     365                                U(i,j) = beta + b(i)*lambda;  
     366                                b(i)   = b(i) + b(j)*beta;  
     367                        } 
     368                } 
     369                double dzs = gamma*dz;  // apply scaling to innovations  
     370                _mu   = _mu + dzs*b; // multiply by unscaled Kalman gain  
     371                //cout << "Ub: " << U << endl; 
     372                //cout << "Db: " << D << endl <<endl; 
     373                 
     374        } 
     375 
     376UDtof(U,D,Utf,Dtf,xref); 
     377cout << "Uf: " << Utf << endl; 
     378cout << "Df: " << Dtf << endl; 
     379cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl; 
     380 
     381        int difz[2]; 
     382        vec_to_int(round_i((yt-yp)/xref(0)*(1<<15)), difz); 
     383        int xf[5]; 
     384        vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf); 
     385        int Rf[2]={1,1}; 
     386        //vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf); 
     387        //beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx  
     388         
     389        cout <<endl<< "bierman fixed" <<endl; 
     390        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl; 
     391        for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl; 
     392        for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl; 
     393 
     394        for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl; 
     395 
     396        int xf_old[5]; 
     397        vec_to_int(ivec(xf,5),xf_old); 
     398        bierman(difz,xf, Uf, Df, Rf, 2, 5); 
     399         
     400        UDtof(U,D,Utf,Dtf,xref); 
     401        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl; 
     402        for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl; 
     403        for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl; 
     404        cout << endl; 
     405         
     406         
     407         
     408        ///// 
     409        est._R().__L()=U.T(); 
     410        est._R().__D()=D; 
     411         
     412        if ( evalll == true ) { //likelihood of observation y 
     413        } 
     414} 
     415 
     416void EKF_UDfix::from_setting ( const Setting &set ) { 
     417        BM::from_setting ( set ); 
     418        shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory ); 
     419        shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory ); 
     420         
     421        //statistics 
     422        int dim = IM->dimension(); 
     423        vec mu0; 
     424        if ( !UI::get ( mu0, set, "mu0" ) ) 
     425                mu0 = zeros ( dim ); 
     426         
     427        mat P0; 
     428        vec dP0; 
     429        if ( UI::get ( dP0, set, "dP0" ) ) 
     430                P0 = diag ( dP0 ); 
     431        else if ( !UI::get ( P0, set, "P0" ) ) 
     432                P0 = eye ( dim ); 
     433         
     434        est._mu()=mu0; 
     435        est._R()=ldmat(P0); 
     436         
     437        //parameters 
     438        vec dQ, dR; 
     439        UI::get ( dQ, set, "dQ", UI::compulsory ); 
     440        UI::get ( dR, set, "dR", UI::compulsory ); 
     441        set_parameters ( IM, OM, diag ( dQ ), dR  ); 
     442         
     443        UI::get(log_level, set, "log_level", UI::optional); 
     444} 
  • applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

    r1168 r1174  
    1818#include "fixed.h" 
    1919#include "matrix.h" 
     20#include "matrix_vs.h" 
    2021#include "reference.h" 
    2122#include "parametry_motoru.h" 
     
    2425 
    2526double minQ(double Q); 
     27 
     28void mat_to_int(const imat &M, int *I); 
     29void vec_to_int(const ivec &v, int *I); 
    2630 
    2731/*! 
     
    97101UIREGISTER(EKFfixed); 
    98102 
     103//! EKF for comparison of EKF_UD with its fixed-point implementation 
     104class EKF_UDfix : public BM { 
     105        protected: 
     106                //! logger 
     107                LOG_LEVEL(EKF_UDfix,logU, logG); 
     108                //! Internal Model f(x,u) 
     109                shared_ptr<diffbifn> pfxu; 
     110                 
     111                //! Observation Model h(x,u) 
     112                shared_ptr<diffbifn> phxu; 
     113                 
     114                //! U part 
     115                mat U; 
     116                //! D part 
     117                vec D; 
     118                 
     119                int Uf[25]; 
     120                int Df[5]; 
     121                int Dfold[5]; 
     122                 
     123                mat A; 
     124                mat C; 
     125                mat Q; 
     126                vec R; 
     127                 
     128                int PSI[25]; 
     129                int PSIU[25]; 
     130                int Gf[25]; 
     131 
     132                enorm<ldmat> est; 
     133                 
     134                 
     135        public: 
     136                 
     137                //! copy constructor duplicated  
     138                EKF_UDfix* _copy() const { 
     139                        return new EKF_UDfix(*this); 
     140                } 
     141                 
     142                const enorm<ldmat>& posterior()const{return est;}; 
     143                 
     144                enorm<ldmat>& prior() { 
     145                        return const_cast<enorm<ldmat>&>(posterior()); 
     146                } 
     147                 
     148                EKF_UDfix(){} 
     149                 
     150                 
     151                EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){} 
     152                 
     153                //! Set nonlinear functions for mean values and covariance matrices. 
     154                void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 ); 
     155                 
     156                //! Here dt = [yt;ut] of appropriate dimensions 
     157                void bayes ( const vec &yt, const vec &cond = empty_vec ); 
     158                 
     159                void log_register ( bdm::logger& L, const string& prefix ){ 
     160                        BM::log_register ( L, prefix ); 
     161                         
     162                        if ( log_level[logU] ) 
     163                                L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix ); 
     164                        if ( log_level[logG] ) 
     165                                L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix ); 
     166                         
     167                } 
     168                /*! Create object from the following structure 
     169                 
     170                \code 
     171                class = 'EKF_UD'; 
     172                OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting 
     173                IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting 
     174                dQ = [...];                             % vector containing diagonal of Q 
     175                dR = [...];                             % vector containing diagonal of R 
     176                --- optional fields --- 
     177                mu0 = [...];                            % vector of statistics mu0 
     178                dP0 = [...];                            % vector containing diagonal of P0 
     179                -- or -- 
     180                P0 = [...];                             % full matrix P0 
     181                --- inherited fields --- 
     182                bdm::BM::from_setting 
     183                \endcode 
     184                If the optional fields are not given, they will be filled as follows: 
     185                \code 
     186                mu0 = [0,0,0,....];                     % empty statistics 
     187                P0 = eye( dim );               
     188                \endcode 
     189                */ 
     190                void from_setting ( const Setting &set ); 
     191                 
     192                void validate() {}; 
     193                // TODO dodelat void to_setting( Setting &set ) const; 
     194                 
     195}; 
     196UIREGISTER(EKF_UDfix); 
     197 
     198 
    99199#endif // KF_H 
    100200