[Toon-members] tag/tag kalmanfilter.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/06/05 18:04:57

Modified files:
        tag            : kalmanfilter.h 

Log message:
        using optimized transformCovariance from TooN

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.4&r2=1.5

Patches:
Index: kalmanfilter.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/kalmanfilter.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- kalmanfilter.h      5 Jun 2006 15:19:30 -0000       1.4
+++ kalmanfilter.h      5 Jun 2006 18:04:57 -0000       1.5
@@ -90,7 +90,7 @@
     void predict(double dt){
         TooN::Matrix<State::STATE_DIMENSION> A = model.getJacobian( state, dt 
);
         model.updateState( state, dt );
-        state.covariance = A * state.covariance * A.T() + 
model.getNoiseCovariance( dt );
+        state.covariance = TooN::transformCovariance(A, state.covariance) + 
model.getNoiseCovariance( dt );
         TooN::Symmetrize(state.covariance);
     }
 
@@ -99,7 +99,7 @@
     template<class Measurement> void filter(Measurement & m){
         TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> H = 
m.getMeasurementJacobian( state );
         TooN::Matrix<Measurement::M_DIMENSION> R = m.getMeasurementCovariance( 
state );
-        TooN::Matrix<Measurement::M_DIMENSION> I = H * state.covariance * 
H.T() + R;
+        TooN::Matrix<Measurement::M_DIMENSION> I = 
TooN::transformCovariance(H, state.covariance) + R;
         TooN::LU<Measurement::M_DIMENSION> lu(I);
         TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K = 
state.covariance * H.T() * lu.get_inverse();
         TooN::Vector<Measurement::M_DIMENSION> innovation = m.getInnovation( 
state );