Diff: Estimation/kalman.cpp

Revision:

3:42f3821c4e54

Parent:

2:fbc6e3cf3ed8

--- a/Estimation/kalman.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/Estimation/kalman.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "Matrix.h"
+#include "util.h"
 
 #define DEBUG 1
 
-#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
+//#define clamp360(x) ((((x) < 0) ? 360: 0) + fmod((x), 360))
 
 /*
  * Kalman Filter Setup
@@ -176,17 +177,14 @@
      ***********************************************************************/
     float Hx[2];
     Matrix_Multiply(2,2,1, Hx, H, xp);
-
+    
     //Matrix_print(2,2, H, "6. H");
     //Matrix_print(2,1, x, "6. x");
     //Matrix_print(2,1, Hx, "6. Hx");
     
     float zHx[2];
     Matrix_Subtract(2,1, zHx, z, Hx);
-
-    // At this point we need to be sure to correct heading to -180 to 180 range
-    if (zHx[0] > 180.0)   zHx[0] -= 360.0;
-    if (zHx[0] <= -180.0) zHx[0] += 360.0;
+    zHx[0] = clamp180(zHx[0]);
 
     //Matrix_print(2,1, z, "6. z");
     //Matrix_print(2,1, zHx, "6. zHx");
@@ -198,10 +196,7 @@
     //Matrix_print(2,1, KzHx, "6. KzHx");
     
     Matrix_Add(2,1, x, xp, KzHx);
-
-    // Clamp to 0-360 range
-    while (x[0] < 0) x[0] += 360.0;
-    while (x[0] >= 360.0) x[0] -= 360.0;
+    x[0] = clamp360(x[0]);    // Clamp to 0-360 range
 
     //Matrix_print(2,1, x, "6. x");
 
@@ -223,6 +218,5 @@
     Matrix_Copy(2, 2, P, P2);
 
     //Matrix_print(2,2, P, "7. P");
-
     return x[0];
 }