Changeset 1174

Timestamp:

09/01/10 10:12:19 (15 years ago)

Author:

smidl

Message:

development of fixed Bierman code

Location:

applications/pmsm

Files:

• pmsm_estim.cpp (modified) (1 diff)
• simulator_zdenek/ekf_example/CMakeLists.txt (modified) (1 diff)
• simulator_zdenek/ekf_example/ekf_obj.cpp (modified) (2 diffs)
• simulator_zdenek/ekf_example/ekf_obj.h (modified) (3 diffs)
• simulator_zdenek/ekf_example/matrix_vs.cpp (added)
• simulator_zdenek/ekf_example/matrix_vs.h (added)

applications/pmsm/pmsm_estim.cpp

@@ -3 +3 @@
 \file
 \brief Multi-Estimator (developped for PMSM)
-
-
+ 
  */
 

applications/pmsm/simulator_zdenek/ekf_example/CMakeLists.txt

@@ -2 +2 @@
 
 include_directories(../../bdm)
-add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp)
+add_library (ekf_obj ekf_obj.cpp fixed.cpp matrix.cpp matrix_vs.cpp)

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.cpp

@@ -6 +6 @@
 
 double minQ(double Q){if (Q>1.0){ return 1.0;} else {return Q;};};
+
+void mat_to_int(const imat &M, int *I){
+        for (int i=0;i<M.rows(); i++){
+                for (int j=0;j<M.cols(); j++){
+                        *I++ = M(i,j);
+                }
+        }
+}
+void vec_to_int(const ivec &v, int *I){
+        for (int i=0;i<v.length(); i++){
+                        *I++ = v(i);
+        }
+}
 
 ///////////////
@@ -167 +180 @@
   PSI[15]=0x7FFF;
 }
