From 1cc497bd7409bb468583cc52c088935d8dfb83be Mon Sep 17 00:00:00 2001
From: Marek Piechula <mpiechula@autonomous-systems.pl>
Date: Fri, 11 Mar 2022 17:15:20 +0100
Subject: [PATCH] Protect wrapping angles from NaN and inf +/- in ekf and ukf
 filter

---
 src/ekf.cpp | 6 ++++--
 src/ukf.cpp | 6 ++++--
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 4f4fa259e..28d6c6da3 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -36,6 +36,7 @@
 #include <Eigen/Dense>
 #include <rclcpp/duration.hpp>
 #include <vector>
+#include <cmath>
 
 namespace robot_localization
 {
@@ -173,9 +174,10 @@ void Ekf::correct(const Measurement & measurement)
 
   // Wrap angles in the innovation
   for (size_t i = 0; i < update_size; ++i) {
-    if (update_indices[i] == StateMemberRoll ||
+    if ((update_indices[i] == StateMemberRoll ||
       update_indices[i] == StateMemberPitch ||
-      update_indices[i] == StateMemberYaw)
+      update_indices[i] == StateMemberYaw) &&
+      std::isfinite(innovation_subset(i)))
     {
       while (innovation_subset(i) < -PI) {
         innovation_subset(i) += TAU;
diff --git a/src/ukf.cpp b/src/ukf.cpp
index 4305c763e..42203a8b3 100644
--- a/src/ukf.cpp
+++ b/src/ukf.cpp
@@ -34,6 +34,7 @@
 #include <robot_localization/ukf.hpp>
 #include <Eigen/Cholesky>
 #include <vector>
+#include <cmath>
 
 
 namespace robot_localization
@@ -240,9 +241,10 @@ void Ukf::correct(const Measurement & measurement)
 
   // Wrap angles in the innovation
   for (size_t i = 0; i < updateSize; ++i) {
-    if (update_indices[i] == StateMemberRoll ||
+    if ((update_indices[i] == StateMemberRoll ||
       update_indices[i] == StateMemberPitch ||
-      update_indices[i] == StateMemberYaw)
+      update_indices[i] == StateMemberYaw) &&
+      std::isfinite(innovation_subset(i)))
     {
       while (innovation_subset(i) < -PI) {
         innovation_subset(i) += TAU;