Protect wrapping angles from NaN and inf +/- in ekf and ukf filter

cra-ros-pkg · Mar 11, 2022 · 1cc497b · 1cc497b
1 parent 5c9d411
commit 1cc497b
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git 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
@@ -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;