Changeset 679 for library/bdm/estim/kalman.cpp

Timestamp:

10/23/09 00:05:25 (15 years ago)

Author:

smidl

Message:

Major changes in BM -- OK is only test suite and tests/tutorial -- the rest is broken!!!

Files:

• library/bdm/estim/kalman.cpp (modified) (10 diffs)

library/bdm/estim/kalman.cpp

@@ -8 +8 @@
 
 
-void KalmanFull::bayes ( const vec &dt ) {
-        bdm_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& mu = est->_mu();
-        mat &P = est->_R();
+void KalmanFull::bayes ( const vec &yt, const vec &cond ) {
+        bdm_assert_debug ( yt.length() == ( dimy ), "KalmanFull::bayes wrong size of dt" );
+
+        const vec &u = cond; // in this case cond=ut
+        const vec &y = yt;
+        
+        vec& mu = est._mu();
+        mat &P = est._R();
         mat& _Ry = fy._R();
         vec& yp = fy._mu();
@@ -42 +43 @@
         phxu = phxu0;
 
-        dimx = pfxu0->_dimx();
+        set_dim( pfxu0->_dimx());
         dimy = phxu0->dimension();
-        dimu = pfxu0->_dimu();
-        est->set_parameters( zeros(dimx), eye(dimx) );
-
-        A.set_size ( dimx, dimx );
-        C.set_size ( dimy, dimx );
+        dimc = pfxu0->_dimu();
+        est.set_parameters( zeros(dimension()), eye(dimension()) );
+
+        A.set_size ( dimension(), dimension() );
+        C.set_size ( dimy, dimension() );
         //initialize matrices A C, later, these will be only updated!
-        pfxu->dfdx_cond ( est->_mu(), zeros ( dimu ), A, true );
+        pfxu->dfdx_cond ( est._mu(), zeros ( dimc ), A, true );
         B.clear();
-        phxu->dfdx_cond ( est->_mu(), zeros ( dimu ), C, true );
+        phxu->dfdx_cond ( est._mu(), zeros ( dimc ), C, true );
         D.clear();
 
@@ -59 +60 @@
 }
 
-void EKFfull::bayes ( const vec &dt ) {
-        bdm_assert_debug ( dt.length() == ( dimy + dimu ), "EKFull::bayes wrong size of dt" );
-
-        vec u = dt.get ( dimy, dimy + dimu - 1 );
-        vec y = dt.get ( 0, dimy - 1 );
-        vec &mu = est->_mu();
-        mat &P = est->_R();
+void EKFfull::bayes ( const vec &yt, const vec &cond) {
+        bdm_assert_debug ( yt.length() == ( dimy ), "EKFull::bayes wrong size of dt" );
+        bdm_assert_debug ( cond.length() == ( dimc ), "EKFull::bayes wrong size of dt" );
+        
+        const vec &u = cond;
+        const vec &y = yt; //lazy to change it
+        vec &mu = est._mu();
+        mat &P = est._R();
         mat& _Ry = fy._R();
         vec& yp = fy._mu();
         
-        pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true );
-        phxu->dfdx_cond ( mu, zeros ( dimu ), C, true );
+        pfxu->dfdx_cond ( mu, zeros ( dimc ), A, true );
+        phxu->dfdx_cond ( mu, zeros ( dimc ), C, true );
 
         //Time update
@@ -94 +96 @@
         ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 );
         
-        _K=zeros(dimx,dimy);
-        // Cholesky special!
-        initialize();
+        _K=zeros(dimension(),dimy);
 }
 
 void KalmanCh::initialize(){
-        preA = zeros ( dimy + dimx + dimx, dimy + dimx );
+        preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() );
 //      preA.clear();
         preA.set_submatrix ( 0, 0, R._Ch() );
