def compute_feature(scans):
num_raw_scans = len(scans)
feature_scans = []
angle, dist = scans[0][0], scans[0][1]
num = 1.0
for i in range(1, num_raw_scans):
if scans[i][0] == scans[i - 1][0] + 1 and abs(scans[i][1] -
scans[i - 1][1]) < 1.0:
angle += scans[i][0]
dist += scans[i][1]
num += 1.0
else:
# if not the case, previous recording list up to one feature. calc mid
feature_scans += [[
dist / num,
normalize_angle(radians(angle / (2 * num)) - np.pi / 2)
]]
# reset counters
angle = scans[i][0]
dist = scans[i][1]
num = 1.0
feature_scans += [[
dist / num,
normalize_angle(radians(angle / (2 * num)) - np.pi / 2)
]]
return np.asarray(feature_scans)
def vic_f(current_state, u_t, Q, args=None, do_noise=True):
x, y, theta = current_state[0], current_state[1], current_state[2]
vel = u_t[0]
alpha = u_t[1]
a = args[1]
delta_t = args[0]
b = args[2]
L = args[3]
H = args[4]
res = np.asarray([
x + delta_t * (v * cos(theta)) - (vel / L) * tan(theta) *
(a * sin(theta) + b * cos(theta)), y + delta_t * (v * sin(theta)) +
(vel / L) * tan(theta) * (a * cos(theta) - b * sin(theta)),
normalize_angle(theta + delta_t * (vel / L) * tan(alpha))
]).T[0]
# if norm(res[:2] - current_state[:2]) > 10.00:
# print("Current u :" , u_t)
# print("Current state: ", current_state)
# print(res)
# raise Exception
if do_noise:
noise = np.random.normal(0, Q.diagonal(), 3)
res += noise
current_state[:3] = res
return current_state
def update(self, z, ind): # check if landmark was never seen before position_of_landmark_in_state_vector = 2 * ind + self.dim_state_without_landmarks if self.state[position_of_landmark_in_state_vector] == 0.0 and self.state[ position_of_landmark_in_state_vector + 1] == 0.0: # remember landmark self.state[position_of_landmark_in_state_vector] = self.state[0] + z[0] * cos( z[1] + self.state[2]) self.state[position_of_landmark_in_state_vector + 1] = self.state[1] + z[0] * sin( z[1] + self.state[2]) # compute delta - the estimated position of the landmark delta = np.asarray([0.0, 0.0]) delta[0] = self.state[position_of_landmark_in_state_vector] - self.state[0] delta[1] = self.state[position_of_landmark_in_state_vector + 1] - self.state[1] # compute q q = np.matmul(np.transpose(delta), delta) # compute z_dash z_dash = np.array([sqrt(q), normalize_angle(atan2(delta[1], delta[0]) - self.state[2])]) # define F_x_i F_x_i = np.zeros((self.dim_state_without_landmarks + self.dim_z, self.dim_state)) for kk in range(self.dim_state_without_landmarks): F_x_i[kk, kk] = 1.0 for kk in range(self.dim_z): F_x_i[self.dim_state_without_landmarks + kk, self.dim_state_without_landmarks + 2 * ind + kk] = 1 # get matrix H H_i = np.array([[-sqrt(q) * delta[0], -sqrt(q) * delta[1], 0.0, sqrt(q) * delta[0], sqrt(q) * delta[1]], [delta[1], -delta[0], -q, -delta[1], delta[0]]], dtype=np.double) H_i = (1 / q) * np.matmul(H_i, F_x_i) # compute innovation S_k = np.add(np.matmul(np.matmul(H_i, self.sigma), np.transpose(H_i)), self.R) # compute Kalman gain K_k = np.matmul(np.matmul(self.sigma, np.transpose(H_i)), inv(S_k)) # compute difference between measurement and estimated measurement diff = z - z_dash diff[1] = normalize_angle(diff[1]) y_k = np.transpose(np.asmatrix(diff)) self.state = np.add(self.state, np.asarray(np.dot(K_k, y_k).T.tolist()[0])) self.sigma = np.matmul(np.subtract(np.eye(self.dim_state), np.matmul(K_k, H_i)), self.sigma)
def compute_measurements(laser_scanner, thresh=25):
dist, angl, num = [], [], 0.0
features = []
for k in range(len(laser_scanner)):
if laser_scanner[k] < 8000:
c_l = float(laser_scanner[k])
if len(dist) == 0 or abs(c_l - dist[-1]) < thresh:
dist += [c_l]
angl += [normalize_angle(radians(k / 2) - np.pi / 2)]
num += 1
else:
features += [[sum(dist) / num, mean_angles(angl)]]
dist, angl, num = [c_l], [
normalize_angle(radians(k / 2) - np.pi / 2)
], 1.0
features += [[sum(dist) / num, mean_angles(angl)]]
return features
def h(state, m_i): """ The measurement function, translating the current state of the robot/filter into measurement space :param state: The current state :param m_i: The position of the landmark corresponding to the measurement :return: The expected (r,b) measurement """ l_x, l_y = m_i[0], m_i[1] x, y, heading = state[0], state[1], state[2] angle = normalize_angle(atan2((l_y - y), (l_x - x)) - heading) return np.array([sqrt((l_x - x) ** 2 + (l_y - y) ** 2), angle])
def calc_weight(x, mean, cov):
"""
Calculate the density for a multinormal distribution
Taken from: https://github.com/nwang57/FastSLAM
Thanks!
"""
# return scipy_multi.pdf(x, mean=mean, cov=cov)
den = 2 * np.pi * sqrt(det(cov))
diff = x - mean
diff[1] = normalize_angle(diff[1])
num = np.exp(-0.5 * dot3(diff.T, inv(cov), diff))
result = num / den
return result
def update(self, z, h, h_args=()):
x_h = np.asarray([h(member, h_args) for member in self.ensemble])
x_dash = np.mean(self.ensemble, axis=0)
y_dash = np.mean(x_h, axis=0)
y_diff = x_h - y_dash
for i in range(len(y_diff)):
y_diff[i][1] = normalize_angle(y_diff[i][1])
X = (1.0 / sqrt(self.N - 1)) * (self.ensemble - x_dash).T
Y = (1.0 / sqrt(self.N - 1)) * y_diff.T
D = np.dot(Y, Y.T) + self.R
K = np.dot(X, Y.T)
K = np.dot(K, inv(D))
v_r = np.random.multivariate_normal([0] * self.dim_z, self.R, self.N)
for j in range(self.N):
diff = z + v_r[j] - x_h[j]
diff[1] = normalize_angle(diff[1])
self.ensemble[j] += np.dot(K, diff)
self.state = np.mean(self.ensemble, axis=0)
def calc_diff(entries, mean, index_of_angles):
diff = entries - mean
for i in range(len(diff)):
diff[i, index_of_angles] = normalize_angle(diff[i, index_of_angles])
return diff
def calc_mean(entries, index_of_angles):
new_mean = np.mean(entries, axis=0)
new_mean[index_of_angles] = normalize_angle(
mean_angles(entries[:, index_of_angles]))
return new_mean
def generate_simulation(num_landmarks,
x_min,
x_max,
orig_state,
threshold,
number_of_steps,
f,
Q,
R,
step_size=3,
go_straight=False,
do_test=False,
additional_args=(1, 1),
dim_state=3,
do_circle=False,
asso_test=False):
"""
Generates the data needed for the simulation: The true robots path, the controls needed and the noisy measurements needed.
:param dim_state: The size of the state vector
:param num_landmarks: The number of landmarks in the world
:param x_min: Min. value in Euclidean plane
:param x_max: Max. value in Euclidean plane
:param orig_state: The starting position
:param threshold: The maximum range in which to record measurements
:param number_of_steps: The number of steps to be run in the simulation
:param f: The state transition function, i.e. the movement model
:param Q: The state transition covariance
:param R: The measurement covariance
:param step_size: The size of the steps taking a each time step t
:param go_straight: Boolean indicating whether to go straight or take turns
:param do_test: If true, simply place four landmarks in the world. Only for testing
:param additional_args: Additional arguments. Must at least include arguments for the state transition function (w,delta_t)
:return: A 5-tuple, containing:
- The total size of the world
- The now placed number of landmarks
- A list of the landmarks
- A list of the true positions of the robot in the simulation
- A list of all the measurements recorded at each time step
- A list of all the controls fed to the robot at each time step
"""
w, delta_t = additional_args[:2]
world_size = abs(x_min) + abs(x_max)
landmarks = [[
randint(x_min + 5, x_max - 5),
randint(x_min + 5, x_max - 5)
] for _ in range(num_landmarks)]
measurements_at_i = [None]
u_at_i = [None]
if do_test:
landmarks = [[25, 25], [25, 75], [75, 25], [75, 75]]
num_landmarks = len(landmarks)
test_state = np.array([0.0 for _ in range(dim_state)]).T
if orig_state is not None:
test_state = deepcopy(orig_state)
else:
test_state[0] = (x_max - abs(x_min)) / 2
test_state[1] = (x_max - abs(x_min)) / 2
if do_circle:
test_state[0] = -97.5
test_state[1] = -5
test_state[2] = .5 * np.pi
if asso_test:
test_state[0] = x_min + 5
test_state[1] = x_max / 2
test_state[2] = 0.0
landmarks = [[i, 25] for i in range(x_min + 5, x_max - 5, 20)
] + [[i, 75] for i in range(x_min + 5, x_max - 5, 20)]
num_landmarks = len(landmarks)
position_at_i = [test_state]
current_circle_u = -0.01
for _ in range(number_of_steps):
if not go_straight:
# test_u = np.array([step_size, choice([0.0, -.05, -.1, -.15, .05, .1, .15])])
test_u = np.array(
[step_size,
choice([0.0, -.05, -.1, .05, .1, -.025, .025])])
else:
test_u = np.array([step_size, 0.0])
if do_circle:
test_u = np.array([step_size, current_circle_u])
current_circle_u -= 0.00001
# check if we run into a wall
test_u = move(test_state, test_u, x_min, x_max, f, Q, (w, delta_t))
# do real movement - therefore no noise!
test_state = f(test_state, test_u, Q, (w, delta_t), do_noise=False)
test_state[2] = normalize_angle(test_state[2])
# store current position
position_at_i += [test_state]
# store correct control
u_at_i += [test_u]
# store measurements received at i
if do_test:
z = get_range_bearing_measurements(test_state,
landmarks,
R,
do_noise=True,
threshold=None)
else:
z = get_range_bearing_measurements(test_state,
landmarks,
R,
do_noise=True,
threshold=threshold)
measurements_at_i += [z]
return world_size, num_landmarks, landmarks, position_at_i, measurements_at_i, u_at_i
def slam_update(self, z, ind, h, H_wrt_l, h_inv):
"""
Here we are following the approach in the above mentioned thesis - therefore we are performing everything
in one run - no need to split to two methods.
If you are looking at this and are wondering, I strongly recommend to check out the above mentioned thesis,
specifically p.85
:param z: The current measurement
:param ind: The index of the according landmark
:param h: Measurement function
:param H_wrt_l: Jacobian of measurement function w.r.t. the landmark
:param h_inv: The inverse of the measurement function
"""
# First thing: Loop over all particles:
for i in range(self.N):
par = self.particles[i]
index_in_particle = 2 + ind * 2
# This is with known associations, wherefore we skip the loop over all potential data associations
# Compute prediction s_dash using the state transition function f
s_dash = par[0]
# first, check if the landmark was previously undiscovered:
if par[index_in_particle] is None and par[index_in_particle + 1] is None:
# Landmark not yet discovered. Add it
# Then, compute the landmark position and sigma
l_mean = h_inv(par[0], z)
par[index_in_particle] = l_mean
par[index_in_particle + 1] = np.eye(2) * self.new_cov_infl
self.weights[i] = self.p_0
else:
# Get Jacobian
H_wrt_l_mat = H_wrt_l(s_dash, par[index_in_particle])
# Compute the expected measurement z_dash using the measurement function h
z_dash = h(s_dash, par[index_in_particle])
# Compute the difference and normalize!
z_diff = z - z_dash
z_diff[1] = normalize_angle(z_diff[1])
l_sigma = par[index_in_particle + 1]
# Compute Z_t
Z_t = self.R + dot3(H_wrt_l_mat, l_sigma, H_wrt_l_mat.T)
# print(H_wrt_l_mat)
# update the landmark prediction accordingly
l_mean = par[index_in_particle]
# compute Kalman gain
K = dot3(l_sigma, H_wrt_l_mat.T, inv(Z_t))
# compute new mean
par[index_in_particle] = l_mean + np.dot(K, z_diff)
# compute new sigma
par[index_in_particle + 1] = np.dot((np.eye(2) - np.dot(K, H_wrt_l_mat)), l_sigma)
# compute new particle weight
# if self.weights[i] * (calc_weight(z, z_dash, Z_t) + 1.e-300) == 0.0:
# print("PRE-ALERT: %d" % i)
# print("Particle %d Pos: %f,%f - Landmark Estimate: %f,%f - Weight: %f" % (i, par[0][0], par[0][1], par[index_in_particle][0], par[index_in_particle][1], self.weights[i]))
# print(self.weights[i])
# print((calc_weight(z, z_dash, Z_t) + 1.e-300))
# print("....")
# if self.weights[i] == 0.0:
# print("ALERT")
self.weights[i] *= calc_weight(z, z_dash, Z_t)
# print("[%d] Diff is: [%f,%f] - Prob is: %f" % (i, z_diff[0], z_diff[1], calc_weight(z, z_dash, Z_t)))
# print("Particle %d Pos: %f,%f - Landmark Estimate: %f,%f - Weight: %f" % (i, par[0][0], par[0][1], par[index_in_particle][0],par[index_in_particle][1], self.weights[i]))
# print(z, z_dash)
# if sum(self.weights) < 1.0:
# self.normalize_weights()
self.normalize_weights()
def update(self, z, h, ind, h_args=(), full_ensemble=False): """ Update the current prediction using incoming measurements :param full_ensemble: :param z: The incoming measurement :param h: The measurement function :param ind: The index of the landmark according to the measurement :param h_args: Additional arguments for the state transition function """ # according to roth_2017, we do not need a batch update, but can iterate over the updates individually. # check if landmark was never seen before position_of_landmark_in_state_vector = 2 * ind + self.state_without_landmarks new_landmark = abs(self.state[position_of_landmark_in_state_vector]) < self.eps \ and abs(self.state[position_of_landmark_in_state_vector + 1]) < self.eps if new_landmark: # remember landmark - IN EACH MEMBER! for i in range(self.N): self.ensemble[i, position_of_landmark_in_state_vector:position_of_landmark_in_state_vector + self.dim_landmarks] \ = get_coordinate_from_range_bearing(z, self.ensemble[i]) if full_ensemble: tmp_ensemble = self.ensemble col_indx = [col_ind for col_ind in range(self.dim_state)] else: # lets try something: select only robot state and landmark state and form new tmp state # row_indx -> ensemble member, col_indx -> value in ensemble member col_indx = np.array([v for v in range(self.state_without_landmarks)] + [b for b in range( position_of_landmark_in_state_vector, position_of_landmark_in_state_vector + self.dim_landmarks)]) tmp_ensemble = self.ensemble[:, col_indx] tap_N = len(tmp_ensemble) # Use this function for using correct landmark # x_h = np.asarray([h(member, h_args) for member in tmp_ensemble]) if full_ensemble: x_h = np.asarray([h(member, self.get_ith_landmark_from_member(member, ind)) for member in tmp_ensemble]) else: x_h = np.asarray([h(member, self.get_ith_landmark_from_member(member, 0)) for member in tmp_ensemble]) # Angles can not be "meaned" like normal values! # Therefore: Use own mean function # Calculate the mean of the chosen ensemble x_dash = calc_mean(tmp_ensemble, 2) # Calculate the expected measurement for each ensemble member y_dash = calc_mean(x_h, 1) # Tang_2015 argue, that the following has to hold: y_dash == h(np.mean(tmp_ensemble, axis=0), h_args) # This is the normal calculation X = (1.0 / sqrt(tap_N - 1)) * (calc_diff(tmp_ensemble, x_dash, 2)).T Y = (1.0 / sqrt(tap_N - 1)) * (calc_diff(x_h, y_dash, 1)).T # Follow simple calculation for computation of Kalman gain D = np.dot(Y, Y.T) + self.R K = np.dot(X, Y.T) K = np.dot(K, inv(D)) v_r = np.random.multivariate_normal([0] * self.dim_z, self.R, self.N) for j in range(self.N): # self.ensemble[j, col_indx] += np.dot(K, z + v_r[j] - x_h[j]) diff = z - x_h[j] + v_r[j] diff[1] = normalize_angle(diff[1]) update = np.dot(K, diff) self.ensemble[j, col_indx] += update self.normalize_ensemble_angles() self.state = calc_mean(self.ensemble, 2)
def f(state, u, Q, args, do_noise=False, dim_robot_state=3):
"""
The underlying state transition model for our robot.
It is very well explained in: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter.ipynb
:param dim_robot_state: The dimension of the robot state. Defaults to | [x,y,heading] | = 3
:param state: The current state
:param u: The control input u = (v,alpha)
:param Q: The state transition covariance matrix
:param args: Additional arguments, carry at least the robots width and the time difference delta_t
:param do_noise: Boolean indicating whether to add noise or to do true movement
:return: The new state
"""
##############################################################################################
if len(state) < 3:
raise ValueError("Current State is not large enough, should at least contain [x,y,heading]")
if len(args) < 2:
raise ValueError("The robot width and the time step must be specified in args!")
if len(u) != 2:
raise ValueError("Please specify u = (v, alpha)")
##############################################################################################
# Get the robots width
w = args[0]
# Get the time change
delta_t = args[1]
# Compute the distance travelled based on velocity and time based
if do_noise and len(Q) == len(u):
noise = np.random.normal(0, Q.diagonal(), len(u))
velocity = u[0] + noise[0]
alpha = u[1] + noise[1]
else:
velocity = u[0]
alpha = u[1]
d = velocity * delta_t
# Compute the additional turning for the heading
beta = (d / w) * tan(alpha)
theta = state[2]
new_state = np.transpose(np.asarray(np.array([0.0 for _ in range(len(state))])))
if abs(u[1]) > 0.001:
R = d / beta
angle_sum = normalize_angle(theta + beta)
arg = np.array( [-R * sin(theta) + R * sin(angle_sum),
R * cos(theta) - R * cos(angle_sum),
beta])
else:
arg = np.array([d * cos(theta),
d * sin(theta),
0])
if do_noise and len(Q) == dim_robot_state:
noise = np.random.normal(0, Q.diagonal(), dim_robot_state)
arg += noise
for i in range(len(arg)):
new_state[i] = arg[i]
new_state[2] = normalize_angle(arg[2])
res = np.add(state, new_state)
return res
def normalize_ensemble_angles(self):
"""
Normalize an angle to be within [-pi, pi)
"""
for i in range(self.N):
self.ensemble[i, 2] = normalize_angle(self.ensemble[i, 2])
len_drive_information = len(drive['time'])
L, b, a, H_vic = 2.83, 0.5, 3.78, 0.76
current_laser_index = 0
ens_prediction = [EnsKFSlam_UA.state]
# iterate over driving information
for i in range(len_drive_information):
if i % 250 == 0:
print("--> Doing step %d" % i)
v = vel[i]
alpha = normalize_angle(steering[i])
u_t = [v, alpha]
timestamp = driving_time_stamps[i]
# dirty hack!
if i > 0:
# Divide by thounds - the timestamp is in milliseconds
# and the velocity is in m/sec
delta_t = abs(timestamp - driving_time_stamps[i - 1]) / 1000
else:
continue
# print("Pre-predict: ", EnsKFSlam_UA.state)
EnsKFSlam_UA.predict(vic_f,
u_t,