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

Timestamp:

10/12/09 19:38:54 (15 years ago)

Author:

smidl

Message:

corrections in Kalman and particles

Files:

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

library/bdm/estim/kalman.cpp

@@ -16 +16 @@
         mat &P = est->_R();
         mat& _Ry = fy._R();
+        vec& yp = fy._mu();
         //Time update
         mu = A * mu + B * u;
@@ -24 +25 @@
         _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
-        mu += _K * ( y - C * mu - D * u );
+        yp = C * mu + D * u;
+        mu += _K * ( y - yp );
 
         if ( evalll ) {
-                ll=est->evallog(y);
+                ll=fy.evallog(y);
         }
 };
@@ -65 +67 @@
         mat &P = est->_R();
         mat& _Ry = fy._R();
+        vec& yp = fy._mu();
         
         pfxu->dfdx_cond ( mu, zeros ( dimu ), A, true );
@@ -77 +80 @@
         _K = P * C.transpose() * inv ( _Ry );
         P -= _K * C * P; // P = P -KCP;
-        vec yp = phxu->eval ( mu, u );
+        yp = phxu->eval ( mu, u );
         mu += _K * ( y - yp );