def simulate_sensor_data(x, sigma_r, sigma_phi):
ranges = []
bearings = []
for i in range(0,len(landmark_pts)):
ranges.append(dynamics.norm([landmark_pts[i][0]-x[0],landmark_pts[i][1]-x[1]]) + stat_filter.noise(sigma_r*sigma_r))
bearing_map = np.arctan2(landmark_pts[i][1]-x[1],landmark_pts[i][0]-x[0])
bearings.append(bearing_map-x[2] + stat_filter.noise(sigma_phi*sigma_phi))
return ranges, bearings
def ukf_turtlebot(mu, sigma, v_c, w_c, range_ldm, bearing_ldm, landmark_pts, dt, Q, alpha):
M = np.array([[alpha[0]*v_c*v_c + alpha[1]*w_c*w_c, 0.0],
[0.0, alpha[2]*v_c*v_c + alpha[3]*w_c*w_c]])
mu_aug = np.zeros((7,1))
sigma_aug = np.zeros((7,7))
for i in range(0,len(mu)):
mu_aug[i][0] = mu[i][0]
sigma_aug = fill_sigma_aug(sigma_aug, sigma, M, Q)
sigma_points, wm, wc = generate_sigma_points_and_weights(mu_aug, sigma_aug)
n = len(mu_aug)
x_sigma_points = np.zeros((len(mu), (2*n+1)))
Z_bar = np.zeros((len(Q),(2*n+1)))
#Pass sigma points through motion model and predict observations
for i in range(0,len(sigma_points[0])):
x_sigma_points[:,[i]] = dynamics.propogate_next_state(sigma_points[0:3,[i]], v_c+sigma_points[3][i], w_c+sigma_points[4][i], dt)
q = dynamics.norm([landmark_pts[0] - x_sigma_points[0][i], landmark_pts[1] - x_sigma_points[1][i]]) #this is not the same q as in the book (it is sqrt(q))
Z_bar[:,[i]] = np.array([[q],[np.arctan2(landmark_pts[1]-x_sigma_points[1][i],landmark_pts[0]-x_sigma_points[0][i]) - x_sigma_points[2][i]]]) + sigma_points[5:7,[i]]
#compute Gaussian statistics
mu_bar = np.zeros((len(mu), 1))
sigma_bar = np.zeros((len(mu), len(mu)))
z_hat = np.zeros((len(Q),1))
S = np.zeros((len(Q),len(Q)))
sigma_xy = np.zeros((len(mu),len(Q)))
for i in range(0,2*n+1):
mu_bar = mu_bar + wm[i]*x_sigma_points[:,[i]]
for i in range(0,2*n+1):
sigma_bar = sigma_bar + wc[i]*np.dot((x_sigma_points[:,[i]] - mu_bar),np.transpose((x_sigma_points[:,[i]] - mu_bar)))
for i in range(0,2*n+1):
z_hat = z_hat + wm[i]*Z_bar[:,[i]]
for i in range(0,2*n+1):
S = S + wc[i]*np.dot((Z_bar[:,[i]] - z_hat),np.transpose((Z_bar[:,[i]] - z_hat)))
sigma_xy = sigma_xy + wc[i]*np.dot((x_sigma_points[:,[i]] - mu_bar),np.transpose((Z_bar[:,[i]] - z_hat)))
#update mean and covariance
K_plot = np.zeros((1, 1))
K = np.dot(sigma_xy, np.linalg.inv(S))
z = np.array([[range_ldm], [bearing_ldm]])
mu = mu_bar + np.dot(K, (z - z_hat))
sigma = sigma_bar - np.dot(np.dot(K,S),np.transpose(K))
K_plot[0][0] = np.linalg.norm(K)
return mu, sigma, K_plot
def simulate_sensor_data(x, sigma_r, sigma_phi, fov=360.0 * np.pi / 180.0):
ranges = []
bearings = []
correspondence = []
for i in range(0, len(landmark_pts)):
bearing_map = np.arctan2(landmark_pts[i][1] - x[1][0],
landmark_pts[i][0] - x[0][0])
bearing = bearing_map - x[2][0] + stat_filter.noise(
sigma_phi * sigma_phi)
if bearing <= fov and bearing >= -fov:
bearings.append(bearing)
ranges.append(
dynamics.norm(
[landmark_pts[i][0] - x[0], landmark_pts[i][1] - x[1]]) +
stat_filter.noise(sigma_r * sigma_r))
correspondence.append(i)
return ranges, bearings, correspondence
def ekf_turtlebot(mu, sigma, v_c, w_c, ranges, bearings, landmark_pts, dt, Q,
alpha):
theta = mu[2][0]
G = np.array([[
1.0, 0.0,
-v_c / w_c * np.cos(theta) + v_c / w_c * np.cos(theta + w_c * dt)
],
[
0.0, 1.0, -v_c / w_c * np.sin(theta) +
v_c / w_c * np.sin(theta + w_c * dt)
], [0.0, 0.0, 1.0]])
V = np.array([[(-np.sin(theta) + np.sin(theta + w_c * dt)) / w_c,
(v_c * (np.sin(theta) - np.sin(theta + w_c * dt))) /
(w_c * w_c) + (v_c * np.cos(theta + w_c * dt) * dt) / w_c],
[(np.cos(theta) - np.cos(theta + w_c * dt)) / w_c,
(-v_c * (np.cos(theta) - np.cos(theta + w_c * dt))) /
(w_c * w_c) + (v_c * np.sin(theta + w_c * dt) * dt) / w_c],
[0.0, dt]])
M = np.array([[alpha[0] * v_c * v_c + alpha[1] * w_c * w_c, 0.0],
[0.0, alpha[2] * v_c * v_c + alpha[3] * w_c * w_c]])
mu = dynamics.propogate_next_state(mu, v_c, w_c, dt)
sigma_bar = np.dot(np.dot(G, sigma), np.transpose(G)) + np.dot(
np.dot(V, M), np.transpose(V))
K_plot = np.full((len(landmark_pts), 1), 0.0)
for j in range(0, len(landmark_pts)):
q = dynamics.norm([
landmark_pts[j][0] - mu[0], landmark_pts[j][1] - mu[1]
]) #this is not the same q as in the book (it is sqrt(q))
z_hat = np.array([[q],
[
np.arctan2(landmark_pts[j][1] - mu[1],
landmark_pts[j][0] - mu[0]) - mu[2]
]])
H = np.array([[
-(landmark_pts[j][0] - mu[0][0]) / q,
-(landmark_pts[j][1] - mu[1][0]) / q, 0.0
],
[(landmark_pts[j][1] - mu[1][0]) / (q * q),
-(landmark_pts[j][0] - mu[0][0]) / (q * q), -1.0]])
S = np.dot(np.dot(H, sigma_bar), np.transpose(H)) + Q
K = np.dot(np.dot(sigma_bar, np.transpose(H)), np.linalg.inv(S))
z = np.array([[ranges[j]], [bearings[j]]])
mu = mu + np.dot(K, (z - z_hat))
sigma_bar = np.dot((np.eye(3) - np.dot(K, H)), sigma_bar)
K_plot[j][0] = np.linalg.norm(K)
sigma = sigma_bar
return mu, sigma, K_plot