+
+
+void EKF_UDfix::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const vec R0 ) {
+        pfxu = pfxu0;
+        phxu = phxu0;
+        
+        set_dim ( pfxu0->_dimx() );
+        dimy = phxu0->dimension();
+        dimc = pfxu0->_dimu();
+        
+        vec &_mu = est._mu();
+        // if mu is not set, set it to zeros, just for constant terms of A and C
+        if ( _mu.length() != dimension() ) _mu = zeros ( dimension() );
+        A = zeros ( dimension(), dimension() );
+        C = zeros ( dimy, dimension() );
+        
+        //initialize matrices A C, later, these will be only updated!
+        pfxu->dfdx_cond ( _mu, zeros ( dimc ), A, true );
+        //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
+        phxu->dfdx_cond ( _mu, zeros ( dimc ), C, true );
+        //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
+        
+        R = R0;
+        Q = Q0;
+        
+        // 
+}
+// aux fnc 
+void UDtof(const mat &U, const vec &D, imat &Uf, ivec &Df, const vec &xref){
+        mat P= U*diag(D)*U.T();
+        mat T = diag(1.0/(xref));
+        mat Pf = T*P*T;
+        
+        ldmat Pld(Pf);
+        
+        mat Ut=Pld._L().T()*(1<<15); // U is in q15 -- diagonal is 0!!!
+        Uf=round_i(Ut);
+        Df=round_i(Pld._D()*(1<<15));
+        ivec zer=find(Df==0);
+        for(int i=0; i<zer.length(); i++) Df(zer(i))=1;
+}
+
+
+void EKF_UDfix::bayes ( const vec &yt, const vec &cond ) {
+        //preparatory
+        vec &_mu=est._mu();
+        const vec &u=cond;
+        int dim = dimension();
+        
+        U = est._R()._L().T();
+        D =  est._R()._D();
+        
+        ////////////
+        
+        pfxu->dfdx_cond ( _mu, u, A, false ); //update A by a derivative of fx
+        phxu->dfdx_cond ( _mu, u, C, false ); //update A by a derivative of fx
+        
+        mat PhiU = A*U;
+
+        //////
+        vec xref(5);
+        xref(0)= 30.0*1.4142;
+        xref(1)= 30.0*1.4142;
+        xref(2)= 6.283185*200.;
+        xref(3) = 3.141593;
+        xref(4) = 34.0;
+        
+        imat Utf;
+        ivec Dtf;
+        UDtof(U,D,Utf,Dtf,xref);
+        mat invxref=diag(1.0/xref);
+        imat Af=round_i(invxref*A*diag(xref)*(1<<15));
+        mat_to_int(Af, PSI);
+        mat_to_int(Utf,Uf);             
+        vec_to_int(Dtf, Df); 
+        
+        // A*U == PSI*U
+        mmultAU(PSI,Uf,PSIU,5,5);       
+/*      cout << ivec(PSIU,25) << endl;
+        cout << (Af*Utf)/(1<<15) <<endl;*/
+        
+        vec Din = D;
+        int i,j,k;
+        double sigma;
+        mat G = eye(dim);
+        //////// thorton
+        
+        //move mean;
+        _mu = pfxu->eval(_mu,u);
+        
+        for (i=dim-1; i>=0;i--){
+                sigma = 0.0; 
+                for (j=0; j<dim; j++) {
+                        sigma += PhiU(i,j)*PhiU(i,j) *Din(j); 
+                        sigma += G(i,j)*G(i,j) * Q(j,j);
+                }
+                
+/*              double sigma2= 0.0;
+                for (j=0; j<dim; j++) {
+                        sigma2 += PhiU(i,j)*PhiU(i,j) *Din(j); 
+                }
+                sigma2 +=Q(i,i);//*G(i,i)=1.0
+                for (j=i+1; j<dim; j++) {
+                        sigma2 += G(i,j)*G(i,j) * Q(j,j);
+                }*/
+                
+/*              UDtof(U,D,Utf,Dtf,xref);
+                cout << "d=sig"<<endl;
+                cout << Utf << endl << Dtf << endl;
+                cout << G << endl << Din << endl<<endl;*/
+                
+                D(i) = sigma; 
+                for (j=0;j<i;j++){ 
+//                      cout << i << "," << j << endl;
+                        sigma = 0.0; 
+                        for (k=0;k<dim;k++){ 
+                                sigma += PhiU(i,k)*Din(k)*PhiU(j,k); 
+                        }
+                        for (k=0;k<dim;k++){ 
+                                sigma += G(i,k)*Q(k,k)*G(j,k); 
+                        }
+                        //
+                        U(j,i) = sigma/D(i); 
+                        
+/*                      cout << "U=sig/D"<<endl;
+                        UDtof(U,D,Utf,Dtf,xref);
+                        cout << Utf << endl << Dtf << endl;
+                        cout << G << endl << Din << endl<<endl;*/
+                        
+                        for (k=0;k<dim;k++){ 
+                                PhiU(j,k) = PhiU(j,k) - U(j,i)*PhiU(i,k); 
+                        }
+                        for (k=0;k<dim;k++){ 
+                                G(j,k) = G(j,k) - U(j,i)*G(i,k); 
+                        }
+                
+/*                      cout << "end"<<endl;
+                        UDtof(U,D,Utf,Dtf,xref);
+                        cout << G << endl << Din << endl;
+                        cout << Utf << endl << Dtf << endl<<endl;*/
+                }
+        }
+
+        int Qf[25];
+        imat Qrnd=round_i(diag(elem_div(diag(Q),pow(xref,2)))*(1<<15));
+//      cout << "Qr" << Qrnd<<endl;
+        mat_to_int(Qrnd,Qf);
+
+        thorton(Uf,Df,PSIU,Qf,Gf,Dfold,5);
+        
+        cout << "bierman double I" <<endl;
+        UDtof(U,D,Utf,Dtf,xref);
+        cout << "Uf: " << Utf << endl;
+        cout << "Df: " << Dtf << endl;
+        cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
+        mat_to_int(Utf,Uf);             
+        vec_to_int(Dtf, Df); 
+        
+        // bierman
+        
+        double dz,alpha,gamma,beta,lambda;
+        vec a;
+        vec b;
+        vec yp = phxu->eval(_mu,u);
+        vec xp=_mu; // used in bierman
+        
+        for (int iy=0; iy<dimy; iy++){
+                a     = U.T()*C.get_row(iy);    // a is not modified, but 
+                b     = elem_mult(D,a);                          // b is modified to become unscaled Kalman gain. 
+                dz    = yt(iy) - yp(iy); 
+                
+                alpha = R(iy); 
+                gamma = 1/alpha; 
+                for (j=0;j<dim;j++){
+                        beta   = alpha; 
+                        alpha  = alpha + a(j)*b(j); 
+                        lambda = -a(j)*gamma; 
+                        gamma  = 1.0/alpha; 
+                        D(j) = beta*gamma*D(j); 
+                        
+                        //                      cout << "a: " << alpha << "g: " << gamma << endl;
+                        for (i=0;i<j;i++){
+                                beta   = U(i,j); 
+                                U(i,j) = beta + b(i)*lambda; 
+                                b(i)   = b(i) + b(j)*beta; 
+                        }
+                }
+                double dzs = gamma*dz;  // apply scaling to innovations 
+                _mu   = _mu + dzs*b; // multiply by unscaled Kalman gain 
+                //cout << "Ub: " << U << endl;
+                //cout << "Db: " << D << endl <<endl;
+                
+        }
+
+UDtof(U,D,Utf,Dtf,xref);
+cout << "Uf: " << Utf << endl;
+cout << "Df: " << Dtf << endl;
+cout << "xf:" << round_i(elem_div(_mu,xref)*(1<<15)) <<endl;
+
+        int difz[2];
+        vec_to_int(round_i((yt-yp)/xref(0)*(1<<15)), difz);
+        int xf[5];
+        vec_to_int(round_i(elem_div(xp,xref)*(1<<15)), xf);
+        int Rf[2]={1,1};
+        //vec_to_int(round_i(R/pow(xref(0),2)*(1<<15)), Rf);
+        //beirman(int *difz, int *xp, int *U, int *D, int *R, unsigned int dimy, unsigned int dimx 
+        
+        cout <<endl<< "bierman fixed" <<endl;
+        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+        for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+        for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+
+        for (i=0; i<2;i++) cout << difz[i] << ","; cout << endl;
+
+        int xf_old[5];
+        vec_to_int(ivec(xf,5),xf_old);
+        bierman(difz,xf, Uf, Df, Rf, 2, 5);
+        
+        UDtof(U,D,Utf,Dtf,xref);
+        for (i=0; i<25;i++) cout << Uf[i] << ","; cout <<endl;
+        for (i=0; i<5;i++) cout << Df[i] << ","; cout << endl;
+        for (i=0; i<5;i++) cout << xf[i] << ","; cout << endl;
+        cout << endl;
+        
+        
+        
+        /////
+        est._R().__L()=U.T();
+        est._R().__D()=D;
+        
+        if ( evalll == true ) { //likelihood of observation y
+        }
+}
+
+void EKF_UDfix::from_setting ( const Setting &set ) {
+        BM::from_setting ( set );
+        shared_ptr<diffbifn> IM = UI::build<diffbifn> ( set, "IM", UI::compulsory );
+        shared_ptr<diffbifn> OM = UI::build<diffbifn> ( set, "OM", UI::compulsory );
+        
+        //statistics
+        int dim = IM->dimension();
+        vec mu0;
+        if ( !UI::get ( mu0, set, "mu0" ) )
+                mu0 = zeros ( dim );
+        
+        mat P0;
+        vec dP0;
+        if ( UI::get ( dP0, set, "dP0" ) )
+                P0 = diag ( dP0 );
+        else if ( !UI::get ( P0, set, "P0" ) )
+                P0 = eye ( dim );
+        
+        est._mu()=mu0;
+        est._R()=ldmat(P0);
+        
+        //parameters
+        vec dQ, dR;
+        UI::get ( dQ, set, "dQ", UI::compulsory );
+        UI::get ( dR, set, "dR", UI::compulsory );
+        set_parameters ( IM, OM, diag ( dQ ), dR  );
+        
+        UI::get(log_level, set, "log_level", UI::optional);
+}

