Changeset 83 for bdm/estim/libKF.cpp

Timestamp:

04/21/08 16:03:29 (16 years ago)

Author:

smidl

Message:

oprava Kalmana pro nove enorm

Files:

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

bdm/estim/libKF.cpp

@@ -130 +130 @@
         vec u = dt.get ( dimy,dimy+dimu-1 );
         vec y = dt.get ( 0,dimy-1 );
-
+        vec pom(dimy);
+        
         //TODO get rid of Q in qr()!
 //      mat Q;
 
         //R and Q are already set in set_parameters()
-        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
+        preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() );
         //Fixme can be more efficient if .T() is not used
-        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );
+        preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() );
 
 //      if ( !qr ( preA,Q,postA ) ) it_warning ( "QR in kalman unstable!" );
         if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );
 
-        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
-        _K = ( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
-        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
-        _Ry->inv ( *_iRy );
-        fy._cached ( true );
-        *_mu = A* ( *_mu ) + B*u + ( _K ) * ( _iRy->_Ch() ).T() * ( y-C* ( *_mu )-D*u );
-
-        /*      cout << "P:" <<_P->to_mat() <<endl;
-                cout << "Ry:" <<_Ry->to_mat() <<endl;
-                cout << "iRy:" <<_iRy->to_mat() <<endl;*/
+        ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
+        _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
+        ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
+        
+        _mu = A* ( _mu ) + B*u;
+        _yp = C*_mu -D*u;
+        
+        backward_substitution(_Ry._Ch(),( y-_yp),pom);
+        _mu +=  ( _K ) * pom;
+
+                cout << "P:" <<_P.to_mat() <<endl;
+                cout << "Ry:" <<_Ry.to_mat() <<endl;
+                cout << "_K:" <<_K <<endl;
+        
         if ( evalll==true ) { //likelihood of observation y
                 ll=fy.evalpdflog ( y );
@@ -165 +170 @@
 
         //initialize matrices A C, later, these will be only updated!
-        pfxu->dfdx_cond ( *_mu,zeros ( dimu ),A,true );
+        pfxu->dfdx_cond ( _mu,zeros ( dimu ),A,true );
 //      pfxu->dfdu_cond ( *_mu,zeros ( dimu ),B,true );
         B.clear();
-        phxu->dfdx_cond ( *_mu,zeros ( dimu ),C,true );
+        phxu->dfdx_cond ( _mu,zeros ( dimu ),C,true );
 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
         D.clear();
@@ -184 +189 @@
 void EKFCh::bayes ( const vec &dt ) {
 
-        vec u = dt.get ( dimy,dimy+dimu-1 );
-        vec y = dt.get ( 0,dimy-1 );
-
-        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
+        vec pom(dimy);
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
+
+        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()
-        preA.set_submatrix ( dimy,0, ( _P->_Ch() ) *C.T() );
+        preA.set_submatrix ( dimy,0, ( _P._Ch() ) *C.T() );
         //Fixme can be more efficient if .T() is not used
-        preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );
+        preA.set_submatrix ( dimy,dimy, ( _P._Ch() ) *A.T() );
 
 //      mat Sttm = _P->to_mat();
@@ -200 +206 @@
         if ( !qr ( preA,postA ) ) it_warning ( "QR in kalman unstable!" );
 
-        ( _Ry->_Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
+        ( _Ry._Ch() ) =postA ( 0,dimy-1, 0,dimy-1 );
         _K = inv(A)*( postA ( 0,dimy-1 ,dimy,dimy+dimx-1 ) ).T();
-        ( _P->_Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
-        _Ry->inv ( *_iRy );
-        fy._cached ( true );
+        ( _P._Ch() ) = postA ( dimy,dimy+dimx-1,dimy,dimy+dimx-1 );
         
 //      mat iRY = inv(_Ry->to_mat());
@@ -211 +215 @@
 
         // prediction
-        *_mu = pfxu->eval ( *_mu ,u );
-
-        *_yp = phxu->eval ( *_mu,u );
+        _mu = pfxu->eval ( _mu ,u );
+        _yp = phxu->eval ( _mu,u );
         
 /*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/
         
         //correction //= initial value is already prediction!
-        *_mu += ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );
+        backward_substitution(_Ry._Ch(),( y-_yp ),pom);
+        _mu += ( _K ) *pom ;
 
 /*      _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;
-                cout << "iRy:" <<_iRy->to_mat() <<endl;*/
+                cout << "P:" <<_P.to_mat() <<endl;
+                cout << "Ry:" <<_Ry.to_mat() <<endl;
+                cout << "_K:" <<_K <<endl;
 
         if ( evalll==true ) { //likelihood of observation y