Changeset 62 for bdm/estim/libKF.cpp

Timestamp:

04/06/08 20:14:56 (16 years ago)

Author:

smidl

Message:

nova simulace s EKFfixed a novy EKF na plnych maticich

Files:

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

bdm/estim/libKF.cpp

@@ -59 +59 @@
 }
 
+
+
+ /////////////////////////////// EKFS
+EKFfull::EKFfull ( RV rvx0, RV rvy0, RV rvu0 ) : BM ( rvx0 ) {};
+
+void EKFfull::set_parameters ( diffbifn* pfxu0,  diffbifn* phxu0,const mat Q0,const mat R0 ) {
+        pfxu = pfxu0;
+        phxu = phxu0;
+
+        dimx = rv.count();
+        dimy = phxu0->_dimy();
+        dimu = phxu0->_dimu();
+
+        A.set_size(dimx,dimx);
+        C.set_size(dimy,dimx);
+        //initialize matrices A C, later, these will be only updated!
+        pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true );
+        B.clear();
+        phxu->dfdx_cond ( mu,zeros ( dimu ),C,true );
+        D.clear();
+
+        R = R0;
+        Q = Q0;
+}
+
+void EKFfull::bayes ( const vec &dt ) {
+        it_assert_debug ( dt.length() == ( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
+
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
+        
+        pfxu->dfdx_cond ( mu,zeros ( dimu ),A,true );
+        phxu->dfdx_cond ( mu,zeros ( dimu ),C,true );
+
+        //Time update
+        mu = pfxu->eval(mu,u);// A*mu + B*u;
+        P  = A*P*A.transpose() + Q;
+
+        //Data update
+        _Ry = C*P*C.transpose() + R;
+        _iRy = inv ( _Ry );
+        _K = P*C.transpose() *_iRy;
+        P -= _K*C*P; // P = P -KCP;
+        vec yp = phxu->eval(mu,u);
+        mu += _K* ( y-yp );
+
+        if ( BM::evalll ) {
+                //from enorm
+                vec x=y-yp;
+                BM::ll = -0.5* ( R.cols() * 1.83787706640935 +log ( det ( _Ry ) ) +x* ( _iRy*x ) );
+        }
+};
+
+
+
 void KalmanCh::set_parameters ( const mat &A0,const mat &B0,const mat &C0,const mat &D0,const chmat &R0,const chmat &Q0 ) {
 
@@ -138 +193 @@
         preA.set_submatrix ( dimy,dimy, ( _P->_Ch() ) *A.T() );
 
+//      mat Sttm = _P->to_mat();
+
 //      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();
+        _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 );
+        
+//      mat iRY = inv(_Ry->to_mat());
+//      mat Stt = Sttm - Sttm * C.T() * iRY * C * Sttm;
+//      mat _K2 = Stt*C.T()*inv(R.to_mat());
+
+        // prediction
+        *_mu = pfxu->eval ( *_mu ,u );
+
         *_yp = phxu->eval ( *_mu,u );
         
-        *_mu = pfxu->eval ( *_mu ,u ) + ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );
+/*      vec mu2 = *_mu + ( _K2 ) * ( y-*_yp );*/
+        
+        //correction //= initial value is already prediction!
+        *_mu += ( _K ) * ( _iRy->_Ch() ).T() * ( y-*_yp );
+
 /*      _K = (_P->to_mat())*C.transpose() * ( _iRy->to_mat() );
         *_mu = pfxu->eval ( *_mu ,u ) + ( _K )* ( y-*_yp );*/
@@ -173 +242 @@
         R.setD ( R0 );
 };
+
+