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

Timestamp:

08/27/09 15:39:31 (15 years ago)

Author:

smidl

Message:

redesign of Kalman to use BM, new object StateSpace? created

Files:

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

library/bdm/estim/kalman.cpp

@@ -6 +6 @@
 using std::endl;
 
-KalmanFull::KalmanFull ( mat A0, mat B0, mat C0, mat D0, mat Q0, mat R0, mat P0, vec mu0 ) {
-        dimx = A0.rows();
-        dimu = B0.cols();
-        dimy = C0.rows();
-
-        bdm_assert_debug ( A0.cols() == dimx, "KalmanFull: A is not square" );
-        bdm_assert_debug ( B0.rows() == dimx, "KalmanFull: B is not compatible" );
-        bdm_assert_debug ( C0.cols() == dimx, "KalmanFull: C is not square" );
-        bdm_assert_debug ( ( D0.rows() == dimy ) || ( D0.cols() == dimu ), "KalmanFull: D is not compatible" );
-        bdm_assert_debug ( ( Q0.cols() == dimx ) || ( Q0.rows() == dimx ), "KalmanFull: Q is not compatible" );
-        bdm_assert_debug ( ( R0.cols() == dimy ) || ( R0.rows() == dimy ), "KalmanFull: R is not compatible" );
-
-        A = A0;
-        B = B0;
-        C = C0;
-        D = D0;
-        R = R0;
-        Q = Q0;
-        mu = mu0;
-        P = P0;
-
-        ll = 0.0;
-        evalll = true;
-}
+
 
 void KalmanFull::bayes ( const vec &dt ) {
@@ -36 +13 @@
         vec u = dt.get ( dimy, dimy + dimu - 1 );
         vec y = dt.get ( 0, dimy - 1 );
+        vec& mu = est->_mu();
+        mat &P = est->_R();
+        mat& _Ry = fy._R();
         //Time update
         mu = A * mu + B * u;
-        P  = A * P * A.transpose() + Q;
+        P  = A * P * A.transpose() + (mat)Q;
 
         //Data update
-        _Ry = C * P * C.transpose() + R;
-        _iRy = inv ( _Ry );
-        _K = P * C.transpose() * _iRy;
+        _Ry = C * P * C.transpose() + (mat)R;
+        _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
         mu += _K * ( y - C * mu - D * u );
 
         if ( evalll ) {
-                //from enorm
-                vec x = y - C * mu - D * u;
-                ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) );
+                ll=est->evallog(y);
         }
 };
 
-std::ostream &operator<< ( std::ostream &os, const KalmanFull &kf ) {
-        os << "mu:" << kf.mu << endl << "P:" << kf.P << endl;
-        return os;
-}
-
 
 
 /////////////////////////////// EKFS
-EKFfull::EKFfull ( ) : BM (), E() {};
+EKFfull::EKFfull ( ) : KalmanFull () {};
 
 void EKFfull::set_parameters ( const shared_ptr<diffbifn> &pfxu0, const shared_ptr<diffbifn> &phxu0, const mat Q0, const mat R0 ) {
@@ -71 +43 @@
         dimy = phxu0->dimension();
         dimu = pfxu0->_dimu();
-        E.epdf::set_parameters ( dimx );
+        est->set_parameters( zeros(dimx), eye(dimx) );
 
         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 );
+        pfxu->dfdx_cond ( est->_mu(), zeros ( dimu ), A, true );
         B.clear();
-        phxu->dfdx_cond ( mu, zeros ( dimu ), C, true );
+        phxu->dfdx_cond ( est->_mu(), zeros ( dimu ), C, true );
         D.clear();
 
@@ -90 +62 @@
         vec u = dt.get ( dimy, dimy + dimu - 1 );
         vec y = dt.get ( 0, dimy - 1 );
-
+        vec &mu = est->_mu();
+        mat &P = est->_R();
+        mat& _Ry = fy._R();
+        
         pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true );
         phxu->dfdx_cond ( mu, zeros ( dimu ), C, true );
@@ -96 +71 @@
         //Time update
         mu = pfxu->eval ( mu, u );// A*mu + B*u;
-        P  = A * P * A.transpose() + Q;
+        P  = A * P * A.transpose() + (mat)Q;
 
         //Data update
-        _Ry = C * P * C.transpose() + R;
-        _iRy = inv ( _Ry );
-        _K = P * C.transpose() * _iRy;
+        _Ry = C * P * C.transpose() + (mat)R;
+        _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
         vec yp = phxu->eval ( mu, u );
         mu += _K * ( y - yp );
 
-        E.set_mu ( mu );
-
         if ( BM::evalll ) {
-                //from enorm
-                vec x = y - yp;
-                BM::ll = -0.5 * ( R.cols() * 1.83787706640935 + log ( det ( _Ry ) ) + x * ( _iRy * x ) );
+                ll=fy.evallog(y);
         }
 };
@@ -119 +89 @@
 void KalmanCh::set_parameters ( const mat &A0, const mat &B0, const mat &C0, const mat &D0, const chmat &Q0, const chmat &R0 ) {
 
-        ( ( Kalman<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 );
+        ( ( StateSpace<chmat>* ) this )->set_parameters ( A0, B0, C0, D0, Q0, R0 );
+        
+        _K=zeros(dimx,dimy);
         // Cholesky special!
+        initialize();
+}
+
+void KalmanCh::initialize(){
         preA = zeros ( dimy + dimx + dimx, dimy + dimx );
 //      preA.clear();
@@ -127 +103 @@
 }
 
-
 void KalmanCh::bayes ( const vec &dt ) {
 
@@ -134 +109 @@
         vec pom ( dimy );
 
+        chmat &_P=est->_R();
+        vec &_mu = est->_mu();
+        mat _K(dimx,dimy);
+        chmat &_Ry=fy._R();
+        vec &_yp = fy._mu();
         //TODO get rid of Q in qr()!
 //      mat Q;
@@ -174 +154 @@
         dimy = phxu0->dimension();
         dimu = 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 );
+        if ( _mu.length() != dimx ) _mu = zeros ( dimx ); 
         A = zeros ( dimx, dimx );
         C = zeros ( dimy, dimx );
@@ -203 +185 @@
         vec u = dt.get ( dimy, dimy + dimu - 1 );
         vec y = dt.get ( 0, dimy - 1 );
-
+        vec &_mu = est->_mu();
+        chmat &_P = est->_R();
+        chmat &_Ry = fy._R();
+        vec &_yp = fy._mu();
+        
         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