Using angles library to normalize angles (#739)

* Using angles library for innovation angle normalization
cra-ros-pkg · Apr 25, 2022 · a0c1904 · a0c1904
1 parent a737b27
commit a0c1904
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 14 deletions.
diff --git a/package.xml b/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>

diff --git a/src/ekf.cpp b/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));
     }
   }
 

diff --git a/src/ukf.cpp b/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));
     }
   }