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 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.
'''
H = np.zeros((2, 3))
H[0, 0] = 1
H[1, 1] = 1
sigma = ekf_state['P']
Q_t = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S = np.linalg.inv(np.matmul(np.matmul(H, sigma[0:3, 0:3]), H.T) + Q_t)
r = np.reshape(gps - ekf_state['x'][0:2], (-1, 1))
d = np.matmul(np.matmul(r.T, S), r)
if (d > 13.8):
return ekf_state
K_last_term = S
K = np.matmul(np.matmul(sigma[0:3, 0:3], H.T), K_last_term)
ekf_state['x'][0:3] += np.matmul(K, r)[:, 0]
ekf_state['P'][0:3, 0:3] = slam_utils.make_symmetric(
np.matmul((np.eye(3) - np.matmul(K, H)), sigma[0:3, 0:3]))
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.
'''
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 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 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 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 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.
##
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 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
###
state = np.reshape(ekf_state['x'], (ekf_state['x'].shape[0], 1))
dim = state.shape[0] - 3
F_x = np.hstack((np.eye(3), np.zeros((3, dim))))
mot, g = motion_model(u, dt, ekf_state, vehicle_params)
new_x = state + np.matmul(np.transpose(F_x), mot)
Rt = np.diag([sigmas['xy']**2, sigmas['xy']**2, sigmas['phi']**2])
Gt = np.vstack((np.hstack((g, np.zeros(
(3, dim)))), np.hstack((np.zeros((dim, 3)), np.eye(dim)))))
ekf_state['P'] = slam_utils.make_symmetric(
np.matmul(Gt, np.matmul(ekf_state['P'], np.transpose(Gt))) +
np.matmul(np.transpose(F_x), np.matmul(Rt, F_x)))
ekf_state['x'] = np.reshape(new_x, (new_x.shape[0], ))
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.
'''
# 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 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):
'''
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 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 initialize_landmark(ekf_state, tree):
'''
Initializes a newly observed landmark in the filter state, increasing its
dimension by 2.
Returns the new ekf_state.
'''
###
x = ekf_state['x'].copy()
P = ekf_state['P'].copy()
z_r = tree[0]
z_beta = tree[1]
x_l = x[0] + z_r * np.cos(z_beta + x[2])
y_l = x[1] + z_r * np.sin(z_beta + x[2])
ekf_state['x'] = np.append(x, np.array([x_l, y_l]))
####
initial_size = P.shape[0]
new_P = 200 * np.eye(initial_size + 2)
new_P[:initial_size, :initial_size] = P
ekf_state['P'] = slam_utils.make_symmetric(new_P)
#####
ekf_state["num_landmarks"] = ekf_state["num_landmarks"] + 1
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.
'''
t_st = ekf_state['x'].copy()
t_st = np.reshape(t_st,(t_st.shape[0],1))
t_cov = ekf_state['P'].copy()
dim = t_st.shape[0]-3
F_x = np.hstack((np.eye(3),np.zeros((3,dim))))
mot, g = motion_model(u,dt,ekf_state,vehicle_params)
new_x = t_st + np.matmul(np.transpose(F_x),mot)
R_t = np.zeros((3,3))
R_t[0,0] = sigmas['xy']*sigmas['xy']
R_t[1,1] = sigmas['xy']*sigmas['xy']
R_t[2,2] = sigmas['phi']*sigmas['phi']
Gt_1 = np.hstack((g, np.zeros((3,dim))))
Gt_2 = np.hstack((np.zeros((dim,3)),np.eye(dim)))
Gt = np.vstack((Gt_1,Gt_2))
new_cov = np.matmul(Gt,np.matmul(t_cov,np.transpose(Gt)))+np.matmul(np.transpose(F_x),np.matmul(R_t,F_x))
new_cov = slam_utils.make_symmetric(new_cov)
new_x = np.reshape(new_x,(new_x.shape[0],))
ekf_state['x'] = new_x
ekf_state['P'] = new_cov
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'].copy()
x = ekf_state['x'].copy()
H = np.zeros([2, x.shape[0]])
H[:2, :2] = np.array([[1, 0], [0, 1]])
Q = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S = H.dot(P).dot(H.T) + Q.T
K = P.dot(H.T).dot(slam_utils.invert_2x2_matrix(S))
r = (gps - x[:2]).reshape([2, 1])
if r.T.dot(slam_utils.invert_2x2_matrix(S)).dot(r) < 13.8:
x_new = x + K.dot(gps - x[:2])
cor_new = (np.eye(x.shape[0]) - K.dot(H)).dot(P)
cor_new = slam_utils.make_symmetric(cor_new)
ekf_state['P'] = cor_new
ekf_state['x'] = x_new
# ekf_state=slam.gps_update(gps, ekf_state, sigmas)
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 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 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.
:param gps:
:param ekf_state:
:param sigmas:
:return:
"""
# measurement noise
gps_noise = sigmas['gps']
# check validity
r = gps.reshape((2, 1)) - ekf_state['x'][:2].copy().reshape((2, 1))
S = slam_utils.make_symmetric(ekf_state['P'][0:2, 0:2].copy() + 10e-6)
d = np.dot(np.dot(r.T, slam_utils.invert_2x2_matrix(S)), r)
if d > 13.8:
return ekf_state
### Kalman Gain ###
x = ekf_state['x'][0:3].copy().reshape((3, 1))
P = ekf_state['P'][0:3, 0:3].copy()
H = np.asarray([[1, 0, 0], [0, 1, 0]])
Q = np.diag((gps_noise**2, gps_noise**2))
# 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))
### Measurement Model ###
# update mu and sigma based on measurement
x_t = x + np.dot(K_t, r)
P_t = np.dot(np.identity(3) - np.dot(K_t, H), P)
ekf_state['x'][0:3] = x_t.reshape(-1)
ekf_state['P'][0:3, 0:3] = slam_utils.make_symmetric(P_t)
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 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 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)
cor_prev = ekf_state['P']
R = np.diag([sigmas['xy']**2, sigmas['xy']**2, sigmas['phi']**2])
cor = G.dot(cor_prev).dot(G.T) + R
cor = slam_utils.make_symmetric(cor)
ekf_state['P'][:3, :3] = cor
ekf_state['x'] = motion
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.
'''
ekf_state['num_landmarks'] += 1
x = ekf_state['x']
xL = np.array([
tree[0] * np.cos(tree[1] + x[2]) + x[0],
tree[0] * np.sin(tree[1] + x[2]) + x[1]
])
ekf_state['x'] = np.concatenate((x, xL), axis=0)
N = ekf_state['P'].shape
pTemp = 10.0**3 * np.eye((N[0] + 2))
pTemp[:-2, :-2] = ekf_state['P']
ekf_state['P'] = slam_utils.make_symmetric(pTemp)
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.
:param u:
:param dt:
:param ekf_state:
:param vehicle_params:
:param sigmas:
:return:
returns the new ekf_state
"""
### Motion Model, Prediction Model ###
N = len(ekf_state['x'])
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
# motion model noise
xy_noise = sigmas['xy']
phi_noise = sigmas['phi']
### Prediction Model ###
# update mu and sigma based on prediction
# mu
motion = motion.reshape(-1)
ekf_state['x'][0] += motion[0]
ekf_state['x'][1] += motion[1]
ekf_state['x'][2] += motion[2]
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
# sigma
G_t = np.block([[G, np.zeros((3, N - 3))],
[np.zeros((N - 3, 3)),
np.identity(N - 3)]])
sigma_t = np.dot(np.dot(G_t, ekf_state['P'].copy()), G_t.T)
sigma_t[0:3, 0:3] += np.diag((xy_noise**2, xy_noise**2, phi_noise**2))
ekf_state['P'] = slam_utils.make_symmetric(sigma_t)
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.array([[1, 0, 0], [0, 1, 0]])
if ((np.shape(ekf_state['x'])[0] - 3) > 0):
landmarks = (np.shape(ekf_state['x'])[0] - 3)
temp = np.zeros((2, landmarks))
H = np.hstack((H, temp))
Q = np.eye(2, 2) * sigmas['gps'] * sigmas['gps']
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)))
r = (gps - ekf_state['x'][:2])
S = (np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T)
mahala = np.matmul(np.matmul(r.T, np.linalg.inv(S)), r)
if mahala > chi2.ppf(0.999, df=2):
return ekf_state
ekf_state['x'] = ekf_state['x'] + np.matmul(K, (gps - ekf_state['x'][:2]))
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.
'''
x_var = sigmas['xy']
y_var = sigmas['xy']
phi_var = sigmas['phi']
sigma_prev = ekf_state['P'][0:3, 0:3]
motion, G = motion_model(u, dt, ekf_state, vehicle_params)
ekf_state['x'][0:3] += motion
R_t = np.diag([x_var**2, y_var**2, phi_var**2])
sigma = slam_utils.make_symmetric(
np.matmul(G, np.matmul(sigma_prev, G.T)) + R_t)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'][0:3, 0:3] = sigma
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.
'''
H = np.zeros((2, 3 + 2 * ekf_state['num_landmarks']))
H[0:2, 0:2] = np.eye(2)
Q = np.diag([sigmas['gps']**2, sigmas['gps']**2])
P = ekf_state['P']
r = gps - ekf_state['x'][0:2]
Sinv = np.linalg.inv(np.matmul(np.matmul(H, P), H.T) + Q.T)
if mahalanobisDist(r, Sinv) <= 13.816:
K = np.matmul(np.matmul(P, H.T), Sinv)
ekf_state['x'] = ekf_state['x'] + np.dot(K, r)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.matmul((np.eye(P.shape[0]) - np.matmul(K, H)), P))
return ekf_state
