Changeset 1158 for library

Timestamp:

08/17/10 22:06:50 (14 years ago)

Author:

smidl

Message:

Kalman with UD (bierman, thorton) and tests

Location:

library

Files:

• bdm/estim/kalman.cpp (modified) (1 diff)
• bdm/estim/kalman.h (modified) (5 diffs)
• tests/stresssuite/kalman_stress.cpp (modified) (3 diffs)
• tests/stresssuite/kalman_stress.m (modified) (3 diffs)

library/bdm/estim/kalman.cpp

@@ -436 +436 @@
 }
 
-}
+void EKF_UD::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;
+        
+        // 
+}
+
+
+void EKF_UD::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 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); 
+                }
+                D(i) = sigma; 
+                for (j=0;j<i;j++){ 
+                        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); 
+                        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 << "Ut: " << U << endl;
+        //cout << "Dt: " << D << endl;
+        // bierman
+        
+        double dz,alpha,gamma,beta,lambda;
+        vec a;
+        vec b;
+        vec yp = phxu->eval(_mu,u);
+        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;
+                
+        }
+                
+/////
+est._R().__L()=U.T();
+est._R().__D()=D;
+
+        if ( evalll == true ) { //likelihood of observation y
+        }
+}
+
+void EKF_UD::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  );
+}
+
+}

library/bdm/estim/kalman.h

