From 56afbd0461681a9cf82747cbc38608d2e95e0f3a Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Mon, 22 Jul 2013 15:07:32 +0200
Subject: [PATCH] Log all steps of Kalman Filter

---
 lib/EKF.js     | 12 ++++++----
 lib/Mission.js | 64 ++++++++++++++++++++++++++++++++++----------------
 2 files changed, 52 insertions(+), 24 deletions(-)

diff --git a/lib/EKF.js b/lib/EKF.js
index e0ea610..dab07d7 100644
--- a/lib/EKF.js
+++ b/lib/EKF.js
@@ -29,7 +29,7 @@ EKF.prototype.reset = function() {
   this._state       = this._options.state   || {x: 0, y: 0, yaw: 0};
   this._sigma       = Matrix.I(3);
   this._q           = Matrix.Diagonal([0.0003, 0.0003, 0.0001]);
-  this._r           = Matrix.Diagonal([0.3, 0.3, 0.1]);
+  this._r           = Matrix.Diagonal([0.3, 0.3, 0.3]);
   this._last_yaw    = null;
 }
 
@@ -85,18 +85,22 @@ EKF.prototype.correct = function(measure, pose) {
     // Compute expected measurement given our current state and the marker pose
     var state  = this._state;
     var psi    = state.yaw;
+    this._s    = {x: state.x, y: state.y, yaw: state.yaw};
 
     // Normalized the measure yaw
     measure.yaw = normAngle(measure.yaw);
+    this._m = {x: measure.x, y: measure.y, yaw: measure.yaw};
 
-    var z1     = Math.cos(psi) * (pose.x - state.x) + Math.sin(psi) * (pose.y - state.y);
-    var z2     = -1 * Math.sin(psi) * (pose.x - state.x) + Math.cos(psi) * (pose.y - state.y);
-    var z3     = pose.yaw - psi;
+    var z1 = Math.cos(psi) * (pose.x - state.x) + Math.sin(psi) * (pose.y - state.y);
+    var z2 = -1 * Math.sin(psi) * (pose.x - state.x) + Math.cos(psi) * (pose.y - state.y);
+    var z3 = pose.yaw - psi;
+    this._z = {x: z1, y: z2, yaw: z3};
 
     // Compute the error
     var e1 = measure.x - z1;
     var e2 = measure.y - z2;
     var e3 = measure.yaw - z3;
+    this._e = {x: e1, y: e2, yaw: e3};
 
     // Compute the H term
     var H = $M([[ -Math.cos(psi), -Math.sin(psi), Math.sin(psi) * (state.x - pose.x) - Math.cos(psi) * (state.y - pose.y)],
diff --git a/lib/Mission.js b/lib/Mission.js
index 3e1afba..d793d5b 100644
--- a/lib/Mission.js
+++ b/lib/Mission.js
@@ -28,27 +28,51 @@ Mission.prototype.run = function(callback) {
 
 Mission.prototype.log = function(path) {
     var dataStream = fs.createWriteStream(path);
+    var ekf = this._control._ekf;
+
     this._control.on('controlData', function(d) {
-        dataStream.write(d.state.x + "," +
-                        d.state.y + "," +
-                        d.state.z + "," +
-                        d.state.yaw + "," +
-                        d.state.vx + "," +
-                        d.state.vy + "," +
-                        d.goal.x + "," +
-                        d.goal.y + "," +
-                        d.goal.z + "," +
-                        d.goal.yaw + "," +
-                        d.error.ex + "," +
-                        d.error.ey + "," +
-                        d.error.ez + "," +
-                        d.error.eyaw + "," +
-                        d.control.ux + "," +
-                        d.control.uy + "," +
-                        d.control.uz + "," +
-                        d.control.uyaw + "," +
-                        d.last_ok + "," +
-                        d.tag + "\n");
+        var log = (d.state.x + "," +
+                   d.state.y + "," +
+                   d.state.z + "," +
+                   d.state.yaw + "," +
+                   d.state.vx + "," +
+                   d.state.vy + "," +
+                   d.goal.x + "," +
+                   d.goal.y + "," +
+                   d.goal.z + "," +
+                   d.goal.yaw + "," +
+                   d.error.ex + "," +
+                   d.error.ey + "," +
+                   d.error.ez + "," +
+                   d.error.eyaw + "," +
+                   d.control.ux + "," +
+                   d.control.uy + "," +
+                   d.control.uz + "," +
+                   d.control.uyaw + "," +
+                   d.last_ok + "," +
+                   d.tag);
+
+        if (d.tag > 0) {
+            log = log + "," +
+                  ekf._s.x + "," +
+                  ekf._s.y + "," +
+                  ekf._s.yaw.toDeg() + "," +
+                  ekf._m.x + "," +
+                  ekf._m.y + "," +
+                  ekf._m.yaw.toDeg() + "," +
+                  ekf._z.x + "," +
+                  ekf._z.y + "," +
+                  ekf._z.yaw.toDeg() + "," +
+                  ekf._e.x + "," +
+                  ekf._e.y + "," +
+                  ekf._e.yaw.toDeg()
+        } else {
+            log = log + ",0,0,0,0,0,0"
+        }
+
+        log = log + "\n";
+
+        dataStream.write(log);
     });
 }