Changeset 83

Timestamp:

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

Author:

smidl

Message:

oprava Kalmana pro nove enorm

Location:

bdm/estim

Files:

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

bdm/estim/libKF.h

@@ -95 +95 @@
         mat _K;
         //! cache of fy.mu
-        vec* _yp;
+        vec& _yp;
         //! cache of fy.R
-        sq_T* _Ry;
-        //! cache of fy.iR
-        sq_T* _iRy;
+        sq_T& _Ry;
         //!cache of est.mu
-        vec* _mu;
+        vec& _mu;
         //!cache of est.R
-        sq_T* _P;
-        //!cache of est.iR
-        sq_T* _iP;
+        sq_T& _P;
 
 public:
@@ -266 +262 @@
                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
                 Q(dimx), R(dimy),
-                est ( rv ), fy ( rvy ) {
+                est ( rv ), fy ( rvy ), _mu(est._mu()), _P(est._R()), _yp(fy._mu()),_Ry(fy._R()) {
 
         this->set_parameters ( K0.A, K0.B, K0.C, K0.D, K0.R, K0.Q );
 
-//establish pointers
-        _mu = est._mu();
-        est._R ( _P,_iP );
-
-//fy
-        _yp = fy._mu();
-        fy._R ( _Ry,_iRy );
-
 // copy values in pointers
-        *_mu = *K0._mu;
-        *_P = *K0._P;
-        *_iP = *K0._iP;
-        *_yp = *K0._yp;
-        *_iRy = *K0._iRy;
-        *_Ry = *K0._Ry;
+        _mu = K0._mu;
+        _P = K0._P;
+        _yp = K0._yp;
+        _Ry = K0._Ry;
 
 }
@@ -293 +279 @@
                 A ( dimx,dimx ), B ( dimx,dimu ), C ( dimy,dimx ), D ( dimy,dimu ),
                 Q(dimx), R (dimy),
-                est ( rvx ), fy ( rvy ) {
-//assign cache
-//est
-        _mu = est._mu();
-        est._R ( _P,_iP );
-
-//fy
-        _yp = fy._mu();
-        fy._R ( _Ry,_iRy );
+                est ( rvx ), fy ( rvy ), _mu(est._mu()), _P(est._R()), _yp(fy._mu()),_Ry(fy._R()) {
 };
 
@@ -325 +303 @@
         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
 
+        sq_T iRy(dimy);
         vec u = dt.get ( dimy,dimy+dimu-1 );
         vec y = dt.get ( 0,dimy-1 );
         //Time update
-        *_mu = A* ( *_mu ) + B*u;
+        _mu = A* _mu + B*u;
         //P  = A*P*A.transpose() + Q; in sq_T
-        _P->mult_sym ( A );
-        ( *_P ) +=Q;
+        _P.mult_sym ( A );
+        _P  +=Q;
 
         //Data update
         //_Ry = C*P*C.transpose() + R; in sq_T
-        _P->mult_sym ( C, *_Ry );
-        ( *_Ry ) +=R;
-
-        mat Pfull = _P->to_mat();
-
-        _Ry->inv ( *_iRy ); // result is in _iRy;
-        fy._cached ( true );
-        _K = Pfull*C.transpose() * ( _iRy->to_mat() );
+        _P.mult_sym ( C, _Ry );
+        _Ry  +=R;
+
+        mat Pfull = _P.to_mat();
+
+        _Ry.inv ( iRy ); // result is in _iRy;
+        _K = Pfull*C.transpose() * ( iRy.to_mat() );
 
         sq_T pom ( ( int ) Pfull.rows() );
-        _iRy->mult_sym_t ( C*Pfull,pom );
-        ( *_P ) -= pom; // P = P -PC'iRy*CP;
-        ( *_yp ) = C* ( *_mu ) +D*u; //y prediction
-        ( *_mu ) += _K* ( y- ( *_yp ) );
+        iRy.mult_sym_t ( C*Pfull,pom );
+        (_P ) -= pom; // P = P -PC'iRy*CP;
+        (_yp ) = C* _mu  +D*u; //y prediction
+        (_mu ) += _K* ( y- _yp  );
 
 
@@ -372 +350 @@
 
         //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();
@@ -387 +365 @@
         it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
 
+        sq_T iRy(dimy,dimy);
         vec u = dt.get ( dimy,dimy+dimu-1 );
         vec y = dt.get ( 0,dimy-1 );
         //Time update
-        *_mu = pfxu->eval ( *_mu, u );
-        pfxu->dfdx_cond ( *_mu,u,A,false ); //update A by a derivative of fx
+        _mu = pfxu->eval ( _mu, u );
+        pfxu->dfdx_cond ( _mu,u,A,false ); //update A by a derivative of fx
 
         //P  = A*P*A.transpose() + Q; in sq_T
-        _P->mult_sym ( A );
-        ( *_P ) +=Q;
+        _P.mult_sym ( A );
+        _P +=Q;
 
         //Data update
-        phxu->dfdx_cond ( *_mu,u,C,false ); //update C by a derivative hx
+        phxu->dfdx_cond ( _mu,u,C,false ); //update C by a derivative hx
         //_Ry = C*P*C.transpose() + R; in sq_T
-        _P->mult_sym ( C, *_Ry );
-        ( *_Ry ) +=R;
-
-        mat Pfull = _P->to_mat();
-
-        _Ry->inv ( *_iRy ); // result is in _iRy;
-        fy._cached ( true );
-        _K = Pfull*C.transpose() * ( _iRy->to_mat() );
+        _P.mult_sym ( C, _Ry );
+        ( _Ry ) +=R;
+
+        mat Pfull = _P.to_mat();
+
+        _Ry.inv ( iRy ); // result is in _iRy;
+        _K = Pfull*C.transpose() * ( iRy.to_mat() );
 
         sq_T pom ( ( int ) Pfull.rows() );
-        _iRy->mult_sym_t ( C*Pfull,pom );
-        ( *_P ) -= pom; // P = P -PC'iRy*CP;
-        *_yp = phxu->eval ( *_mu,u ); //y prediction
-        ( *_mu ) += _K* ( y-*_yp );
+        iRy.mult_sym_t ( C*Pfull,pom );
+        (_P ) -= pom; // P = P -PC'iRy*CP;
+        _yp = phxu->eval ( _mu,u ); //y prediction
+        ( _mu ) += _K* ( y-_yp );
 
         if ( evalll==true ) {ll+=fy.evalpdflog ( y );}