Changeset 1158 for library/bdm/estim/kalman.cpp

Timestamp:

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

Author:

smidl

Message:

Kalman with UD (bierman, thorton) and tests

Files:

• library/bdm/estim/kalman.cpp (modified) (1 diff)

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  );
+}
+
+}