From 8d79ad7bd05667d834cc56bd031589e6e6599a31 Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Fri, 19 Jul 2013 09:15:50 +0200
Subject: [PATCH] Do not correct yaw in EKF

A temporary fix since it appears the yaw correction does
not work as expected
---
 lib/EKF.js | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/lib/EKF.js b/lib/EKF.js
index aac81f8..e0ea610 100644
--- a/lib/EKF.js
+++ b/lib/EKF.js
@@ -112,7 +112,10 @@ EKF.prototype.correct = function(measure, pose) {
     var c = K . multiply(err);
     state.x = state.x + c.e(1);
     state.y = state.y + c.e(2);
-    state.yaw = state.yaw + c.e(3);
+
+//  TODO - This does not work, need more investigation.
+//  In the meanwhile, we don't correct yaw based on observation.
+//  state.yaw = state.yaw + c.e(3);
 
     this._sigma = Matrix.I(3).subtract(K.multiply(H)).multiply(this._sigma);
 };