From a0c1904bb5dd465ace94d4e01dacc7c8b96005ad Mon Sep 17 00:00:00 2001
From: Marek Piechula <marpiechula@gmail.com>
Date: Mon, 25 Apr 2022 18:42:47 +0200
Subject: [PATCH] Using angles library to normalize angles (#739)

* Using angles library for innovation angle normalization
---
 package.xml | 1 +
 src/ekf.cpp | 9 ++-------
 src/ukf.cpp | 9 ++-------
 3 files changed, 5 insertions(+), 14 deletions(-)

diff --git a/package.xml b/package.xml
index 7b3b6b81d..d7806c311 100644
--- 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
index 4f4fa259e..b3dcc21f2 100644
--- 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
index 4305c763e..ee77100bb 100644
--- 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));
     }
   }