def predict(self, u, dt=None):
iR = self.iR # Robot indexes
iM = self.iM # Map indexes
mu_r = self.mu[:iR]
G_x = self.get_g_prime_wrt_state(mu_r, u)
V_x = self.get_g_prime_wrt_motion(mu_r, u)
M_t = self.get_motion_noise_covariance(u)
R_t = V_x @ M_t @ V_x.T
self.R[:R_t.shape[0], :R_t.shape[1]] = R_t
self.G[:G_x.shape[0], :G_x.shape[1]] = G_x
# EKF prediction of the state mean.
self.state_bar.mu[:iR] = get_prediction(mu_r, u)[np.newaxis].T
# EKF prediction of the state covariance.
self.state_bar.Sigma[:iR, :
iR] = G_x @ self.Sigma[:iR, :iR] @ G_x.T + R_t
if iM > 0:
self.state_bar.Sigma[:iR, iR:iR +
iM] = G_x @ self.Sigma[:iR, iR:iR + iM]
self.state_bar.Sigma[iR:iR +
iM, :iR] = self.state.Sigma[iR:iR +
iM, :iR] @ G_x.T
self.mu_bar[2] = wrap_angle(self.mu_bar[2])
self.mu[2] = wrap_angle(self.mu[2])
return self.mu_bar, self.Sigma_bar
def predict(self, u, dt=None): iR = self.iR # Robot indexes iM = self.iM # Map indexes mu_r = self.mu[:iR] G_x = self.get_g_prime_wrt_state(mu_r, u) V_x = self.get_g_prime_wrt_motion(mu_r, u) M_t = self.get_motion_noise_covariance(u) R_t = V_x @ M_t @ V_x.T # self.Wx.append(np.linalg.inv( cholesky(R_t, lower=True).T) ) # EKF prediction of the state mean. self.state_bar.mu[:iR] = get_prediction(mu_r, u)[np.newaxis].T self.state_bar.mu[2] = wrap_angle(self.mu_bar[2]) # EKF prediction of the state covariance. self.state_bar.Sigma[:iR,:iR] = G_x @ self.Sigma[:iR, :iR] @ G_x.T + R_t if iM > 0: self.state_bar.Sigma[:iR, iR:iR+iM] = G_x @ self.Sigma[:iR, iR:iR+iM] self.state_bar.Sigma[iR:iR+iM, :iR] = self.state.Sigma[iR:iR+iM, :iR] @ G_x.T self.mu_bar[2] = wrap_angle(self.mu_bar[2]) self.mu[2] = wrap_angle(self.mu[2]) return self.mu_bar, self.Sigma_bar
def update(self, z):
self.w = np.zeros((self.M))
for i in range(self.M):
distance = wrap_angle(get_observation(self.X[i], z[1])[0])
self.w[i] = gaussian.pdf(z[0], distance, np.sqrt(self._Q))
self.w = self.w / sum(self.w)
# Systematic_resampling (said to be efficient in most situations)
positions = (np.arange(self.M) + np.random.uniform(0, 1)) / self.M
indexes = np.zeros(self.M, 'i')
cum_dist = np.cumsum(self.w)
i, j = 0, 0
while i < self.M:
if positions[i] < cum_dist[j]:
indexes[i] = j
i += 1
else:
j += 1
self.X[:] = self.X[indexes]
self.X[:, -1] = np.array([wrap_angle(x) for x in self.X[:, -1]])
self.w.fill(1.0 / self.M)
# np.random.shuffle(self.X)
stats = get_gaussian_statistics(self.X)
self._state.mu = stats.mu
self._state.Sigma = stats.Sigma
def update(self, z):
iR = self.iR
for batch_i in range(z.shape[0]):
lm_id = z[batch_i, 2]
z[batch_i, 1] = wrap_angle(z[batch_i, 1])
if lm_id not in self.lm_seq:
self.initialize_new_landmark(lm_id, z, batch_i)
j = int(np.where(self.lm_seq == lm_id)[0] +
1) # number of ID in landmark array
delta = self.mu_bar[iR + 2 * j - 2:iR + 2 * j] - self.mu_bar[:2]
q = np.dot(delta, delta.T)
H = self.get_jacobian_H(q, delta, lm_id)
S = H @ self.Sigma_bar @ H.T + self.Q
K = self.Sigma_bar @ H.T @ np.linalg.inv(S)
z_expected = np.array([
sqrt(q),
wrap_angle(atan2(delta[1], delta[0]) - self.mu_bar[2])
])
innovation_vector = z[batch_i, :2] - z_expected
innovation_vector[1] = wrap_angle(innovation_vector[1])
self.state_bar.mu = self.mu_bar[
np.newaxis].T + K @ (innovation_vector)[np.newaxis].T
self.state_bar.Sigma = (np.eye(
(K @ H).shape[0]) - (K @ H)) @ self.Sigma_bar
self.mu_bar[2] = wrap_angle(self.mu_bar[2])
self.mu[2] = wrap_angle(self.mu[2])
self.state.mu = self.state_bar.mu
self.state.Sigma = self.state_bar.Sigma
return self.mu, self.Sigma
def initialize_new_landmark(self, lm_id, z, batch_i): self.lm_seq.append(lm_id) self.iM += 2 r = z[batch_i,0] phi = wrap_angle(z[batch_i,1]) theta = wrap_angle(self.mu_bar[2]) ang = wrap_angle(phi+theta) mu_new = self.mu[:2] + np.array([r*cos(ang), r*sin(ang)]) # position of new landmark self.state.mu = np.append(self.mu, mu_new)[np.newaxis].T self.lm_poses = self.state.mu[3:]
def predict(self, u):
# TODO Implement here the EKF, perdiction part. HINT: use the auxiliary functions imported above from tools.task
self._state_bar.mu = get_prediction(self.mu, u)[np.newaxis].T
alphas = self._alphas
G = np.array([[1, 0, -u[1] * np.sin(wrap_angle(self.mu[2] + u[0]))],
[0, 1, u[1] * np.cos(wrap_angle(self.mu[2] + u[0]))],
[0, 0, 1]])
V = np.array(
[[-u[1] * np.sin(self.mu[2] + u[0]),
np.cos(self.mu[2] + u[0]), 0],
[u[1] * np.cos(self.mu[2] + u[0]),
np.sin(self.mu[2] + u[0]), 0], [1, 0, 1]])
# self._state_bar.Sigma = G.dot(self.Sigma).dot(G.T) + get_motion_noise_covariance(u, alphas)
self._state_bar.Sigma = G.dot(self.Sigma).dot(G.T) + V.dot(
get_motion_noise_covariance(u, alphas)).dot(V.T)
def update(self, z):
# TODO implement correction step
Qt = self._Q
standard_deviation = np.sqrt(Qt)
observation = z[0]
lm_id = z[1]
expected_observation = np.array([
get_observation(self.particles[i], lm_id)[0] for i in range(self.M)
])
angle_deviations = np.array([
wrap_angle(expected_observation[i] - observation)
for i in range(self.M)
])
weights = gaussian().pdf(angle_deviations / standard_deviation)
weights = weights / np.sum(weights) # normalization
self.particles = self.particles[self.low_variance_sampling(weights)]
self.X = self.particles
gaussian_parameters = get_gaussian_statistics(self.particles)
self._state.mu = gaussian_parameters.mu
self._state.Sigma = gaussian_parameters.Sigma
def initialize_new_landmark(self, lm_id, z, batch_i):
self.lm_seq.append(lm_id)
self.iM += 2
r = z[batch_i, 0]
phi = wrap_angle(z[batch_i, 1])
theta = wrap_angle(self.mu_bar[2])
ang = wrap_angle(phi + theta)
mu_new = self.mu[:2] + np.array([r * cos(ang), r * sin(ang)
]) # position of new landmark
self.state.mu = np.append(self.mu, mu_new)[np.newaxis].T
L = self.get_jacobian_L(r, phi, theta)
W = self.get_jacobian_W(r, phi, theta)
self.state_bar.Sigma = self.expandSigma(
self.Sigma_bar, L, W, self.Q) # expand covariance matrix
def update(self, u, z): for batch_i in range(z.shape[0]): # for all observed features self.N += 1 self.M = len(self.lm_seq) self.K = self.N * z.shape[0] lm_id = z[batch_i,2] if (lm_id not in self.lm_seq): # lm never seen before self.initialize_new_landmark(lm_id, z, batch_i) # G, H, J calculation j = int(np.where(self.lm_seq==lm_id)[0] + 1) # number of ID in landmark array self.js.append(j) delta = self.mu_bar[self.iR+2*j-2 : self.iR+2*j] - self.mu_bar[:2] q = np.dot( delta, delta.T ) z_expected = np.array([ sqrt(q), wrap_angle( atan2(delta[1],delta[0])-self.mu_bar[2] ) ]) self.H.append( self.get_jacobian_Hx(q, delta) ) self.J.append( self.get_jacobian_J(q, delta) ) self.G.append( self.get_g_prime_wrt_state(self.mu[:self.iR], u) ) # self.H = self.get_jacobian_Hx(q, delta) # self.J = self.get_jacobian_J(q, delta) # self.G = self.get_g_prime_wrt_state(self.mu[:self.iR], u) self.x_traj = np.vstack((self.x_traj, self.state.mu[:3])) A = self.adjacency_matrix(lm_id) # x0 = self.mu_bar # linearization point # a = x0 - get_prediction(self.mu, u) # c = z[batch_i,:2] - z_expected b = np.random.rand(A.shape[0]) # TODO: should consist of a-s and c-s # delta = self.QR_factorization(A,b) # self.state_bar.mu[:3] = (self.mu_bar[:3] + delta[:3])[np.newaxis].T # print(len(self.x_traj), len(delta[:-len(self.lm_seq)*2])) # self.state.mu = self.state_bar.mu self.mu_bar[2] = wrap_angle(self.mu_bar[2]) self.mu[2] = wrap_angle(self.mu[2]) self.state.mu = self.state_bar.mu self.state.Sigma = self.state_bar.Sigma return self.mu, self.Sigma
def b_matrix(self):
state = self.state.copy()
b = np.zeros(self.number_of_factors)
# b[:3] = self.initial_state - self.state[:3]
for i in range(self.motions.shape[0]):
odom_id = 3 * i + 3
W = self.get_pre_mult_matrix_motion(state[3 * i:3 * i + 3],
self.motions[i])
new_robot_state = get_prediction(state[odom_id - 3:odom_id],
self.motions[i])
diff = new_robot_state - state[odom_id:odom_id + 3]
diff[2] = wrap_angle(diff[2])
b[odom_id:odom_id + 3] = W @ diff
# print(b)
for (i, j) in itertools.product(range(self.observations.shape[0]),
range(self.observations.shape[1])):
odom_id = 3 * i + 3
x = state[odom_id:odom_id + 3]
lm_idx = self.number_of_robot_pos_nodes + 2 * int(
self.observations[i, j, 2])
m = state[lm_idx:lm_idx + 2]
W = self.get_pre_mult_matrix_observation()
new_observation = get_observation_b(x, m)
diff = new_observation - self.observations[i, j, :2]
diff[1] = wrap_angle(diff[1])
factor_idx = self.number_of_robot_pos_nodes + 2 * (
i * self.observations.shape[1] + j)
b[factor_idx:factor_idx + 2] = W @ diff
# print(b)
return b
def update(self, z):
# TODO implement correction step
mu_bar = self.mu_bar
Sigma_bar = self.Sigma_bar
land_x = self._field_map.landmarks_poses_x[int(z[-1])]
land_y = self._field_map.landmarks_poses_y[int(z[-1])]
H = np.array([[
(land_y - mu_bar[1])/((land_x - mu_bar[0]) ** 2 + (land_y - mu_bar[1]) ** 2),
-(land_x - mu_bar[0])/((land_x - mu_bar[0]) ** 2 + (land_y - mu_bar[1]) ** 2),
-1
]])
innovation = wrap_angle((z - get_expected_observation(mu_bar, z[-1]))[0])
K = Sigma_bar @ H.T / (H @ Sigma_bar @ H.T + self._Q)
mu = mu_bar + K.reshape(-1) * innovation
# mu[-1] = wrap_angle(mu[-1])
Sigma = (np.eye(self.state_dim) - K @ H) @ Sigma_bar
self._state.mu = mu[np.newaxis].T
self._state.Sigma = Sigma
def get_jacobian_L(r, phi, theta):
# L = dh^(-1) / dy, y = state
return np.array([[1, 0, -r * sin(wrap_angle(phi + theta))],
[0, 1, r * cos(wrap_angle(phi + theta))]])
def get_jacobian_W(r, phi, theta):
# W = dh^(-1) / dz, z = [r, phi] - lm measurement wrt robot's pose
return np.array(
[[cos(wrap_angle(phi + theta)), -r * sin(wrap_angle(phi + theta))],
[sin(wrap_angle(phi + theta)), r * cos(wrap_angle(phi + theta))]])
def update(self, z):
# TODO implement correction step
Q = self._Q
G = -np.eye(len(self._state_est.mu))
initial_pre_multiply_for_transition = np.linalg.cholesky(
np.linalg.inv(self._state_est.Sigma)).T
G[0:3, 0:3] = initial_pre_multiply_for_transition.dot(G[0:3, 0:3])
self._Sigma = np.zeros((3, 3, len(self._state_est.mu) // 3))
self._Sigma[0:3, 0:3, 0] = self._state_est.Sigma
for i in range(3, len(self._state_est.mu), 3):
Mi = get_motion_noise_covariance(self.u[i:i + 3], self._alphas)
Gi, Vi = state_jacobian(self.mu_est[i - 3:i], self.u[i:i + 3])
G[i:i + 3, i - 3:i] = Gi
pre_multiply_for_transition = np.linalg.cholesky(
np.linalg.inv(Vi.dot(Mi.dot(Vi.T)))).T
self._Sigma[:, :, i // 3] = Vi.dot(Mi.dot(Vi.T))
G[i:i + 3, :] = pre_multiply_for_transition.dot(G[i:i + 3, :])
self._a[i:i +
3] = self.mu_est[i:i + 3][np.newaxis].T - get_prediction(
self.mu_est[i - 3:i], self.u[i:i + 3])[np.newaxis].T
self._a[i:i + 3] = pre_multiply_for_transition.dot(self._a[i:i +
3])
# Observation landmark vectors are created
observed_lds = []
for zk in z:
ld_id = int(zk[2])
observed_lds.extend([ld_id])
zk_xy = get_landmark_xy(self.mu_est[-3:], zk[:2])
L, W = inverse_observation_jacobian(self.mu_est[-3:], zk)
if not ld_id in self._ld_ids:
self._ld_ids.extend([ld_id])
if self._ld_est.size == 0:
self._ld_est = zk_xy
else:
self._ld_est = np.vstack((self._ld_est, zk_xy))
if self._Sigma_ld.size == 0:
self._Sigma_ld = (L.dot(self.Sigma[:, :, -1].dot(L.T)) +
W.dot(Q.dot(W.T)))[:, :, np.newaxis]
else:
self._Sigma_ld = np.concatenate(
(self._Sigma_ld,
(L.dot(self.Sigma[:, :, -1].dot(L.T)) +
W.dot(Q.dot(W.T)))[:, :, np.newaxis]),
axis=2)
if self._z.size == 0:
self._z = np.array([[zk[0]], [zk[1]]])
else:
self._z = np.vstack((self._z, np.array([[zk[0]], [zk[1]]])))
if self._observed_ld_ids.size == 0:
self._observed_ld_ids = np.array(observed_lds)
self._observed_ld_ids = np.vstack(
(self._observed_ld_ids, np.array(observed_lds)))
H = np.array([])
J = np.array([])
h = np.array([])
pre_multiply_for_observation = np.linalg.cholesky(np.linalg.inv(Q)).T
for i in range(3, len(self._state_est.mu), 3):
for ld_id in self._observed_ld_ids[i // 3]:
j = self._ld_ids.index(ld_id)
Hk, Jk = observation_jacobian(self.mu_est[i:i + 3],
self.ld_est[2 * j:2 * j + 2])
hk = get_expected_observation(
self.mu_est[i:i + 3],
self.ld_est[2 * j:2 * j + 2][np.newaxis].T)
L, W = inverse_observation_jacobian(self.mu_est[i:i + 3],
hk.T[0])
self._Sigma_ld[:, :, j] = L.dot(self.Sigma[:, :, i // 3].dot(
L.T)) + W.dot(Q.dot(W.T))
H1 = np.zeros((2, len(self.mu_est)))
H1[:, i:i + 3] = Hk
H1 = pre_multiply_for_observation.dot(H1)
if H.size == 0:
H = H1
else:
H = np.vstack((H, H1))
J1 = np.zeros((2, len(self.ld_est)))
J1[:, 2 * j:2 * j + 2] = Jk
J1 = pre_multiply_for_observation.dot(J1)
if J.size == 0:
J = J1
else:
J = np.vstack((J, J1))
if h.size == 0:
h = hk
else:
h = np.vstack((h, hk))
Zeros = np.zeros((len(self.mu_est), len(self.ld_est)))
A = np.block([[G, Zeros], [H, J]])
self._A = A
self._c = self._z - h
for i in range(0, len(self._c), 2):
self._c[i:i + 2] = pre_multiply_for_observation.dot(self._c[i:i +
2])
b = np.vstack((self._a, self._c))
# So, for now we have A and b
delta = np.linalg.inv(A.T.dot(A)).dot(A.T.dot(b))
res = (A.dot(delta) - b)
if self._chi.size == 0:
self._chi = (res.T.dot(res))[0]
else:
self._chi = np.hstack((self._chi, (res.T.dot(res))[0]))
# Update
self._state_est.mu = self._state_est.mu + delta[:len(self._state_est.mu
)]
self._ld_est = self._ld_est + delta[len(self._state_est.mu):]
for i in range(0, len(self._state_est.mu), 3):
self._state_est.mu[i + 2] = wrap_angle(self._state_est.mu[i + 2])