Update to SLAM example

Author: botprof

vanilla_SLAM.py

@@ -6,15 +6,17 @@
 
 # %%
 # SIMULATION SETUP
-
 import numpy as np
 import matplotlib.pyplot as plt
 from scipy.stats import chi2
 from matplotlib import patches
 
+# Uncomment for interactive plots in Jupyter
+# %matplotlib ipympl
+
 # Set the simulation time [s] and the sample period [s]
 SIM_TIME = 60.0
-T = 0.01
+T = 0.1
 
 # Create an array of time values [s]
 t = np.arange(0.0, SIM_TIME, T)
@@ -37,6 +39,7 @@
 # %%
 # VEHICLE, SENSOR MODELS AND KALMAN FILTER FUNCTIONS
 
+
 # Discrete-time omnidirectional vehicle model
 def vehicle(x, u, T):
     x_new = x + T * u
@@ -154,7 +157,7 @@ def KF_correct(x, y, H, R, P):
         x_hat[2 : 2 * m, i] = x_hat[2 : 2 * m, i - 1]
         P_hat[2 : 2 * m, 0 : 2 * m, i] = P_hat[2 : 2 * m, 0 : 2 * m, i - 1]
 
-        # Run the KF correction step
+        # Run the KF correction step (try commenting out and see what happens)
         x_hat[:, i], P_hat[:, :, i] = KF_correct(x_hat[:, i], y, H, R, P_hat[:, :, i])
 
 # %%
@@ -166,11 +169,8 @@ def KF_correct(x, y, H, R, P):
 plt.rc("savefig", format="pdf")
 plt.rc("savefig", bbox="tight")
 
-# Uncomment for interactive plots in IPython
-# %matplotlib widget
-
-# Find the scaling factor for plotting covarianc`e bounds
-ALPHA = 0.05
+# Find the scaling factor for plotting covariance bounds
+ALPHA = 0.01
 s1 = chi2.isf(ALPHA, 1)
 s2 = chi2.isf(ALPHA, 2)