Using angles library to normalize angles (#739)

* Using angles library for innovation angle normalization

Author: firemark

package.xml

@@ -28,6 +28,7 @@
   <depend>geographiclib</depend>
   <depend>message_filters</depend>
   <depend>nav_msgs</depend>
+  <depend>angles</depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>rmw_implementation</build_depend>
   <depend>sensor_msgs</depend>

src/ekf.cpp

@@ -33,6 +33,7 @@
 
 #include <robot_localization/ekf.hpp>
 #include <robot_localization/filter_common.hpp>
+#include <angles/angles.h>
 #include <Eigen/Dense>
 #include <rclcpp/duration.hpp>
 #include <vector>
@@ -177,13 +178,7 @@ void Ekf::correct(const Measurement & measurement)
       update_indices[i] == StateMemberPitch ||
       update_indices[i] == StateMemberYaw)
     {
-      while (innovation_subset(i) < -PI) {
-        innovation_subset(i) += TAU;
-      }
-
-      while (innovation_subset(i) > PI) {
-        innovation_subset(i) -= TAU;
-      }
+      innovation_subset(i) = ::angles::normalize_angle(innovation_subset(i));
     }
   }
 

src/ukf.cpp

@@ -32,6 +32,7 @@
 
 #include <robot_localization/filter_common.hpp>
 #include <robot_localization/ukf.hpp>
+#include <angles/angles.h>
 #include <Eigen/Cholesky>
 #include <vector>
 
@@ -244,13 +245,7 @@ void Ukf::correct(const Measurement & measurement)
       update_indices[i] == StateMemberPitch ||
       update_indices[i] == StateMemberYaw)
     {
-      while (innovation_subset(i) < -PI) {
-        innovation_subset(i) += TAU;
-      }
-
-      while (innovation_subset(i) > PI) {
-        innovation_subset(i) -= TAU;
-      }
+      innovation_subset(i) = ::angles::normalize_angle(innovation_subset(i));
     }
   }