diff --git a/src/ekf.cpp b/src/ekf.cpp
index 4f4fa259e..86642a683 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -173,9 +173,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..5e027da54 100644
--- a/src/ukf.cpp
+++ b/src/ukf.cpp
@@ -240,9 +240,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;