Changeset 51 for bdm/estim/libKF.cpp

Timestamp:

03/25/08 13:37:45 (16 years ago)

Author:

smidl

Message:

oprava EKF + debug vypisove fce

Files:

• bdm/estim/libKF.cpp (modified) (2 diffs)

bdm/estim/libKF.cpp

@@ -130 +130 @@
         vec y = dt.get ( 0,dimy-1 );
 
-        //TODO get rid of Q in qr()!
-//      mat Q;
+        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
 
         //R and Q are already set in set_parameters()
@@ -147 +147 @@
         fy._cached ( true );
         *_yp = phxu->eval ( *_mu,u );
+        
         *_mu = pfxu->eval ( *_mu ,u ) + ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );
-
+/*      _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() );
+        *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/
+        
         /*      cout << "P:" <<_P->to_mat() <<endl;
                 cout << "Ry:" <<_Ry->to_mat() <<endl;