changed motion equations

Author: adlarkin

EKF/ekf.py

@@ -108,7 +108,7 @@ def get_mu_bar(prev_mu, v, w, angle, dt):
     ########################################################################################
     ############################## DEFINE PARAMETERS HERE ##################################
     ########################################################################################
-    use_mat_data = False
+    use_mat_data = True
     # noise in the command velocities (translational and rotational)
     alpha_1 = .1
     alpha_2 = .01
@@ -137,7 +137,7 @@ def get_mu_bar(prev_mu, v, w, angle, dt):
     num_landmarks = len(lm_x)
 
     # noise free inputs (NOT ground truth)
-    v_c = 1 + (.5*cos(2*np.pi*.2*t))
+    v_c = 1 + (.5*sin(2*np.pi*.2*t))
     om_c = -.2 + (2*cos(2*np.pi*.6*t))
 
     # ground truth

EKF_SLAM/ekf_slam.py

@@ -184,7 +184,7 @@ def wrap(angle):
     F_x[0:3,0:3] = np.identity(3)
 
     # noise free inputs (NOT ground truth)
-    v_c = 1 + (.5*cos(2*np.pi*.2*t))
+    v_c = 1 + (.5*sin(2*np.pi*.2*t))
     om_c = -.2 + (2*cos(2*np.pi*.6*t))
 
     # ground truth

Fast_SLAM/fast_slam.py

@@ -50,7 +50,7 @@ def animate(true_states, pose_particles, markers, lm_pred_uncertainty,
         next_lm_unceratinty, = ax.plot([], [], color='r')
         lm_uncertanties.append(next_lm_unceratinty)
     vision_beam = Wedge((x_tr[0],y_tr[0]), 1000, np.rad2deg(th_tr[0] - fov_bound), 
-        np.rad2deg(th_tr[0] + fov_bound), zorder=-5, alpha=.1, color='m')
+        np.rad2deg(th_tr[0] + fov_bound), zorder=-5, alpha=.15, color='m')
     ax.add_artist(vision_beam)
     ax.legend(loc='upper left', bbox_to_anchor=(1, 1))
 
@@ -186,13 +186,13 @@ def get_avg_uncertainty(lm_uncertainty_matrix):
     alpha_2 = .01
     alpha_3 = .01
     alpha_4 = .1
-    alpha_5 = .01
-    alpha_6 = .01
+    alpha_5 = .001
+    alpha_6 = .001
     # std deviation of range and bearing sensor noise for each landmark
     std_dev_range = .1
     std_dev_bearing = .05
     # sensor field of view (in degrees)
-    FOV = 90
+    FOV = 120
     # number of landmarks
     num_landmarks = 10
     ########################################################################################
@@ -220,8 +220,8 @@ def get_avg_uncertainty(lm_uncertainty_matrix):
     lm_y = world_markers[1,:]
 
     # control inputs - noise free (NOT ground truth)
-    v_c = 1 + (.5*cos(2*np.pi*.2*t))
-    om_c = -.2 + (2*cos(2*np.pi*.6*t))
+    v_c = 1 + (.1*sin(2*np.pi*.3*t))
+    om_c = -.2 + (.1*cos(2*np.pi*.2*t))
     # om_c = -.2 + (1*cos(2*np.pi*.2*t))
 
     # ground truth inputs (with noise)

UKF/ukf.py

@@ -162,7 +162,7 @@ def get_mu_and_sig_bar(dimensionality_bound, w_m, w_c, chi_b_x):
     num_landmarks = len(lm_x)
 
     # noise free inputs (NOT ground truth)
-    v_c = 1 + (.5*cos(2*np.pi*.2*t))
+    v_c = 1 + (.5*sin(2*np.pi*.2*t))
     om_c = -.2 + (2*cos(2*np.pi*.6*t))
 
     # ground truth