Changeset 8 for libKF.h

Timestamp:

01/23/08 11:32:31 (16 years ago)

Author:

smidl

Message:

Kalmany funkci, PF nefunkci

Files:

• libKF.h (modified) (5 diffs)

libKF.h

@@ -22 +22 @@
 
 /*!
-* \brief Basic Kalman filter with full matrices (education purpose only)!
+* \brief Basic Kalman filter with full matrices (education purpose only)! Will be deleted soon!
 */
 class KalmanFull : public BM { 
@@ -58 +58 @@
         
         //cache
-        mat _K, _yp;
+        mat _K;
+        vec _yp;
         sq_T _Ry,_iRy;
 public:
@@ -103 +104 @@
         ll = 0;
 //Fixme should we assign cache??
+        _iRy = eye(dimy); // needed in inv(_iRy)
 }
 
@@ -114 +116 @@
         mu = A*mu + B*u;
         //P  = A*P*A.transpose() + Q; in sq_T
-        P.mult_qform( A );
+        P.mult_sym( A );
         P+=Q;
 
         //Data update
         //_Ry = C*P*C.transpose() + R; in sq_T
-        _Ry.mult_qform( C );
+        _Ry.mult_sym( C, P);
         _Ry+=R;
 
@@ -131 +133 @@
         
         if (evalll==true) {
-        ll+= -0.5*_Ry.logdet() -0.5*_iRy.qform(_yp);
+        ll+= -0.5*(_Ry.cols()*0.79817986835811504957 \
+        +_Ry.logdet() +_iRy.qform(_yp));
         }
 };