Changeset 51

Timestamp:

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

Author:

smidl

Message:

oprava EKF + debug vypisove fce

Location:

bdm

Files:

• estim/libKF.cpp (modified) (2 diffs)
• estim/libKF.h (modified) (2 diffs)
• math/libDC.h (modified) (1 diff)
• stat/libBM.h (modified) (1 diff)
• stat/libEF.h (modified) (1 diff)

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;

bdm/math/libDC.h

@@ -145 +145 @@
                 void setD (const vec &nD){M=diag(nD);}
                 //! Access functions
+                vec getD (){return diag(M);}
+                //! Access functions
                 void setD (const vec &nD, int i){for(int j=i;j<nD.length();j++){M(j,j)=nD(j-i);}} //Fixme can be more general
 

bdm/stat/libBM.h

@@ -75 +75 @@
         //! generate a list of indeces, i.e. which
         ivec indexlist();
+
+        //!access function
+        Array<std::string>& _names(){return names;};
 };
 

bdm/stat/libEF.h

@@ -110 +110 @@
         //! set cache as inconsistent
         void _cached ( bool what ) {cached=what;}
+        //! access mthod
+        mat getR () {return R.to_mat();}
 };