applications/pmsm/simulator_zdenek/ekf_example/ekf_obj.h

@@ -18 +18 @@
 #include "fixed.h"
 #include "matrix.h"
+#include "matrix_vs.h"
 #include "reference.h"
 #include "parametry_motoru.h"
@@ -24 +25 @@
 
 double minQ(double Q);
+
+void mat_to_int(const imat &M, int *I);
+void vec_to_int(const ivec &v, int *I);
 
 /*!
@@ -97 +101 @@
 UIREGISTER(EKFfixed);
 
+//! EKF for comparison of EKF_UD with its fixed-point implementation
+class EKF_UDfix : public BM {
+        protected:
+                //! logger
+                LOG_LEVEL(EKF_UDfix,logU, logG);
+                //! Internal Model f(x,u)
+                shared_ptr<diffbifn> pfxu;
+                
+                //! Observation Model h(x,u)
+                shared_ptr<diffbifn> phxu;
+                
+                //! U part
+                mat U;
+                //! D part
+                vec D;
+                
+                int Uf[25];
+                int Df[5];
+                int Dfold[5];
+                
+                mat A;
+                mat C;
+                mat Q;
+                vec R;
+                
+                int PSI[25];
+                int PSIU[25];
+                int Gf[25];
+
+                enorm<ldmat> est;
+                
+                
+        public:
+                
+                //! copy constructor duplicated 
+                EKF_UDfix* _copy() const {
+                        return new EKF_UDfix(*this);
+                }
+                
+                const enorm<ldmat>& posterior()const{return est;};
+                
+                enorm<ldmat>& prior() {
+                        return const_cast<enorm<ldmat>&>(posterior());
+                }
+                
+                EKF_UDfix(){}
+                
+                
+                EKF_UDfix(const EKF_UDfix &E0): pfxu(E0.pfxu),phxu(E0.phxu), U(E0.U), D(E0.D){}
+                
+                //! Set nonlinear functions for mean values and covariance matrices.
+                void set_parameters ( const shared_ptr<diffbifn> &pfxu, const shared_ptr<diffbifn> &phxu, const mat Q0, const vec R0 );
+                
+                //! Here dt = [yt;ut] of appropriate dimensions
+                void bayes ( const vec &yt, const vec &cond = empty_vec );
+                
+                void log_register ( bdm::logger& L, const string& prefix ){
+                        BM::log_register ( L, prefix );
+                        
+                        if ( log_level[logU] )
+                                L.add_vector ( log_level, logU, RV ( dimension()*dimension() ), prefix );
+                        if ( log_level[logG] )
+                                L.add_vector ( log_level, logG, RV ( dimension()*dimension() ), prefix );
+                        
+                }
+                /*! Create object from the following structure
+                
+                \code
+                class = 'EKF_UD';
+                OM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+                IM = configuration of bdm::diffbifn;    % any offspring of diffbifn, bdm::diffbifn::from_setting
+                dQ = [...];                             % vector containing diagonal of Q
+                dR = [...];                             % vector containing diagonal of R
+                --- optional fields ---
+                mu0 = [...];                            % vector of statistics mu0
+                dP0 = [...];                            % vector containing diagonal of P0
+                -- or --
+                P0 = [...];                             % full matrix P0
+                --- inherited fields ---
+                bdm::BM::from_setting
+                \endcode
+                If the optional fields are not given, they will be filled as follows:
+                \code
+                mu0 = [0,0,0,....];                     % empty statistics
+                P0 = eye( dim );              
+                \endcode
+                */
+                void from_setting ( const Setting &set );
+                
+                void validate() {};
+                // TODO dodelat void to_setting( Setting &set ) const;
+                
+};
+UIREGISTER(EKF_UDfix);
+
+
 #endif // KF_H