Changeset 32 for bdm/estim/libKF.cpp

Timestamp:

03/03/08 13:00:32 (16 years ago)

Author:

smidl

Message:

test KF : estimation of R in KF is not possible! Likelihood of y_t is growing when R -> 0

Files:

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

bdm/estim/libKF.cpp

@@ -6 +6 @@
 using std::endl;
 
-KalmanFull::KalmanFull( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
+KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat R0, mat Q0, mat P0, vec mu0 ) {
         dimx = A0.rows();
         dimu = B0.cols();
         dimy = C0.rows();
 
-        it_assert_debug( A0.cols()==dimx, "KalmanFull: A is not square" );
-        it_assert_debug( B0.rows()==dimx, "KalmanFull: B is not compatible" );
-        it_assert_debug( C0.cols()==dimx, "KalmanFull: C is not square" );
-        it_assert_debug(( D0.rows()==dimy ) || ( D0.cols()==dimu ),     "KalmanFull: D is not compatible" );
-        it_assert_debug(( R0.cols()==dimy ) || ( R0.rows()==dimy ), "KalmanFull: R is not compatible" );
-        it_assert_debug(( Q0.cols()==dimx ) || ( Q0.rows()==dimx ), "KalmanFull: Q is not compatible" );
+        it_assert_debug ( A0.cols() ==dimx, "KalmanFull: A is not square" );
+        it_assert_debug ( B0.rows() ==dimx, "KalmanFull: B is not compatible" );
+        it_assert_debug ( C0.cols() ==dimx, "KalmanFull: C is not square" );
+        it_assert_debug ( ( D0.rows() ==dimy ) || ( D0.cols() ==dimu ), "KalmanFull: D is not compatible" );
+        it_assert_debug ( ( R0.cols() ==dimy ) || ( R0.rows() ==dimy ), "KalmanFull: R is not compatible" );
+        it_assert_debug ( ( Q0.cols() ==dimx ) || ( Q0.rows() ==dimx ), "KalmanFull: Q is not compatible" );
 
         A = A0;
@@ -28 +28 @@
 }
 
-void KalmanFull::bayes( const vec &dt) {
-        it_assert_debug( dt.length()==( dimy+dimu ),"KalmanFull::bayes wrong size of dt" );
+void KalmanFull::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 );
+        vec u = dt.get ( dimy,dimy+dimu-1 );
+        vec y = dt.get ( 0,dimy-1 );
         //Time update
         mu = A*mu + B*u;
@@ -39 +39 @@
         //Data update
         _Ry = C*P*C.transpose() + R;
-        _iRy = inv( _Ry );
-        _K = P*C.transpose()*_iRy;
+        _iRy = inv ( _Ry );
+        _K = P*C.transpose() *_iRy;
         P -= _K*C*P; // P = P -KCP;
-        mu += _K*( y-C*mu-D*u );
+        mu += _K* ( y-C*mu-D*u );
 };
 
@@ -50 +50 @@
 }
 
+void KFcondQR::condition ( const vec &QR ) {
+        it_assert_debug ( QR.length() == ( rvc.count() ),"KFcondRQ: conditioning by incompatible vector" );
+
+        Q.setD ( QR ( 0, dimx-1 ));
+        R.setD ( QR ( dimx, -1 ));
+};
+
+void KFcondR::condition ( const vec &R0 ) {
+        it_assert_debug ( R0.length() == ( rvc.count() ),"KFcondR: conditioning by incompatible vector" );
+
+        R.setD ( R0 );
+};