Merge pull request #241 from cra-ros-pkg/master

Merging orientation Mahalanobis fix

Author: ayrton04

src/ekf.cpp

@@ -162,29 +162,29 @@ namespace RobotLocalization
 
     innovationSubset = (measurementSubset - stateSubset);
 
-    // (2) Check Mahalanobis distance between mapped measurement and state.
-    if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_))
+    // Wrap angles in the innovation
+    for (size_t i = 0; i < updateSize; ++i)
     {
-      // (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
-      // Wrap angles in the innovation
-      for (size_t i = 0; i < updateSize; ++i)
+      if (updateIndices[i] == StateMemberRoll  ||
+          updateIndices[i] == StateMemberPitch ||
+          updateIndices[i] == StateMemberYaw)
       {
-        if (updateIndices[i] == StateMemberRoll ||
-            updateIndices[i] == StateMemberPitch ||
-            updateIndices[i] == StateMemberYaw)
+        while (innovationSubset(i) < -PI)
         {
-          while (innovationSubset(i) < -PI)
-          {
-            innovationSubset(i) += TAU;
-          }
-
-          while (innovationSubset(i) > PI)
-          {
-            innovationSubset(i) -= TAU;
-          }
+          innovationSubset(i) += TAU;
         }
-      }
 
+        while (innovationSubset(i) > PI)
+        {
+          innovationSubset(i) -= TAU;
+        }
+      }
+    }
+    
+    // (2) Check Mahalanobis distance between mapped measurement and state.
+    if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_))
+    {
+      // (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
       state_.noalias() += kalmanGainSubset * innovationSubset;
 
       // (4) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'

src/ukf.cpp

@@ -239,27 +239,28 @@ namespace RobotLocalization
     // (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat)
     innovationSubset = (measurementSubset - predictedMeasurement);
 
-    // (5) Check Mahalanobis distance of innovation
-    if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_))
+    // Wrap angles in the innovation
+    for (size_t i = 0; i < updateSize; ++i)
     {
-      // Wrap angles in the innovation
-      for (size_t i = 0; i < updateSize; ++i)
+      if (updateIndices[i] == StateMemberRoll  ||
+          updateIndices[i] == StateMemberPitch ||
+          updateIndices[i] == StateMemberYaw)
       {
-        if (updateIndices[i] == StateMemberRoll ||
-            updateIndices[i] == StateMemberPitch ||
-            updateIndices[i] == StateMemberYaw)
+        while (innovationSubset(i) < -PI)
+        {
+          innovationSubset(i) += TAU;
+        }
+
+        while (innovationSubset(i) > PI)
         {
-          while (innovationSubset(i) < -PI)
-          {
-            innovationSubset(i) += TAU;
-          }
-
-          while (innovationSubset(i) > PI)
-          {
-            innovationSubset(i) -= TAU;
-          }
+          innovationSubset(i) -= TAU;
         }
       }
+    }
+
+    // (5) Check Mahalanobis distance of innovation
+    if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_))
+    {
 
       state_.noalias() += kalmanGainSubset * innovationSubset;