Changeset 51 for bdm/estim

Timestamp:

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

Author:

smidl

Message:

oprava EKF + debug vypisove fce

Location:

bdm/estim

Files:

• libKF.cpp (modified) (2 diffs)
• libKF.h (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;

bdm/estim/libKF.h

@@ -124 +124 @@
         //!access function
         epdf& _epdf() {return est;}
+        //!access function
+        mat& __K() {return _K;}
+        //!access function
+        vec _dP() {return _P->getD();}
 };
 
@@ -167 +171 @@
 */
 template<class sq_T>
-class EKF : public Kalman<ldmat> {
+class EKF : public Kalman<fsqmat> {
         //! Internal Model f(x,u)
         diffbifn* pfxu;