Update inertial_navigation.py

Correctly compute sigma
callmebylxh · Aug 10, 2020 · fa6c135 · fa6c135
1 parent 67c523b
commit fa6c135
Showing 1 changed file with 2 additions and 4 deletions.
diff --git a/python/ukfm/model/inertial_navigation.py b/python/ukfm/model/inertial_navigation.py
@@ -445,8 +445,7 @@ def plot_results(self, hat_states, hat_P, states):
         ax.legend([r'true trajectory', r'UKF', r'features'])
 
         ukf3sigma = 3 * \
-            np.sqrt(hat_P[:, 0, 0] ** 2 + hat_P[:, 1, 1]
-                    ** 2 + hat_P[:, 2, 2] ** 2)
+            np.sqrt(hat_P[:, 0, 0] + hat_P[:, 1, 1] + hat_P[:, 2, 2])
         fig, ax = plt.subplots(figsize=(10, 6))
         ax.set(xlabel='$t$ (s)', ylabel='error (deg)',
                title='Attitude error (deg)')
@@ -457,8 +456,7 @@ def plot_results(self, hat_states, hat_P, states):
         ax.set_xlim(0, t[-1])
 
         ukf3sigma = 3 * \
-            np.sqrt(hat_P[:, 6, 6] ** 2 + hat_P[:, 7, 7]
-                    ** 2 + hat_P[:, 8, 8] ** 2)
+            np.sqrt(hat_P[:, 6, 6]+ hat_P[:, 7, 7] + hat_P[:, 8, 8])
         fig, ax = plt.subplots(figsize=(10, 6))
         ax.set(xlabel='$t$ (s)', ylabel='error (m)',
                title="Position error (m)")