-        preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
-}
-
-void KalmanCh::bayes ( const vec &dt ) {
-
-        vec u = dt.get ( dimy, dimy + dimu - 1 );
-        vec y = dt.get ( 0, dimy - 1 );
+        preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() );
+}
+
+void KalmanCh::bayes ( const vec &yt, const vec &cond ) {
+        bdm_assert_debug(yt.length()==dimy, "yt mismatch");
+        bdm_assert_debug(cond.length()==dimc, "yt mismatch");
+        
+        const vec &u = cond;
+        const vec &y = yt;
         vec pom ( dimy );
 
-        chmat &_P=est->_R();
-        vec &_mu = est->_mu();
-        mat _K(dimx,dimy);
+        chmat &_P=est._R();
+        vec &_mu = est._mu();
+        mat _K(dimension(),dimy);
         chmat &_Ry=fy._R();
         vec &_yp = fy._mu();
@@ -130 +132 @@
 
         ( _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 );
+        _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T();
+        ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 );
 
         _mu = A * ( _mu ) + B * u;
@@ -154 +156 @@
         phxu = phxu0;
 
-        dimx = pfxu0->dimension();
+        set_dim( pfxu0->_dimx());
         dimy = phxu0->dimension();
-        dimu = pfxu0->_dimu();
-        
-        vec &_mu = est->_mu();
+        dimc = pfxu0->_dimu();
+        
+        vec &_mu = est._mu();
         // if mu is not set, set it to zeros, just for constant terms of A and C
-        if ( _mu.length() != dimx ) _mu = zeros ( dimx ); 
-        A = zeros ( dimx, dimx );
-        C = zeros ( dimy, dimx );
-        preA = zeros ( dimy + dimx + dimx, dimy + dimx );
+        if ( _mu.length() != dimension() ) _mu = zeros ( dimension() ); 
+        A = zeros ( dimension(), dimension() );
+        C = zeros ( dimy, dimension() );
+        preA = zeros ( dimy + dimension() + dimension(), dimy + dimension() );
 
         //initialize matrices A C, later, these will be only updated!
-        pfxu->dfdx_cond ( _mu, zeros ( dimu ), A, true );
+        pfxu->dfdx_cond ( _mu, zeros ( dimc ), 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 ( dimc ), C, true );
 //      phxu->dfdu_cond ( *_mu,zeros ( dimu ),D,true );
         D.clear();
@@ -179 +181 @@
         preA.clear();
         preA.set_submatrix ( 0, 0, R._Ch() );
-        preA.set_submatrix ( dimy + dimx, dimy, Q._Ch() );
-}
-
-
-void EKFCh::bayes ( const vec &dt ) {
+        preA.set_submatrix ( dimy + dimension(), dimy, Q._Ch() );
+}
+
+
+void EKFCh::bayes ( const vec &yt, const vec &cond ) {
 
         vec pom ( dimy );
-        vec u = dt.get ( dimy, dimy + dimu - 1 );
-        vec y = dt.get ( 0, dimy - 1 );
-        vec &_mu = est->_mu();
-        chmat &_P = est->_R();
+        const vec &u = cond;
+        const vec &y = yt;
+        vec &_mu = est._mu();
+        chmat &_P = est._R();
         chmat &_Ry = fy._R();
         vec &_yp = fy._mu();
@@ -211 +213 @@
 
         ( _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 );
+        _K = inv ( A ) * ( postA ( 0, dimy - 1 , dimy, dimy + dimension() - 1 ) ).T();
+        ( _P._Ch() ) = postA ( dimy, dimy + dimension() - 1, dimy, dimy + dimension() - 1 );
 
 //      mat iRY = inv(_Ry->to_mat());
@@ -268 +270 @@
         //connect
         shared_ptr<RV> drv = UI::build<RV> ( set, "drv", UI::compulsory );
-        set_drv ( *drv );
+        set_yrv ( *drv );
         shared_ptr<RV> rv = UI::build<RV> ( set, "rv", UI::compulsory );
         set_rv ( *rv );
@@ -282 +284 @@
 
         set_parameters ( A );
-        set_drv ( A ( 0 )->_drv() );
+        set_yrv ( A ( 0 )->_yrv() );
         //set_rv(A(0)->_rv());