@@ -362 +362 @@
 
     void validate() {
-        KalmanFull::validate();
+       // KalmanFull::validate();
 
         // check stats and IM and OM
@@ -426 +426 @@
 SHAREDPTR ( EKFCh );
 
+//! EKF using bierman and Thorton code
+class EKF_UD : public BM {
+        protected:
+                //! 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;
+                
+                mat A;
+                mat C;
+                mat Q;
+                vec R;
+
+                enorm<ldmat> est;
+        public:
+                //! copy constructor duplicated 
+                EKF_UD* _copy() const {
+                        return new EKF_UD(*this);
+                }
+
+                const enorm<ldmat>& posterior()const{return est;};
+                
+                enorm<ldmat>& prior() {
+                        return const_cast<enorm<ldmat>&>(posterior());
+                }
+                
+
+                EKF_UD(){}
+                
+                EKF_UD(const EKF_UD &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 );
+                
+                /*! 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_UD);
+
 class UKFCh : public EKFCh{
         public:
@@ -468 +541 @@
         
                 //step 4
-                mat Xpred_dif(dim2,dim);
+                mat P4=Q.to_mat();
+                vec tmp;
                 for (i=0;i<dim2;i++){
-                        Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp));
+                        tmp = Xik.get_col(i)-xp;
+                        P4+=w(i)*outer_product(tmp,tmp);
                 }               
-                
-                // NEW Sigma points
-                mat tmp;
-                qr(concat_vertical(Xpred_dif,Q._Ch()), tmp);
-                mat Ppred=tmp.get_rows(0,dim-1);
-                // New Xi(k+1)
-                Xik.set_col(0,xp);
-                for ( i=0;i<dim; i++){
-                        vec tmp=sqrt(npk)*Ppred.get_col(i);
-                        Xik.set_col(i+1, _mu+tmp);
-                        Xik.set_col(i+1+dim, _mu-tmp);
-                }
-                for (i=0;i<dim2;i++){
-                        Xpred_dif.set_row(i, sqrt(w(i)) * (Xik.get_col(i) - xp));
-                }               
-                
-                
-                
+                                        
                 //step 5
                 mat Yi(dimy,dim2);
@@ -501 +559 @@
                 }
                 //step 7
-                mat Ypred_dif(dim2,dimy);
-                
-                for (i=0; i<dim2; i++){
-                        Ypred_dif.set_row(i, sqrt(w(i))*(Yi.get_col(i)-_yp));
-                }
-                if (!qr(concat_vertical(Ypred_dif,R._Ch()) ,tmp)){ bdm_warning("QR failed");}
-                _Ry._Ch() = tmp.get_rows(0,dimy-1);
+                mat Pvv=R.to_mat();
+                for (i=0;i<dim2;i++){
+                        tmp = Yi.get_col(i)-_yp;
+                        Pvv+=w(i)*outer_product(tmp,tmp);
+                }               
+                _Ry._Ch() = chol(Pvv);
                 
                 // step 8
-                mat Pxy=Xpred_dif.T()*Ypred_dif;
+                mat Pxy=zeros(dim,dimy);
+                for (i=0;i<dim2;i++){
+                        Pxy+=w(i)*outer_product(Xi.get_col(i)-xp, Yi.get_col(i)-_yp);
+                }               
                 mat iRy=inv(_Ry._Ch());
                 
@@ -516 +576 @@
                 mat K=Pxy*iRy*iRy.T();
                 mat K2=Pxy*inv(_Ry.to_mat());
-                
+                                
                 /////////////// new filtering density
                 _mu = xp + K*(yt - _yp);
+                
+                if ( _mu ( 3 ) >pi ) _mu ( 3 )-=2*pi;
+                if ( _mu ( 3 ) <-pi ) _mu ( 3 ) +=2*pi;
                 // fill the space in Ppred;
-                mat Rpred =concat_vertical(Xpred_dif - Ypred_dif*K.T(), _Ry._Ch()*K.T());
-                
-                if(!qr(Rpred,tmp)) {bdm_warning("QR failed");}
-                _P._Ch()=tmp.get_rows(0,dim-1);
+                _P._Ch()=chol(P4-K*_Ry.to_mat()*K.T());
         }
         void from_setting(const Setting &set){

library/tests/stresssuite/kalman_stress.cpp

@@ -51 +51 @@
     RV ry ( "{y }", vec_1 ( C.rows() ) );
 
-//      // LDMAT
-//      Kalman<ldmat> KF(rx,ry,ru);
-//      KF.set_parameters(A,B,C,D,ldmat(R),ldmat(Q));
-//      KF.set_est(mu0,ldmat(P0) );
-//      epdf& KFep = KF.posterior();
-//      mat Xt(2,Ndat);
-//      Xt.set_col( 0,KFep.mean() );
+        // LDMAT
+        EKF_UD KFu;
+        KFu.set_rv(rx);
+        KFu.set_yrv(ry);
+        KFu.set_rvc(ru);
+        shared_ptr<bilinfn> f=new bilinfn; f->set_parameters(A,B);
+        shared_ptr<bilinfn> h=new bilinfn; h->set_parameters(C,D);
+        KFu.set_parameters(f,h,Q,diag(R));
+        KFu.prior()._mu()=mu0;
+        KFu.prior()._R()=ldmat(P0);
+        const epdf& KFuep = KFu.posterior();
+        mat Xtu(dimx,Ndat);
+        Xtu.set_col( 0,KFuep.mean() );
 
     //Chol
@@ -99 +105 @@
     for ( int t = 1; t < Ndat; t++ ) {
         dt = Dt.get_col ( t );
-        KF.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
-        Xt.set_col ( t, KFep.mean() );
+        KFu.bayes ( dt.get ( 0, C.rows() - 1 ), dt.get ( C.rows(), dt.length() - 1 ) );
+        Xtu.set_col ( t, KFuep.mean() );
     }
     exec_times ( 0 ) = tt.toc();
@@ -122 +128 @@
 
     it_file fou ( "kalman_stress_res.it" );
-    fou << Name ( "xth" ) << Xt;
+    fou << Name ( "xthu" ) << Xtu;
     fou << Name ( "xth2" ) << Xt2;
     fou << Name ( "xthE" ) << XtE;

library/tests/stresssuite/kalman_stress.m

@@ -60 +60 @@
         Mu(1:2,t)=mu;
 
-        [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
+%       [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
 %       [oxt,oPt,oll(t)] = KalmanSq(oxt,y(:,t),A,C,sQ,sR,oPt);
-        MuK(1:2,t) = Oxt;
+%       MuK(1:2,t) = Oxt;
 %       MuS(1:2,t) = oxt;
 end
@@ -68 +68 @@
 %keyboard
 
-!cd ../;./tests/testKF
-itload('testKF_res.it');
+!./stresssuite kalman_stress
+itload('kalman_stress_res.it');
 
 hold off
@@ -77 +77 @@
 plot(xth2','+');
 plot(xthE','o');
-plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions
+%plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions
 
 exec_times