def predict(self, motion_model_transform, command, noise):
sigma_points, w_m, w_c = compute_sigma_points(self.mean, self.cov,
self.lambd, self.alpha,
self.beta)
# Normalize
for i in range(sigma_points.shape[1]):
sigma_points[2, i] = normalize_angle(sigma_points[2, i])
# Angles are normalized in motion model
sigma_points = motion_model_transform(sigma_points, command)
# TODO: rewrite to normalize differences
self.mean, self.cov = recover_gaussian(sigma_points, w_m, w_c)
# fix angles
average_theta = 0
avg_x = avg_y = 0
for i in range(sigma_points.shape[1]):
avg_x = avg_x + w_m[i] * math.cos(sigma_points[2, i])
avg_y = avg_y + w_m[i] * math.sin(sigma_points[2, i])
average_theta = normalize_angle(np.arctan2(avg_y, avg_x))
self.mean[2] = average_theta
# fix angles in covariance matrix too
for i in range(self.cov.shape[1]):
self.cov[2, i] = normalize_angle(self.cov[2, i])
Rt = np.diag(noise)
self.cov = self.cov + Rt
return self.mean, self.cov
def correct(self, measurements, sensor_noise, landmark_map):
normalizer = 0
for i, particle in enumerate(self.particles):
weight, pose = particle
rx = pose.item(0)
ry = pose.item(1)
rtheta = pose.item(2)
vrange, vbearing = sensor_noise
# Matrix of measurement differences
Zdiff = np.matrix([])
for reading in measurements:
lid, srange, sbearing = reading
# Sensor measurement
z_measured = np.matrix([srange, sbearing]).T
lx, ly = landmark_map.get(lid)
dx = lx - rx
dy = ly - ry
delta = np.matrix([dx, dy]).T
q = delta.T * delta
# Expected (predicted) measurement
z_expected = np.matrix([
math.sqrt(q),
normalize_angle(np.arctan2(dy, dx) - rtheta)
]).T
# Difference between measured and expected
diff = z_expected - z_measured
diff[1] = normalize_angle(diff.item(1))
# Collect all measurement differences
Zdiff = np.concatenate(
(Zdiff, diff)) if Zdiff.size else np.copy(diff)
# Making sensor noise matrix with different diagonal elements
Qdiag = np.array([[vrange, vbearing]
for i in range(len(measurements))])
# Flatten the array
Qdiag.shape = (len(sensor_noise) * len(measurements), )
Qt = np.diag(Qdiag)
# Qt = np.eye(Zdiff.shape[0]) * 0.1
# Normal distribution is probably a good idea
# Highest weight when (z_expected - z_measured) is 0
denom = 1 / math.sqrt(np.linalg.det(2 * math.pi * Qt))
new_weight = denom * math.exp(
-1 / 2 * Zdiff.T * np.linalg.pinv(Qt) * Zdiff)
normalizer = normalizer + new_weight
self.particles[i] = (new_weight, np.copy(pose))
self.particles = [(weight / normalizer, pose)
for weight, pose in self.particles]
def odometry_command(pose, command):
rot1, trans, rot2 = command
theta_old = normalize_angle(pose.item(2))
normalized = normalize_angle(theta_old + rot1)
update_vec = np.matrix([
trans * math.cos(normalized),
trans * math.sin(normalized),
normalize_angle(rot1 + rot2)
]).T
pose = pose + update_vec
pose[2] = normalize_angle(pose.item(2))
return np.copy(pose)
def predict(self, command):
self._mean = self.motion_command(self.mean, command)
# TODO: Put in motion model
theta_n = normalize_angle(self.mean.item(2))
rot1, trans, rot2 = command
ang = normalize_angle(theta_n + rot1)
Gt = np.matrix([
[1, 0, -trans * math.sin(ang)],
[0, 1, trans * math.cos(ang)],
[0, 0, 1],
])
# motion noise
Rt = np.matrix([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.01]])
self._cov = Gt * self.cov * Gt.T + Rt
return self._mean, self._cov
def correct(self, measurements, local_map):
# measurement noise
Qt = np.eye(self._mean.size - 1) * 0.01
rx = self._mean.item(0)
ry = self._mean.item(1)
rtheta = normalize_angle(self._mean.item(2))
# TODO: Fix kalman gain calculation
for reading in measurements:
# TODO: Put in measurement model
lid, srange, sbearing = reading
z_measured = np.matrix([srange, normalize_angle(sbearing)]).T
# Expected observation
lx, ly = local_map.get(lid)
dx = lx - rx
dy = ly - ry
delta = np.matrix([dx, dy]).T
q = delta.T * delta
z_expected = np.matrix(
[math.sqrt(q),
normalize_angle(np.arctan2(dy, dx) - rtheta)]).T
# Jacobian
Ht = np.matrix([[-math.sqrt(q) * dx, -math.sqrt(q) * dy, 0],
[dy, -dx, -q]])
Ht = np.multiply((1.0 / q), Ht)
Kgain = self._cov * Ht.T * np.linalg.inv(Ht * self._cov * Ht.T +
Qt)
diff = z_measured - z_expected
diff[1] = normalize_angle(diff.item(1))
self._mean = self._mean + Kgain * diff
self._cov = (np.eye(self._mean.size) - Kgain * Ht) * self._cov
self.mean = np.copy(self._mean)
self.cov = np.copy(self._cov)
return self.mean, self.cov
def measurement_model_transform(points, measurement, landmark_map):
z_points = np.zeros((points.shape[0] - 1, points.shape[1]))
for i in range(points.shape[1]):
rx = points[0, i]
ry = points[1, i]
rtheta = points[2, i]
lid, srange, sbearing = measurement
z_measured = np.matrix([srange, normalize_angle(sbearing)]).T
lx, ly = landmark_map.get(lid)
dx = lx - rx
dy = ly - ry
delta = np.matrix([dx, dy]).T
q = delta.T * delta
z_expected = np.matrix(
[math.sqrt(q),
normalize_angle(np.arctan2(dy, dx) - rtheta)]).T
z_points[:, [i]] = z_expected
return z_points
def odometry_sample(pose, command, noise, sample=None):
if not sample:
raise ValueError("Provide a sampler")
rot1, trans, rot2 = command
r1_noise, t_noise, r2_noise = noise
theta_old = normalize_angle(pose.item(2))
rot1_h = rot1 - sample(r1_noise)
trans_h = trans - sample(t_noise)
rot2_h = rot2 - sample(r2_noise)
normalized = normalize_angle(theta_old + rot1_h)
update_vec = np.matrix([
trans_h * math.cos(normalized),
trans_h * math.sin(normalized),
normalize_angle(rot1_h + rot2_h)
]).T
temp = pose + update_vec
temp[2] = normalize_angle(temp.item(2))
return np.copy(temp)
def predict(self, command):
robot_pose = self.mean[:self.rsize, :]
self.mean[:self.rsize, :] = self.motion_command(robot_pose, command)
# TODO: Put in motion model
theta_n = normalize_angle(self.mean.item(2))
rot1, trans, rot2 = command
ang = normalize_angle(theta_n + rot1)
Gtx = np.matrix([
[1, 0, -trans * math.sin(ang)],
[0, 1, trans * math.cos(ang)],
[0, 0, 1],
])
lmsize = self.mean.shape[0] - self.rsize
r1zeros = np.zeros((self.rsize, lmsize))
r2zeros = np.copy(r1zeros.T)
gr1 = np.concatenate((Gtx, r1zeros), axis=1)
gr2 = np.concatenate((r2zeros, np.eye(lmsize)), axis=1)
Gt = np.concatenate((gr1, gr2))
# motion noise
Rtx = np.matrix([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.01]])
rr1zeros = np.zeros((self.rsize, lmsize))
rr2zeros = np.copy(rr1zeros.T)
rr1 = np.concatenate((Rtx, rr1zeros), axis=1)
rr2 = np.concatenate((rr2zeros, np.zeros((lmsize, lmsize))), axis=1)
Rt = np.concatenate((rr1, rr2))
self.cov = Gt * self.cov * Gt.T + Rt
return self.mean, self.cov
def update_map(self):
with self.lock:
if not self.last_scan_msg or not self.last_tf_msg:
return
rx = self.last_tf_msg.transform.translation.x
ry = self.last_tf_msg.transform.translation.y
quat = (
self.last_tf_msg.transform.rotation.x,
self.last_tf_msg.transform.rotation.y,
self.last_tf_msg.transform.rotation.z,
self.last_tf_msg.transform.rotation.w,
)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(quat)
rtheta = normalize_angle(yaw)
rob_pose = np.matrix([rx, ry, rtheta]).T
# TODO: Calculate laser offset
range_scan = {
'ranges': np.array(self.last_scan_msg.ranges,
dtype=np.float32),
'maximum_range': self.last_scan_msg.range_max,
'start_angle': self.last_scan_msg.angle_min,
'angular_resolution': self.last_scan_msg.angle_increment,
'laser_offset': np.matrix([0, 0, 0])
}
s_angle = range_scan['start_angle']
ang_res = range_scan['angular_resolution']
num_beams = len(range_scan['ranges'])
print(s_angle, ang_res, s_angle + ang_res * num_beams)
print(
self.last_scan_msg.angle_min,
self.last_scan_msg.angle_increment,
self.last_scan_msg.angle_max,
)
rospy.loginfo('Updating map')
self.gridmap.update(rob_pose, range_scan)
self.gridmap_msg.header.stamp = rospy.Time.now()
prob_map = self.gridmap.get_prob_map() * 100
self.gridmap_msg.data = prob_map.flatten()
self.map_pub.publish(self.gridmap_msg)
def correct(self, meas_model_transform, measurements, noise, landmark_map):
sigma_points, w_m, w_c = compute_sigma_points(self.mean, self.cov,
self.lambd, self.alpha,
self.beta)
# Normalize
for i in range(sigma_points.shape[1]):
sigma_points[2, i] = normalize_angle(sigma_points[2, i])
for reading in measurements:
# Real measurement
lid, srange, sbearing = reading
z_measured = np.matrix([srange, normalize_angle(sbearing)]).T
# Predicted measurement
# Transform points through measurment_model
# Resulting matrix has 2 rows (range, bearing) and same number of columns
z_points = meas_model_transform(sigma_points, reading,
landmark_map)
# Recover gaussian for predicted measurement
# TODO: rewrite to normalize differences
zt, St = recover_gaussian(z_points, w_m, w_c)
# Normalize bearing
average_bearing = 0
avg_x = avg_y = 0
for i in range(z_points.shape[1]):
# normalize
z_points[1, i] = normalize_angle(z_points[1, i])
# weighted sums of cosines and sines of the angle
avg_x = avg_x + w_m[i] * math.cos(z_points[1, i])
avg_y = avg_y + w_m[i] * math.sin(z_points[1, i])
average_bearing = normalize_angle(np.arctan2(avg_y, avg_x))
zt[1] = average_bearing
# normalize bearing in covariance matrix
for i in range(St.shape[1]):
St[1, i] = normalize_angle(St[1, i])
# Measurement noise
Qt = np.diag(noise)
St = St + Qt
sigma_x_z = np.zeros((self.mean.shape[0], zt.shape[0]))
for i in range(z_points.shape[1]):
s_diff = sigma_points[:, [i]] - self.mean
z_diff = z_points[:, [i]] - zt
# Normalize angles
s_diff[2] = normalize_angle(s_diff[2])
z_diff[1] = normalize_angle(z_diff[1])
sigma_x_z = sigma_x_z + w_c[i] * s_diff * z_diff.T
# Normalization time
for i in range(sigma_x_z.shape[1]):
sigma_x_z[2, i] = normalize_angle(sigma_x_z[2, i])
# broadcasting error if not matrix
sigma_x_z = np.matrix(sigma_x_z)
# Kalman gain
Kt = sigma_x_z * np.linalg.pinv(St)
# Normalize
z_diff = z_measured - zt
z_diff[1] = normalize_angle(z_diff[1])
self.mean = self.mean + Kt * (z_diff)
self.cov = self.cov - Kt * St * Kt.T
# normalize angle
self.mean[2] = normalize_angle(self.mean[2])
for i in range(self.cov.shape[1]):
self.cov[2, i] = normalize_angle(self.cov[2, i])
return self.mean, self.cov
def correct(self, measurements, local_map):
rx = self.mean.item(0)
ry = self.mean.item(1)
rtheta = normalize_angle(self.mean.item(2))
Htfull = np.matrix([])
Zdiff = np.matrix([])
for reading in measurements:
# TODO: Put in measurement model
lid, srange, sbearing = reading
z_measured = np.matrix([srange, normalize_angle(sbearing)]).T
mu_lid = self.get_mu_lid(lid)
# Expected observation
lx = 0
ly = 0
if not local_map.is_added(lid):
lx = rx + srange * math.cos(sbearing + rtheta)
ly = ry + srange * math.sin(sbearing + rtheta)
local_map.add((lid, lx, ly))
self.mean[mu_lid, :] = lx
self.mean[mu_lid + 1, :] = ly
else:
lx = self.mean[mu_lid, :].item(0)
ly = self.mean[mu_lid + 1, :].item(0)
dx = lx - rx
dy = ly - ry
delta = np.matrix([dx, dy]).T
q = delta.T * delta
z_expected = np.matrix(
[math.sqrt(q),
normalize_angle(np.arctan2(dy, dx) - rtheta)]).T
qst = math.sqrt(q)
# Measurement jacobian
Htt = np.matrix([[-qst * dx, -qst * dy, 0, qst * dx, qst * dy],
[dy, -dx, -q, -dy, dx]])
Htt = np.multiply((1.0 / q), Htt)
F = np.zeros((5, self.mean.size))
F[:self.rsize, :self.rsize] = np.eye(self.rsize)
F[self.rsize:, mu_lid:mu_lid + self.lmsize] = np.eye(self.lmsize)
Ht = Htt * F
Htfull = np.concatenate(
(Htfull, Ht)) if Htfull.size else np.copy(Ht)
diff = z_measured - z_expected
# Important to normalize_angles
diff[1] = normalize_angle(diff.item(1))
Zdiff = np.concatenate(
(Zdiff, diff)) if Zdiff.size else np.copy(diff)
# measurement noise
Qt = np.eye(Zdiff.shape[0]) * 0.01
Kgain = self.cov * Htfull.T * np.linalg.inv(Htfull * self.cov *
Htfull.T + Qt)
self.mean = self.mean + Kgain * Zdiff
self.cov = (np.eye(self.mean.size) - Kgain * Htfull) * self.cov
return self.mean, self.cov