def initialize_landmark(ekf_state, tree):
'''
Initialize a newly observed landmark in the filter state, increasing its
dimension by 2.
Returns the new ekf_state.
'''
range = tree[0]
angle = tree[1]
xv = ekf_state['x'][0]
yv = ekf_state['x'][1]
phi = ekf_state['x'][2]
xl = range * np.cos(slam_utils.clamp_angle(angle + phi)) + xv
yl = range * np.sin(slam_utils.clamp_angle(angle + phi)) + yv
ekf_state['x'] = np.append(ekf_state['x'], [xl, yl])
m, n = ekf_state['P'].shape
P_new = np.zeros((m + 2, n + 2))
P_new[m, n] = 69.5
P_new[m + 1, n + 1] = 69.5
P_new[0:m, 0:n] = ekf_state['P']
ekf_state['P'] = slam_utils.make_symmetric(P_new)
ekf_state['num_landmarks'] += 1
return ekf_state
def laser_measurement_model(ekf_state, landmark_id):
'''
Returns the measurement model for a (range,bearing) sensor observing the
mapped landmark with id 'landmark_id' along with its jacobian.
Returns:
h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].
dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
matrix corresponding to a measurement of the landmark_id'th feature.
'''
###
# Implement the measurement model and its Jacobian you derived
###
t_st = ekf_state['x'].copy()
t_st[2] = slam_utils.clamp_angle(t_st[2])
dim = t_st.shape[0]
r_x,r_y,phi = t_st[0],t_st[1],t_st[2]
m_x,m_y = t_st[3+2*landmark_id],t_st[4+2*landmark_id]
del_x = m_x - r_x
del_y = m_y - r_y
q = (del_x)**2+(del_y)**2
sqrt_q = np.sqrt(q)
zhat = [[sqrt_q],[slam_utils.clamp_angle(np.arctan2(del_y,del_x)-phi)]]
h = np.array([[-sqrt_q*del_x,-sqrt_q*del_y,0,sqrt_q*del_x,sqrt_q*del_y],[del_y,-del_x,-q,-del_y,del_x]])/q
F_x = np.zeros((5,dim))
F_x[:3,:3] = np.eye(3)
F_x[3,3+2*landmark_id]=1
F_x[4,4+2*landmark_id]=1
H = np.matmul(h,F_x)
return zhat, H
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
for i, j in enumerate(assoc):
if j == -2:
continue
elif j == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
n = ekf_state['num_landmarks']
sigma = ekf_state['P']
zhat, H = laser_measurement_model(ekf_state, n - 1)
K = np.matmul(
np.matmul(sigma, H.T),
np.linalg.inv(np.matmul(np.matmul(H, sigma), H.T) + Q))
z = np.zeros((2, 1))
z[0, 0] = trees[i][0]
z[1, 0] = trees[i][1]
state_update = np.matmul(K, (z - zhat))
ekf_state['x'] = np.reshape(
(np.reshape(ekf_state['x'], (-1, 1)) + state_update), (-1, ))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.matmul((np.eye(ekf_state['x'].shape[0]) - np.matmul(K, H)),
sigma))
else:
sigma = ekf_state['P']
zhat, H = laser_measurement_model(ekf_state, j)
K = np.matmul(
np.matmul(sigma, H.T),
np.linalg.inv(np.matmul(np.matmul(H, sigma), H.T) + Q))
z = np.zeros((2, 1))
z[0, 0] = trees[i][0]
z[1, 0] = trees[i][1]
state_update = np.matmul(K, (z - zhat))
ekf_state['x'] = np.reshape(
(np.reshape(ekf_state['x'], (-1, 1)) + state_update), (-1, ))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.matmul((np.eye(ekf_state['x'].shape[0]) - np.matmul(K, H)),
sigma))
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
# Implement the EKF update for a set of range, bearing measurements.
###
R = np.diag([sigmas['range']**2, sigmas['bearing']**2])
for i in range(0, len(trees)):
if assoc[i] == -2:
continue
elif assoc[i] == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
P = ekf_state['P']
z_hat, H = laser_measurement_model(ekf_state,
ekf_state['num_landmarks'] - 1)
r = np.array(np.array(trees[i][0:2]) - np.array(z_hat))
S_inv = np.linalg.inv(
np.matmul(np.matmul(H, P), np.transpose(H)) + R)
K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
temp1 = np.identity(P.shape[0]) - np.matmul(K, H)
ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp1, P))
else:
P = ekf_state['P']
z_hat, H = laser_measurement_model(ekf_state, assoc[i])
r = np.array(np.array(trees[i][0:2]) - np.array(z_hat))
S_inv = np.linalg.inv(
np.matmul(np.matmul(H, P), np.transpose(H)) + R)
K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
temp2 = np.identity(P.shape[0]) - np.matmul(K, H)
ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp2, P))
return ekf_state
def motion_model(u, dt, ekf_state, vehicle_params):
'''
Computes the discretized motion model for the given vehicle as well as its Jacobian
Returns:
f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
'''
###
# Implement the vehicle model and its Jacobian you derived.
###
Ve = u[0]
alpha = slam_utils.clamp_angle(u[1])
H = vehicle_params['H']
L = vehicle_params['L']
a = vehicle_params['a']
b = vehicle_params['b']
Vc = Ve / (1 - np.tan(alpha) * H / L)
# phi = ekf_state['x'][2]
phi = slam_utils.clamp_angle(ekf_state['x'][2])
motion = np.empty([3], dtype=np.float64)
motion[0] = dt * (Vc * np.cos(phi) - Vc / L * np.tan(alpha) *
(a * np.sin(phi) + b * np.cos(phi)))
motion[1] = dt * (Vc * np.sin(phi) + Vc / L * np.tan(alpha) *
(a * np.cos(phi) - b * np.sin(phi)))
motion[2] = dt * Vc / L * np.tan(alpha)
G = np.empty([3, 3], dtype=np.float64)
G[0, 0] = 1
G[0, 1] = 0
G[0, 2] = dt * (-Vc * np.sin(phi) - Vc / L * np.tan(alpha) *
(a * np.cos(phi) - b * np.sin(phi)))
G[1, 0] = 0
G[1, 1] = 1
G[1, 2] = dt * (Vc * np.cos(phi) + Vc / L * np.tan(alpha) *
(-a * np.sin(phi) - b * np.cos(phi)))
G[2, 0] = 0
G[2, 1] = 0
G[2, 2] = 1
return motion, G
def initialize_landmark(ekf_state, tree):
'''turnin -c ese650 -p project2 slam.py
Initialize a newly observed landmark in the filter state, increasing its
dimension by 2.
Returns the new ekf_state.
'''
###
# Implement this function.
###
current_x = ekf_state['x'][0]
current_y = ekf_state['x'][1]
current_th = ekf_state['x'][2]
measurements_r = tree[0]
measurements_th = tree[1]
ekf_state['x'] = np.hstack(
(ekf_state['x'],
current_x + measurements_r * np.cos(measurements_th + current_th),
current_y + measurements_r * np.sin(measurements_th + current_th)))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['num_landmarks'] = ekf_state['num_landmarks'] + 1
newP = np.zeros((2 * ekf_state['num_landmarks'] + 3,
2 * ekf_state['num_landmarks'] + 3))
newP[-1, -1], newP[-2, -2] = 1000, 1000
newP[:-2, :-2] = ekf_state['P']
ekf_state['P'] = newP
return ekf_state
def initialize_landmark(ekf_state, tree):
'''
Initialize a newly observed landmark in the filter state, increasing its
dimension by 2.
Returns the new ekf_state.
'''
###
# Implement this function.
###
phi = ekf_state["x"][2]
mu = ekf_state["x"][0:2]
landmarks = np.reshape(mu, (2, 1)) + np.vstack(
(tree[0] * np.cos(phi + tree[1]), tree[0] * np.sin(phi + tree[1])))
ekf_state['x'] = np.append(ekf_state['x'], landmarks[0, 0])
ekf_state['x'] = np.append(ekf_state['x'], landmarks[1, 0])
P = ekf_state['P']
# P = np.vstack((P,np.zeros((2,P.shape[0]))))
# P = np.hstack((P, np.zeros((P.shape[0],2))))
P_new = np.eye(P.shape[0] + 2) * 1000
# P[-1,-1], P[-2,-2] = 1000,1000
P_new[:-2, :-2] = P
ekf_state['P'] = slam_utils.make_symmetric(P_new)
ekf_state['num_landmarks'] += 1
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
return ekf_state
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
'''
Perform the propagation step of the EKF filter given an odometry measurement u
and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.
Returns the new ekf_state.
'''
###
# Implement the propagation
###
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
R = np.diag([
sigmas['xy'] * sigmas['xy'], sigmas['xy'] * sigmas['xy'],
sigmas['phi'] * sigmas['phi']
])
ekf_state['x'][0:3] = ekf_state['x'][0:3] + motion
ekf_state['P'][:3, :3] = np.matmul(np.matmul(G, ekf_state['P'][:3, :3]),
G.T) + R
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
r = np.array(gps - ekf_state['x'][0:2])
P = ekf_state['P']
R = np.diag([sigmas['gps']**2, sigmas['gps']**2])
H = np.zeros((2, ekf_state['x'].size))
H[0, 0] = 1
H[1, 1] = 1
S_inv = np.linalg.inv(P[0:2, 0:2] + R)
MD = np.matmul(np.matmul(np.transpose(r), S_inv), r)
if MD < chi2.ppf(0.999, df=2):
K = np.matmul(np.matmul(P, np.transpose(H)), S_inv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
temp1 = np.identity(ekf_state['x'].size) - np.matmul(K, H)
ekf_state['P'] = slam_utils.make_symmetric(np.matmul(temp1, P))
# else:
# print("no gps")
return ekf_state
def motion_model(u, dt, ekf_state, vehicle_params):
'''
Computes the discretized motion model for the given vehicle as well as its Jacobian
Returns:
f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
'''
v_e = u[0].copy()
alpha = u[1].copy()
H = vehicle_params['H']
L = vehicle_params['L']
a = vehicle_params['a']
b = vehicle_params['b']
v_c = (v_e)/(1-np.tan(alpha)*(H/L))
t_st = ekf_state['x'].copy()
phi = t_st[2]
el1 = dt*(v_c*np.cos(phi)-(v_c/L)*np.tan(alpha)*(a*np.sin(phi)+b*np.cos(phi)))
el2 = dt*(v_c*np.sin(phi)+(v_c/L)*np.tan(alpha)*(a*np.cos(phi)-b*np.sin(phi)))
el3 = dt*(v_c/L)*np.tan(alpha)
el31 = slam_utils.clamp_angle(el3)
motion = np.array([[el1],[el2],[el31]])
el13 = -dt*v_c*(np.sin(phi)+(1/L)*np.tan(alpha)*(a*np.cos(phi)-b*np.sin(phi)))
el23 = dt*v_c*(np.cos(phi)-(1/L)*np.tan(alpha)*(a*np.sin(phi)+b*np.cos(phi)))
G = np.array([[1,0,el13],[0,1,el23],[0,0,1]])
return motion, G
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
'''
Perform the propagation step of the EKF filter given an odometry measurement u
and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.
Returns the new ekf_state.
'''
###
# Implement the propagation
###
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
P = ekf_state['P']
Q = np.diag([sigmas['xy']**2, sigmas['xy']**2, sigmas['phi']**2])
upd_state = ekf_state['x'][0:3] + motion
upd_covariance = np.matmul(np.matmul(G, P[0:3, 0:3]), np.transpose(G)) + Q
ekf_state['x'][0:3] = upd_state
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'][0:3, 0:3] = upd_covariance
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
##
#Implement the GPS update.
##
H = np.zeros((2, 3 + 2 * ekf_state['num_landmarks']))
H[0, 0] = 1
H[1, 1] = 1
Q = np.eye(2, 2) * (sigmas['gps']**2)
Sigma = ekf_state['P']
r = gps - ekf_state['x'][:2]
Sinv = np.linalg.inv(np.matmul(np.matmul(H, Sigma), H.T) + Q.T)
d = np.matmul(np.matmul(r.T, Sinv), r)
if d > chi2.ppf(0.999, df=2):
return ekf_state
K = np.matmul(np.matmul(Sigma, H.T), Sinv)
ekf_state['x'] = ekf_state['x'] + np.matmul(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
Sigma = np.matmul(
(np.eye(3 + 2 * ekf_state['num_landmarks'],
3 + 2 * ekf_state['num_landmarks']) - np.matmul(K, H)), Sigma)
ekf_state['P'] = slam_utils.make_symmetric(Sigma)
# print(ekf_state)
return ekf_state
def initialize_landmark(ekf_state, tree):
'''
Initialize a newly observed landmark in the filter state, increasing its
dimension by 2.
Returns the new ekf_state.
'''
###
# Implement this function.
###
xv, yv, phi = ekf_state['x'][0:3]
P = ekf_state['P']
ranges, bearings, dia = tree
bearings = slam_utils.clamp_angle(bearings)
xl = xv + ranges * np.cos(bearings + phi)
yl = yv + ranges * np.sin(bearings + phi)
upd_state = np.array(np.append(ekf_state['x'], np.array([xl, yl])))
upd_covariance = np.zeros((upd_state.size, upd_state.size))
upd_covariance[0:P.shape[0], 0:P.shape[1]] = P
upd_covariance[upd_state.size - 1, upd_state.size - 1] = 100
upd_covariance[upd_state.size - 2, upd_state.size - 2] = 100
ekf_state['num_landmarks'] += 1
ekf_state['x'] = upd_state
ekf_state['P'] = slam_utils.make_symmetric(upd_covariance)
return ekf_state
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
'''
Perform the propagation step of the EKF filter given an odometry measurement u
and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.
Returns the new ekf_state.
'''
###
# Implement the propagation
###
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
# motion += np.vstack([ekf_state['x'][0], ekf_state['x'][1], ekf_state['x'][2]])
# G = G + np.eye(3)
P = ekf_state['P'][:3, :3]
sigma_xy = sigmas['xy']**2
sigma_phi = sigmas['phi']**2
Q = sigma_xy * np.eye(3)
Q[2, 2] = sigma_phi
ekf_state['x'][:3] = motion.reshape(3, )
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'][:3, :3] = np.dot(np.dot(G, P), G.T) + Q
ekf_state['P'] = fix_P(ekf_state['P'])
return ekf_state
def laser_measurement_model(ekf_state, landmark_id):
'''
Returns the measurement model for a (range,bearing) sensor observing the
mapped landmark with id 'landmark_id' along with its jacobian.
Returns:
h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].
dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
matrix corresponding to a measurement of the landmark_id'th feature.
'''
x, y, p = ekf_state['x'][:3]
x_t, y_t = ekf_state['x'][3 + landmark_id * 2:3 + (landmark_id + 1) * 2]
H = np.zeros((2, ekf_state['x'].shape[0]))
zhat = np.array(
[np.sqrt((x_t - x) ** 2 + (y_t - y) ** 2), slam_utils.clamp_angle(np.arctan2(y_t - y, x_t - x) - p)])
H[:, :3] = np.array([[(x - x_t) / zhat[0], (y - y_t) / zhat[0], 0],
[-(y - y_t) / zhat[0] ** 2, (x - x_t) / zhat[0] ** 2, -1]])
# middle_H = np.array([[-(2 * x - 2 * x_t) / (2 * ((x - x_t) ** 2 + (y - y_t) ** 2) ** (1 / 2)),
# -(2 * y - 2 * y_t) / (2 * ((x - x_t) ** 2 + (y - y_t) ** 2) ** (1 / 2))]
# , [(y - y_t) / ((x - x_t) ** 2 + (y - y_t) ** 2),
# -(x - x_t) / ((x - x_t) ** 2 + (y - y_t) ** 2)]])
H[:, 3 + landmark_id * 2:3 + (landmark_id + 1) * 2] = -H[:2, :2]
return zhat, H
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
'''
Perform the propagation step of the EKF filter given an odometry measurement u
and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.
Returns the new ekf_state.
'''
# R - process noise
R = np.zeros([3, 3])
R[0, 0] = sigmas['xy'] * sigmas['xy']
R[1, 1] = sigmas['xy'] * sigmas['xy']
R[2, 2] = sigmas['phi'] * sigmas['phi']
f, G = motion_model(u, dt, ekf_state, vehicle_params)
F = np.zeros((3, ekf_state['x'].size))
F[0:3, 0:3] = np.eye(3)
G = G - np.eye(3)
G = np.eye(ekf_state['x'].size) + np.matmul(np.matmul(F.T, G), (F))
ekf_state['x'] = ekf_state['x'] + np.matmul(F.T, f)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = np.matmul(np.matmul(G, ekf_state['P']), G.T) + np.matmul(
np.matmul(F.T, R), F)
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Performs a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
x = ekf_state["x"].copy()
P = ekf_state["P"].copy()
H = np.zeros([2, len(x)])
H[0, 0] = 1
H[1, 1] = 1
R_t = (sigmas['gps']**2) * np.eye(2)
S = P[0:2, 0:2] + R_t
update = gps.reshape(2, 1) - x[0:2].reshape(2, 1)
inv_S = slam_utils.invert_2x2_matrix(S)
d = np.dot(np.dot(np.transpose(update), inv_S), update)
if d < 13.8:
K = np.dot(P[:, 0:2], inv_S)
ekf_state['x'] = x + np.dot(K, update).flatten()
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.dot((np.eye(len(x)) - np.dot(K, H)), P))
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
residual = np.transpose([gps - ekf_state['x'][:2]])
H_mat = np.matrix(np.zeros([2, P.shape[0]]))
H_mat[0, 0], H_mat[1, 1] = 1, 1
R_mat = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S_mat = H_mat * P * H_mat.T + R_mat
d = (np.matrix(residual)).T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat))) * np.matrix(residual)
if d <= chi2.ppf(0.999, 2):
Kt = P * H_mat.T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat)))
ekf_state['x'] = ekf_state['x'] + np.squeeze(
np.array(Kt * np.matrix(residual)))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.array((np.matrix(np.eye(P.shape[0])) - Kt * H_mat) * P))
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
dim = P.shape[0]-2
H = np.hstack((np.eye(2),np.zeros((2,dim))))
r = np.transpose([gps - ekf_state['x'][:2]])
Q = (sigmas['gps']**2)*(np.eye(2))
S = np.matmul(np.matmul(H,P),H.T) + Q
S_inv = slam_utils.invert_2x2_matrix(S)
d = np.matmul(np.matmul(r.T,S_inv),r)
if d <= chi2.ppf(0.999, 2):
K = np.matmul(np.matmul(P,H.T),S_inv)
ekf_state['x'] = ekf_state['x'] + np.squeeze(np.matmul(K,r))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
P_temp = np.matmul((np.eye(P.shape[0])- np.matmul(K,H)),P)
ekf_state['P'] = slam_utils.make_symmetric(P_temp)
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
# Implement the EKF update for a set of range, bearing measurements.
###
Qt = np.diag([sigmas['range']**2, sigmas['bearing']**2])
z = np.zeros((2, 1))
for i in range(len(trees)):
j = assoc[i]
if j == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
j = np.int(len(ekf_state['x']) / 2) - 2
elif j == -2:
continue
elif j < -2 or j >= ekf_state['num_landmarks']:
raise ValueError('Problem in Data Association')
zhat, H = laser_measurement_model(ekf_state, j)
# Kalman Gain Initialization
PHt = np.matmul(ekf_state['P'], np.transpose(H))
HPHt_R_inv = np.linalg.inv(np.matmul(H, PHt) + Qt)
Kt = np.matmul(PHt, HPHt_R_inv)
# Lidar Landmark j measurement
z[0] = trees[i][0]
z[1] = trees[i][1]
# Mean Update
change_ut = np.matmul(Kt, z - zhat)
ekf_state['x'] = ekf_state['x'] + np.squeeze(change_ut)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
# Covariance Update
ekf_state['P'] = np.matmul(
(np.eye(2 * ekf_state['num_landmarks'] + 3) - np.matmul(Kt, H)),
ekf_state['P'])
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Performs a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
assoc = np.asarray(assoc)
trees = np.asarray(trees)
n = ekf_state['num_landmarks']
if assoc.shape[0] != 0:
new_tree_index = assoc == -1
new_trees = trees[new_tree_index, :]
for i in range(new_trees.shape[0]):
initialize_landmark(ekf_state, new_trees[i, :])
assoc[new_tree_index] = np.linspace(n, n + new_trees.shape[0] - 1,
new_trees.shape[0])
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
exisiting_trees_index = assoc > -1
exisiting_trees = trees[exisiting_trees_index, :]
landmark_ids = assoc[exisiting_trees_index]
for i in range(exisiting_trees.shape[0]):
measure = exisiting_trees[i, 0:2].reshape(2, 1)
zhat, H = laser_measurement_model(ekf_state, int(landmark_ids[i]))
P = ekf_state['P'].copy()
K = np.dot(
np.dot(P, H.transpose()),
slam_utils.invert_2x2_matrix(
np.dot(np.dot(H, P), H.transpose()) + Q))
ekf_state['x'] = ekf_state['x'] + np.dot(K, (measure - zhat))[:, 0]
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.dot((np.eye(P.shape[0]) - np.dot(K, H)), P))
###
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
"""
The diameter component of the measurement can be discarded
:param trees: a list of measurements, where each measurement is a tuple (range, bearing, diameter)
:param assoc: the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as it is too ambiguous to use
:param ekf_state:
:param sigmas:
:param params:
:return:
Returns the ekf_state
"""
# measurement noise
range_noise = sigmas['range']
bearing_noise = sigmas['bearing']
Q = np.diag((range_noise**2, bearing_noise**2))
for i in range(len(trees)):
# add state with new landmark
if assoc[i] == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
# discard ambiguous measurement
elif assoc[i] == -2:
continue
# update state with existing landmark
else:
landmark_id = assoc[i]
z_t, H = laser_measurement_model(ekf_state, landmark_id)
x = ekf_state['x']
P = ekf_state['P']
N = len(ekf_state['x'])
# Kalman Gain update
K_t = np.dot(P, H.T)
K_t = np.dot(
K_t,
slam_utils.invert_2x2_matrix(np.dot(np.dot(H, P), H.T) + Q))
z = np.asarray([trees[i][0], trees[i][1]]).reshape((2, 1))
ekf_state['x'] = x + np.dot(K_t, (z - z_t)).reshape(-1)
ekf_state['P'] = np.dot(np.identity(N) - np.dot(K_t, H), P)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'] + 10e-6)
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
# Implement the EKF update for a set of range, bearing measurements.
###
Q = np.eye(2, 2)
Q[0][0] = sigmas['range']**2
Q[1][1] = sigmas['bearing']**2
for i in range(len(assoc)):
j = assoc[i]
if (j == -1):
ekf_state = initialize_landmark(ekf_state, trees[i])
j = ekf_state['num_landmarks'] - 1
if (j != -2):
z_hat, H = laser_measurement_model(ekf_state, j)
K = np.matmul(
np.matmul(ekf_state['P'], H.T),
np.linalg.inv(
(np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T)))
z = trees[i][:2]
ekf_state['x'] = ekf_state['x'] + np.matmul(
K, (np.asarray(z) - z_hat))
ekf_state['P'] = np.matmul(
(np.eye(np.shape(ekf_state['P'])[0]) - np.matmul(K, H)),
ekf_state['P'])
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
return ekf_state
def odom_predict(u, dt, ekf_state, vehicle_params, sigmas):
'''
Perform the propagation step of the EKF filter given an odometry measurement u
and time step dt where u = (ve, alpha) as shown in the vehicle/motion model.
Returns the new ekf_state.
'''
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
ekf_state['x'][:3] = motion
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'][:3, :3] = np.matmul(np.matmul(G, ekf_state['P'][:3, :3]), G.T) + \
np.diag([sigmas['xy'] ** 2, sigmas['xy'] ** 2, sigmas['phi'] ** 2]) # + \
# 10**-6*np.eye(3)
ekf_state['P'] = fix_cov(ekf_state['P'])
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
# Implement the EKF update for a set of range, bearing measurements.
###
Q_t = np.array([[sigmas['range']**2, 0], [0, sigmas['bearing']**2]])
for i in range(len(trees)):
j = assoc[i]
if j == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
j = np.int(len(ekf_state['x'])/2) - 2
elif j == -2:
continue
dim = ekf_state['x'].shape[0]
z_hat, H = laser_measurement_model(ekf_state, j)
S = np.matmul(H,np.matmul(ekf_state['P'],H.T)) + Q_t
S_inv = np.linalg.inv(S)
K = np.matmul(np.matmul(ekf_state['P'],H.T), S_inv)
z = np.zeros((2, 1))
z[0,0] = trees[i][0]
z[1,0] = trees[i][1]
inno = z - z_hat
temp_st = ekf_state['x'] + np.squeeze(np.matmul(K, inno))
temp_st[2] = slam_utils.clamp_angle(temp_st[2])
ekf_state['x'] = temp_st
temp_p = np.matmul((np.eye(dim) - np.matmul(K, H)), ekf_state['P'])
temp_p = slam_utils.make_symmetric(temp_p)
ekf_state['P'] = temp_p
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
# Perform a measurement update of the EKF state given a set of tree measurements.
# trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
# assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
# ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
# in the state for measurement i. If assoc[i] == -2, discard the measurement as
# it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
assoc = np.array(assoc)
measurements = np.array(trees)[:, 0:2]
good_assoc = assoc[assoc > -1]
good_measurements = measurements[assoc > -1]
Q = np.diag(
np.array([
sigmas['range'] * sigmas['range'],
sigmas['bearing'] * sigmas['bearing']
]))
Kg = np.zeros([ekf_state['x'].shape[0], 2 * good_assoc.size])
residuals = np.zeros((2 * good_measurements.shape[0]))
H = np.zeros([2 * good_assoc.size, ekf_state['x'].shape[0]])
for i in range(good_assoc.size):
zhat, H[i:i + 2, :] = laser_measurement_model(ekf_state, good_assoc[i])
Kg[:, i:i+2] = np.matmul(ekf_state['P'] , np.matmul(H[i:i+2,:].T, \
np.linalg.inv(np.matmul(H[i:i+2,:], np.matmul(ekf_state['P'], H[i:i+2,:].T))+ Q)))
residuals[i:i + 2] = good_measurements[i] - zhat
ekf_state['x'] = ekf_state['x'] + np.matmul(Kg, residuals)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = np.matmul(
(np.eye(ekf_state['P'].shape[0]) - np.matmul(Kg, H)), ekf_state['P'])
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'])
ekf_state['P'] = make_positive_definite(ekf_state['P'])
new_measurements = measurements[assoc == -1]
for i in range(new_measurements.shape[0]):
ekf_state = initialize_landmark(ekf_state, new_measurements[i])
return ekf_state
def motion_model(u, dt, ekf_state, vehicle_params):
'''
Computes the discretized motion model for the given vehicle as well as its Jacobian
Returns:
f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
'''
###
# Implement the vehicle model and its Jacobian you derived.
###
a = vehicle_params['a']
b = vehicle_params['b']
H = vehicle_params['H']
L = vehicle_params['L']
ve = u[0]
alpha = u[1]
vc = ve / (1 - np.tan(alpha) * H / L)
motion = ekf_state['x']
xv = ekf_state['x'][0] + dt * (
vc * np.cos(ekf_state['x'][2]) - vc / L * np.tan(alpha) *
(a * np.sin(ekf_state['x'][2]) + b * np.cos(ekf_state['x'][2])))
yv = ekf_state['x'][1] + dt * (
vc * np.sin(ekf_state['x'][2]) + vc / L * np.tan(alpha) *
(a * np.cos(ekf_state['x'][2]) - b * np.sin(ekf_state['x'][2])))
theta = slam_utils.clamp_angle(ekf_state['x'][2] +
dt * vc / L * np.tan(alpha))
motion[:3] = [xv, yv, theta]
G = np.zeros([3, ekf_state['x'].shape[0]])
xv_G = [
1, 0,
dt * (-vc * np.sin(ekf_state['x'][2]) - vc / L * np.tan(alpha) *
(a * np.cos(ekf_state['x'][2]) - b * np.sin(ekf_state['x'][2])))
]
yv_G = [
0, 1,
dt * (vc * np.cos(ekf_state['x'][2]) + vc / L * np.tan(alpha) *
(-a * np.sin(ekf_state['x'][2]) - b * np.cos(ekf_state['x'][2])))
]
theta_G = [0, 0, 1]
G[:3, :3] = np.vstack([xv_G, yv_G, theta_G])
return motion, G
def motion_model(u, dt, ekf_state, vehicle_params):
'''
Computes the discretized motion model for the given vehicle as well as its Jacobian
Returns:
f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
'''
###
# Implement the vehicle model and its Jacobian you derived.
###
H = vehicle_params['H']
L = vehicle_params['L']
a = vehicle_params['a']
b = vehicle_params['b']
alpha = u[1]
v_c = u[0] / (1 - np.tan(alpha) * (H / L))
phi = ekf_state['x'][2]
motion = np.array(
[[
dt * (v_c * np.cos(phi) - (v_c / L) * np.tan(alpha) *
(a * np.sin(phi) + b * np.cos(phi)))
],
[
dt * (v_c * np.sin(phi) + (v_c / L) * np.tan(alpha) *
(a * np.cos(phi) - b * np.sin(phi)))
], [slam_utils.clamp_angle(dt * (v_c / L) * np.tan(alpha))]])
jacobian_process = np.array([[
1, 0, -dt * v_c * (np.sin(phi) + (1 / L) * np.tan(alpha) *
(a * np.cos(phi) - b * np.sin(phi)))
],
[
0, 1, dt * v_c *
(np.cos(phi) - (1 / L) * np.tan(alpha) *
(a * np.sin(phi) + b * np.cos(phi)))
], [0, 0, 1]])
return motion, jacobian_process
def laser_measurement_model(ekf_state, landmark_id):
"""
Returns the measurement model for a (range,bearing) sensor observing the
mapped landmark with id 'landmark_id' along with its jacobian.
:param ekf_state:
:param landmark_id:
:return:
h(x, l_id): the 2x1 predicted measurement vector [r_hat, theta_hat].
dh/dX: For a measurement state with m mapped landmarks, i.e. a state vector of
dimension 3 + 2*m, this should return the full 2 by 3+2m Jacobian
matrix corresponding to a measurement of the landmark_id'th feature.
"""
# landmark ekf_state
x_l = ekf_state['x'][3 + landmark_id * 2]
y_l = ekf_state['x'][4 + landmark_id * 2]
x = ekf_state['x'][0]
y = ekf_state['x'][1]
phi = ekf_state['x'][2]
### Observation Model ###
z_t = np.zeros((2, 1))
q = np.sqrt((x_l - x)**2 + (y_l - y)**2)
z_t[0, :] = q
z_t[1, :] = np.arctan2(y_l - y, x_l - x) - phi
z_t[1, :] = slam_utils.clamp_angle(z_t[1, :])
# Compute Jacobian for observation
H = np.zeros((2, len(ekf_state['x'])))
H[0, 0] = -(x_l - x) / q
H[0, 1] = -(y_l - y) / q
H[0, 2] = 0
H[0, 3 + landmark_id * 2] = (x_l - x) / q
H[0, 4 + landmark_id * 2] = (y_l - y) / q
H[1, 0] = (y_l - y) / q**2
H[1, 1] = -(x_l - x) / q**2
H[1, 2] = -1
H[1, 3 + landmark_id * 2] = -(y_l - y) / q**2
H[1, 4 + landmark_id * 2] = (x_l - x) / q**2
return z_t, H
def motion_model(u, dt, ekf_state, vehicle_params):
"""
Computes the discretized motion model for the given vehicle as well as its Jacobian
:param u:
:param dt:
:param ekf_state:
:param vehicle_params:
:return:
f(x,u), a 3x1 vector corresponding to motion x_{t+1} - x_t given the odometry u.
df/dX, the 3x3 Jacobian of f with respect to the vehicle state (x, y, phi)
"""
# vehicle parameters
a = vehicle_params['a']
b = vehicle_params['b']
L = vehicle_params['L']
H = vehicle_params['H']
# transform between ve and vc
ve = u[0]
alpha = u[1]
vc = ve / (1 - math.tan(alpha) * H / L)
# parameters for motion model
x, y, phi = ekf_state['x'][0], ekf_state['x'][1], ekf_state['x'][2]
# make sure phi is between -pi and +pi
phi = slam_utils.clamp_angle(phi)
# create motion model
motion = np.zeros((3, 1))
motion[0, 0] = dt * (vc * math.cos(phi) - (vc / L) * math.tan(alpha) *
(a * math.sin(phi) + b * math.cos(phi)))
motion[1, 0] = dt * (vc * math.sin(phi) + (vc / L) * math.tan(alpha) *
(a * math.cos(phi) - b * math.sin(phi)))
motion[2, 0] = dt * (vc * math.tan(alpha)) / L
# create prediction model
G = np.identity(3)
G[0, 2] = dt * (-vc * math.sin(phi) - vc / L * math.tan(alpha) *
(a * math.cos(phi) - b * math.sin(phi)))
G[1, 2] = dt * (vc * math.cos(phi) + vc / L * math.tan(alpha) *
(-a * math.sin(phi) - b * math.cos(phi)))
return motion, G
