From 579c5ad9fbca3ac4351d6be4ac09cc3c238892dd Mon Sep 17 00:00:00 2001
From: Michael Koval <koval.michael@gmail.com>
Date: Thu, 17 May 2012 02:25:37 -0400
Subject: [PATCH] switched to angles for normalization. still wrong.

---
 src/robot_kf.cc | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/robot_kf.cc b/src/robot_kf.cc
index 802fc45..75a3fdd 100644
--- a/src/robot_kf.cc
+++ b/src/robot_kf.cc
@@ -2,6 +2,7 @@
 #include <Eigen/Core>
 #include <Eigen/Dense>
 #include <Eigen/LU>
+#include <angles/angles.h>
 #include <robot_kf/robot_kf.h>
 
 using namespace Eigen;
@@ -77,7 +78,7 @@ void KalmanFilter::measure(Matrix<double, m, 1> z, Matrix<double, m, m> cov_z,
 }
 
 void KalmanFilter::normalize_yaw(void) {
-    x_[2] = fmod(x_[2], 2 * M_PI);
+    x_[2] = angles::normalize_angle(x_[2]);
 }
 
 };