switched to angles for normalization. still wrong.

mkoval · May 17, 2012 · 579c5ad · 579c5ad
1 parent d6b5bb5
commit 579c5ad
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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]);
 }
 
 };