>From 49ab2b8396dff5f0773e8b560297dae7037dc538 Mon Sep 17 00:00:00 2001
From: Ryan Pavlik <address@hidden>
Date: Wed, 5 May 2010 18:10:51 -0500
Subject: [PATCH 1/3] Compile fixes to kalman filter

---
 tag/kalmanfilter.h |    7 +++++--
 1 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/tag/kalmanfilter.h b/tag/kalmanfilter.h
index 71bdc7d..1aeb62d 100644
--- a/tag/kalmanfilter.h
+++ b/tag/kalmanfilter.h
@@ -95,7 +95,8 @@ public:
     /// predicts the state by applying the process model over the time interval dt
     /// @param[in] dt time interval
     void predict(double dt){
-        TooN::transformCovariance(model.getJacobian( state, dt ), state.covariance, state.covariance);
+        TooN::Matrix<state_type::STATE_DIMENSION> origCov(state.covariance);
+        TooN::transformCovariance(model.getJacobian( state, dt ), origCov, state.covariance);
         state.covariance += model.getNoiseCovariance( dt );
         TooN::Symmetrize(state.covariance);
         model.updateState( state, dt );
@@ -106,7 +107,9 @@ public:
     template<class Measurement> void filter(Measurement & m){
         const TooN::Matrix<Measurement::M_DIMENSION,State::STATE_DIMENSION> & H = m.getMeasurementJacobian( state );
         const TooN::Matrix<Measurement::M_DIMENSION> & R = m.getMeasurementCovariance( state );
-        TooN::Matrix<Measurement::M_DIMENSION> I = TooN::transformCovariance(H, state.covariance) + R;
+        TooN::Matrix<Measurement::M_DIMENSION> I = TooN::Zeros;
+        TooN::transformCovariance(H, state.covariance, I);
+        I += R;
         TooN::LU<Measurement::M_DIMENSION> lu(I);
         TooN::Matrix<State::STATE_DIMENSION, Measurement::M_DIMENSION> K = state.covariance * H.T() * lu.get_inverse();
         const TooN::Vector<Measurement::M_DIMENSION> & innovation = m.getInnovation( state );
-- 
1.7.1