def GetTrueMeasurements(self, robot_pose):
'''
Get the true measurements for a robot at robot_pose using a noise-free
range sensor.
===INPUT===
robot_pose: of class RobotPose, the pose at which the robot measures
===OUTPUT===
a list of Measurement
'''
if (not (isinstance(robot_pose, RobotPose))):
raise FeatureBasedWorldException('robot_pose should be an instance of RobotPose.')
m = []
for feature in self.features:
dx = feature.x - robot_pose.x
dy = feature.y - robot_pose.y
d = np.sqrt(dx * dx + dy * dy) # true distance
phi = wrap_angle(np.arctan2(dy, dx) - robot_pose.theta) # true relative bearing
m.append(Measurement(d, phi, feature.s))
return m
def measurement_model(self, measurements, pose):
'''
Compute the probability of the particle and the measurements given
===INPUT===
measurement: a list of Measurement
pose: the proposed pose
===OUTPUT===
a decimal representing the probability
'''
w = 1.0
for m in measurements:
j = m.s # assuming the signature gives the correspondence
fs = self.true_map.GetFeature(j)
delta_x = fs.x - pose.x
delta_y = fs.y - pose.y
d = np.sqrt(delta_x * delta_x + delta_y * delta_y)
b = np.arctan2(delta_y, delta_x) - pose.theta
# deviation from expected measurement
n_r = d - m.r
n_phi = wrap_angle(b - m.phi)
n_s = fs.s - m.s
# compute probability of measurement.
w = w * self.measurement_noise.GetProbability([n_r, n_phi, n_s])
return w
def GetTrueMeasurements(self, robot_pose):
'''
Get the true measurements for a robot at robot_pose using a noise-free
range sensor.
===INPUT===
robot_pose: of class RobotPose, the pose at which the robot measures
===OUTPUT===
a list of Measurement
'''
if (not (isinstance(robot_pose, RobotPose))):
raise FeatureBasedWorldException(
'robot_pose should be an instance of RobotPose.')
m = []
for feature in self.features:
dx = feature.x - robot_pose.x
dy = feature.y - robot_pose.y
d = np.sqrt(dx * dx + dy * dy) # true distance
phi = wrap_angle(np.arctan2(dy, dx) -
robot_pose.theta) # true relative bearing
m.append(Measurement(d, phi, feature.s))
return m
def measurement_model(self, measurements, pose):
'''
Compute the probability of the particle and the measurements given
===INPUT===
measurement: a list of Measurement
pose: the proposed pose
===OUTPUT===
a decimal representing the probability
'''
w = 1.0
for m in measurements:
j = m.s # assuming the signature gives the correspondence
fs = self.true_map.GetFeature(j)
delta_x = fs.x - pose.x
delta_y = fs.y - pose.y
d = np.sqrt(delta_x * delta_x + delta_y * delta_y)
b = np.arctan2(delta_y, delta_x) - pose.theta
# deviation from expected measurement
n_r = d - m.r
n_phi = wrap_angle(b - m.phi)
n_s = fs.s - m.s
# compute probability of measurement.
w = w * self.measurement_noise.GetProbability([n_r, n_phi, n_s])
return w
def measurement_update(self, measurement):
rp = self.get_pose()
# convert measurement into a matrix
z = np.matrix([[measurement.r], \
[measurement.phi], \
[measurement.s]])
# line 10
j = measurement.s # assuming the signature gives the correspondence
# line 11
fs = self.true_map.GetFeature(j)
delta_x = fs.x - rp.x
delta_y = fs.y - rp.y
q = delta_x * delta_x + delta_y * delta_y
# line 12, predicted measurement
z_hat = np.matrix([[np.sqrt(q)], \
[wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
[fs.s]])
# line 13
H_t = np.matrix(np.eye(3))
H_t[0, 0] = -delta_x / np.sqrt(q)
H_t[0, 1] = -delta_y / np.sqrt(q)
H_t[1, 0] = delta_y / q
H_t[1, 1] = delta_x / q
H_t[1, 2] = -1.0
# line 14
S = H_t * self.sigma * H_t.T + self.R
# line 15
K_t = self.sigma * H_t.T * np.linalg.inv(S)
# line 16
self.mu = self.mu + K_t * (z - z_hat)
# line 17
self.sigma = self.sigma - K_t * H_t * self.sigma
def measurement_update(self, measurement):
rp = self.get_pose()
# convert measurement into a matrix
z = np.matrix([[measurement.r], \
[measurement.phi], \
[measurement.s]])
# line 10
j = measurement.s # assuming the signature gives the correspondence
# line 11
fs = self.true_map.GetFeature(j)
delta_x = fs.x - rp.x
delta_y = fs.y - rp.y
q = delta_x * delta_x + delta_y * delta_y
# line 12, predicted measurement
z_hat = np.matrix([[np.sqrt(q)], \
[wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
[fs.s]])
# line 13
H_t = np.matrix(np.eye(3))
H_t[0, 0] = -delta_x / np.sqrt(q)
H_t[0, 1] = -delta_y / np.sqrt(q)
H_t[1, 0] = delta_y / q
H_t[1, 1] = delta_x / q
H_t[1, 2] = -1.0
# line 14
S = H_t * self.sigma * H_t.T + self.R
# line 15
K_t = self.sigma * H_t.T * np.linalg.inv(S)
# line 16
self.mu = self.mu + K_t * (z - z_hat)
# line 17
self.sigma = self.sigma - K_t * H_t * self.sigma
def measurement_update(self, measurement):
'''
update the state and covar estimate using the measurement
c.f page 314, Probabilistic Robotics
'''
print("<---- measurement_update ---->")
print(measurement)
rp = self.get_pose()
# convert measurement into a matrix
z = measurement.ToMatrix()
# line 8
j = measurement.s # assuming the signature gives the correspondence
print("correspondence = " + str(j))
# line 9
if (not self.feature_seen[j]):
f = FeatureState(rp.x + measurement.r * np.cos(measurement.phi + rp.theta), \
rp.y + measurement.r * np.sin(measurement.phi + rp.theta), \
measurement.s)
# line 10, initialize feature state
self.set_feature(j, f)
self.feature_seen[j] = True
print("newly observed " + str(f))
fs = self.get_feature(j)
# line 12
delta_x = fs.x - rp.x
delta_y = fs.y - rp.y
# line 13
q = delta_x * delta_x + delta_y * delta_y
#~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q))
# line 14, predicted measurement
z_hat = np.matrix([[np.sqrt(q)], \
[wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], \
[fs.s]])
#~ print("z_hat = ")
#~ print(z_hat)
# line 15
F_xj = self.get_feature_selection_matrix(j)
# line 16
h_t_i = np.matrix(np.zeros((3, 6)))
h_t_i[0, 0] = -delta_x * np.sqrt(q)
h_t_i[0, 1] = -delta_y * np.sqrt(q)
h_t_i[0, 3] = delta_x * np.sqrt(q)
h_t_i[0, 4] = delta_y * np.sqrt(q)
h_t_i[1, 0] = delta_y
h_t_i[1, 1] = -delta_x
h_t_i[1, 2] = -q
h_t_i[1, 3] = -delta_y
h_t_i[1, 4] = delta_x
h_t_i[2, 5] = q
H_t = h_t_i * F_xj / q
#~ print("H_t=")
#~ print(H_t)
# line 17
K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T +
self.R)
# line 18
self.mu += K_t * (z - z_hat)
print("mu=")
print(self.mu)
# line 19
self.sigma += -K_t * H_t * self.sigma
def linearize(self, u_1t, z_1t, c_1t, mu_0t):
'''
Linearize the constraints (neg log likelihoods) around the given state estimate.
c.f. p347 ~ p348, Probabilistic Robotics
=== INPUT ===
u_1t: a list of RobotAction. The k-th item is the action at step k+1.
z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1.
c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1.
mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state.
Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features.
=== OUTPUT ===
omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix.
ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector.
'''
num_states = self.T*3 + 3 + self.N*3
# line 2
omega = np.matrix(np.zeros((num_states, num_states)))
ksi = np.matrix(np.zeros((num_states, 1)))
# line 3. Fix the origin.
omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY])
rp = RobotPose(0.0, 0.0, 0.0) # change this
# line 4
for t in range(1, self.T+1):
mu_last = self.get_pose(mu_0t, t-1) # robot pose at previous step
# line 5
x_t_hat = self.get_pose(mu_0t, t)
rp_delta = x_t_hat - mu_last
# line 6
G_t = np.matrix(np.eye(3))
G_t[0, 2] = -rp_delta[1, 0]
G_t[1, 2] = rp_delta[0, 0]
# line 7
tmp = np.matrix(np.zeros((3, 6)))
tmp[0:3, 0:3] = -G_t
tmp[0:3, 3:6] = np.eye(3)
F_t = self.get_motion_update_selection_matrix(t)
omega += F_t.T * tmp.T * self.R_inv * tmp * F_t # TODO: find a way to do local update
# line 8
ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last)
# line 10
for t in range(1, self.T+1):
z_t = self.get_measurements(z_1t, t) # measurements at step t
c_t = self.get_correspondence(c_1t, t) # correspondences at step t
mu_t = self.get_pose(mu_0t, t) # robot pose at step t in matrix form
# line 12
for i in range(len(z_t)):
z_t_i = z_t[i].ToMatrix()
# line 13
j = c_t[i]
# if first seen then add feature to state. NOTE: this part is missing in the book
if (not self.feature_seen[j]):
f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \
mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \
z_t_i[2, 0])
self.set_feature(mu_0t, j, f)
self.feature_seen[j] = True
print("newly observed " + str(f))
mu_j = self.get_feature(mu_0t, j) # map feature in matrix form
# line 14
delta_x = mu_j[0, 0] - mu_t[0, 0]
delta_y = mu_j[1, 0] - mu_t[1, 0]
# line 15
q = delta_x * delta_x + delta_y * delta_y
# line 16
z_t_i_hat = np.matrix([[np.sqrt(q)], \
[wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \
[z_t_i[2, 0]]])
# line 17
tmp = np.matrix(np.zeros((3, 6)))
tmp[0, 0] = -delta_x * np.sqrt(q)
tmp[0, 1] = -delta_y * np.sqrt(q)
tmp[0, 3] = delta_x * np.sqrt(q)
tmp[0, 4] = delta_y * np.sqrt(q)
tmp[1, 0] = delta_y
tmp[1, 1] = -delta_x
tmp[1, 2] = -q
tmp[1, 3] = -delta_y
tmp[1, 4] = delta_x
tmp[2, 5] = q
H_t_i = tmp / q
# line 18
F_xj = self.get_measurement_update_selection_matrix(t, j)
omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj # TODO: find a way to do local update
# line 19
tmp = np.matrix(np.zeros((6, 1)))
tmp[0:3, :] = mu_t
tmp[3:6, :] = mu_j
ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat + H_t_i * tmp)
return omega, ksi
def linearize(self, u_1t, z_1t, c_1t, mu_0t):
'''
Linearize the constraints (neg log likelihoods) around the given state estimate.
c.f. p347 ~ p348, Probabilistic Robotics
=== INPUT ===
u_1t: a list of RobotAction. The k-th item is the action at step k+1.
z_1t: a list of (a list of Measurement). The k-th item contains the observations at step k+1.
c_1t: a list of (a list of integers). The k-th item contains the correspondence for the observations at step k+1.
mu_0t: a (3*T+3+3*N)-by-1 matrix, i.e. the full SLAM state.
Rows 0:(3*T+3) are robot poses, while rows (3*T+3):(3*T+3+3*N) are map features.
=== OUTPUT ===
omega: a (3*T+3+3*N)-by-(3*T+3+3*N) matrix, i.e. the informatio matrix.
ksi: a (3*T+3+3*N)-by-1 matrix, i.e. the information vector.
'''
num_states = self.T * 3 + 3 + self.N * 3
# line 2
omega = np.matrix(np.zeros((num_states, num_states)))
ksi = np.matrix(np.zeros((num_states, 1)))
# line 3. Fix the origin.
omega[0:3, 0:3] = np.diag([INFINITY, INFINITY, INFINITY])
rp = RobotPose(0.0, 0.0, 0.0) # change this
# line 4
for t in range(1, self.T + 1):
mu_last = self.get_pose(mu_0t,
t - 1) # robot pose at previous step
# line 5
x_t_hat = self.get_pose(mu_0t, t)
rp_delta = x_t_hat - mu_last
# line 6
G_t = np.matrix(np.eye(3))
G_t[0, 2] = -rp_delta[1, 0]
G_t[1, 2] = rp_delta[0, 0]
# line 7
tmp = np.matrix(np.zeros((3, 6)))
tmp[0:3, 0:3] = -G_t
tmp[0:3, 3:6] = np.eye(3)
F_t = self.get_motion_update_selection_matrix(t)
omega += F_t.T * tmp.T * self.R_inv * tmp * F_t # TODO: find a way to do local update
# line 8
ksi += F_t.T * tmp.T * self.R_inv * (x_t_hat - G_t * mu_last)
# line 10
for t in range(1, self.T + 1):
z_t = self.get_measurements(z_1t, t) # measurements at step t
c_t = self.get_correspondence(c_1t, t) # correspondences at step t
mu_t = self.get_pose(mu_0t,
t) # robot pose at step t in matrix form
# line 12
for i in range(len(z_t)):
z_t_i = z_t[i].ToMatrix()
# line 13
j = c_t[i]
# if first seen then add feature to state. NOTE: this part is missing in the book
if (not self.feature_seen[j]):
f = FeatureState(mu_t[0, 0] + z_t_i[0, 0] * np.cos(z_t_i[1, 0] + mu_t[2, 0]), \
mu_t[1, 0] + z_t_i[0, 0] * np.sin(z_t_i[1, 0] + mu_t[2, 0]), \
z_t_i[2, 0])
self.set_feature(mu_0t, j, f)
self.feature_seen[j] = True
print("newly observed " + str(f))
mu_j = self.get_feature(mu_0t, j) # map feature in matrix form
# line 14
delta_x = mu_j[0, 0] - mu_t[0, 0]
delta_y = mu_j[1, 0] - mu_t[1, 0]
# line 15
q = delta_x * delta_x + delta_y * delta_y
# line 16
z_t_i_hat = np.matrix([[np.sqrt(q)], \
[wrap_angle(np.arctan2(delta_y, delta_x) - mu_t[2, 0])], \
[z_t_i[2, 0]]])
# line 17
tmp = np.matrix(np.zeros((3, 6)))
tmp[0, 0] = -delta_x * np.sqrt(q)
tmp[0, 1] = -delta_y * np.sqrt(q)
tmp[0, 3] = delta_x * np.sqrt(q)
tmp[0, 4] = delta_y * np.sqrt(q)
tmp[1, 0] = delta_y
tmp[1, 1] = -delta_x
tmp[1, 2] = -q
tmp[1, 3] = -delta_y
tmp[1, 4] = delta_x
tmp[2, 5] = q
H_t_i = tmp / q
# line 18
F_xj = self.get_measurement_update_selection_matrix(t, j)
omega += F_xj.T * H_t_i.T * self.Q_inv * H_t_i * F_xj # TODO: find a way to do local update
# line 19
tmp = np.matrix(np.zeros((6, 1)))
tmp[0:3, :] = mu_t
tmp[3:6, :] = mu_j
ksi += F_xj.T * H_t_i.T * self.Q_inv * (z_t_i - z_t_i_hat +
H_t_i * tmp)
return omega, ksi
def measurement_update(self, measurement):
"""
update the state and covar estimate using the measurement
c.f page 314, Probabilistic Robotics
"""
print("<---- measurement_update ---->")
print(measurement)
rp = self.get_pose()
# convert measurement into a matrix
z = measurement.ToMatrix()
# line 8
j = measurement.s # assuming the signature gives the correspondence
print("correspondence = " + str(j))
# line 9
if not self.feature_seen[j]:
f = FeatureState(
rp.x + measurement.r * np.cos(measurement.phi + rp.theta),
rp.y + measurement.r * np.sin(measurement.phi + rp.theta),
measurement.s,
)
# line 10, initialize feature state
self.set_feature(j, f)
self.feature_seen[j] = True
print("newly observed " + str(f))
fs = self.get_feature(j)
# line 12
delta_x = fs.x - rp.x
delta_y = fs.y - rp.y
# line 13
q = delta_x * delta_x + delta_y * delta_y
# ~ print("delta_x = " + str(delta_x) + " delta_y = " + str(delta_y) + " q= " + str(q))
# line 14, predicted measurement
z_hat = np.matrix([[np.sqrt(q)], [wrap_angle(np.arctan2(delta_y, delta_x) - rp.theta)], [fs.s]])
# ~ print("z_hat = ")
# ~ print(z_hat)
# line 15
F_xj = self.get_feature_selection_matrix(j)
# line 16
h_t_i = np.matrix(np.zeros((3, 6)))
h_t_i[0, 0] = -delta_x * np.sqrt(q)
h_t_i[0, 1] = -delta_y * np.sqrt(q)
h_t_i[0, 3] = delta_x * np.sqrt(q)
h_t_i[0, 4] = delta_y * np.sqrt(q)
h_t_i[1, 0] = delta_y
h_t_i[1, 1] = -delta_x
h_t_i[1, 2] = -q
h_t_i[1, 3] = -delta_y
h_t_i[1, 4] = delta_x
h_t_i[2, 5] = q
H_t = h_t_i * F_xj / q
# ~ print("H_t=")
# ~ print(H_t)
# line 17
K_t = self.sigma * H_t.T * np.linalg.inv(H_t * self.sigma * H_t.T + self.R)
# line 18
self.mu += K_t * (z - z_hat)
print("mu=")
print(self.mu)
# line 19
self.sigma += -K_t * H_t * self.sigma
