def get_kalman_results(file, vals, res_file, test_num, oss, iss, rss, x, P):
raw_input = []
num_samples = 0
with open(file, 'r') as f:
reader = csv.reader(f)
for row in reader:
raw_input.append((int(row[0]), int(row[1]), float(row[2]),
float(row[3]), float(row[4])))
num_samples += 1
imu_a_x = 0
imu_a_y = 0
for l, r, ax, ay, ti in raw_input:
z_i = iss.measure_state(x, imu_a_x, imu_a_y, ti)
imu_a_x = ax
imu_a_y = ay
l, r = oss.clean_input(l, r)
z_o = oss.measure_state(x, l, r)
x, P = kalman.predict(x, P, rss.A, rss.Q)
x, P = kalman.update(x, P, z_i, iss.H, iss.R)
x, P = kalman.update(x, P, z_o, oss.H, oss.R)
real_x, real_y, real_theta = get_real_vals(vals, test_num)
data = [x[0][0], x[2][0], x[4][0], real_x, real_y, real_theta, num_samples]
log_data(res_file, data)
def fast_loop(print_lock, state_vector_lock, path_planning_lock, imu, enc,
freq):
clock = Clock()
dt = 1 / freq
# might be worth having an "initial" loop to sort out accleration / dt issues?
imu_a_x = 0 # first time, shouldn't be measured before the prediction because it hasn't been any time yet!
imu_a_y = 0
while True:
# get imu measurements
# consider the fact that acceleration should really predict state (except
# theta) for the NEXT timestep...
imu_theta = imu.euler[0]
z_i = iss.measure_state(
x, imu_a_x, imu_a_y, imu_theta
) # get control vector (actually this is for the next loop, but this was the best way of handling the state...)
imu_a_x = imu.linear_acceleration[
0] # measured AFTER KF so it can be used for the NEXT timestep
imu_a_y = imu.linear_acceleration[1]
# get odometry measurements
enc_l, enc_r = enc.get_ticks()
z_o = oss.measure_state(x, enc_l, enc_r)
# get lidar measurements
# z_l = lss.measure_state(x, scan) # update this whenever you have done lidar stuff...
#Kalman Filter
# State Propagation (Prediction) - based on previous state and any control signal (don't have one atm...)
with state_vector_lock:
x, P = kalman.predict(x, P, A,
Q) # can pass B, u as if you want...
# IMU Update
with state_vector_lock:
x, P = kalman.update(x, P, z_i, H_i, R_i)
# Odometry Update
with state_vector_lock:
x, P = kalman.update(x, P, z_o, H_o, R_o)
# Path correction control code
#with path_planning_lock:
# run control code based on position estimate and current planned path
# does this need to run every loop?
# Actuate motors
#rc_robot.actuate(args)
#time.sleep(0.01) # is this required? Probably not...
clock.tick(
freq
) # limits the loop to "freq" fps - stops unnecessary processing power being used
def get_next_state(X, P, state, delta_t):
print('x', X)
Q = numpy.eye(X.shape[0])
B = numpy.eye(X.shape[0])
U = numpy.zeros((X.shape[0], 1))
Y = array([[state[0]], [state[1]], [state[2]]])
H = array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]])
R = numpy.eye(Y.shape[0])
(X, P, Z1, Z2, Z3, Z4) = kalman.update(X, P, Y, H, R)
print('after update')
print(X)
distance = (state[0] + state[1] - state[2]) * 15 * delta_t
x_dist = distance * math.cos(state[5])
y_dist = distance * math.sin(state[5])
rotation_factor = 0
if state[2] != 0:
rotation_factor = state[2] / (2 * (state[0] + state[1])) * (
math.fabs(state[2]) / 40) * delta_t * math.pi
input_matrix = array([[0], [0], [0], [x_dist], [y_dist],
[rotation_factor]])
(X, P) = kalman.predict(X, P, T_MATRIX, input_matrix)
print('after predict')
print(X)
return (X, P)
def update_boundaries(frame):
# Filter Data
cones = get_cones(frame, LEFT_POLYS, RIGHT_POLYS)
plot_frame(frame, cones)
# Predict Boundaries
predicted_left, predicted_right = predict(LEFT_BOUNDARY, RIGHT_BOUNDARY,
CURR_SPEED, CURR_BEARING)
if predicted_left and predicted_right:
set_boundaries(predicted_left, predicted_right)
# Detect Finish Line
detect_finish_line(cones)
# Create Boundary Lines
left_boundary, right_boundary = create_boundary_lines(cones)
left_boundary, right_boundary = update(left_boundary, right_boundary,
LEFT_BOUNDARY, RIGHT_BOUNDARY)
set_boundaries(left_boundary, right_boundary)
left_xs, left_ys = separate_xy(LEFT_BOUNDARY)
plot_line(LEFT_COEFS, min(left_ys), max(left_ys))
right_xs, right_ys = separate_xy(RIGHT_BOUNDARY)
plot_line(RIGHT_COEFS, min(right_ys), max(right_ys))
def fast_loop(print_lock, state_vector_lock, imu, enc, iss, oss, rss):
clock = Clock()
imu_a_x = 0
imu_a_y = 0
start = time.time()
global x
global P
while True:
period = time.time() - start
actual_freq = 1 / period
# with print_lock:
# print("{}Hz".format(actual_freq))
start = time.time()
# State Estimation Code
# imu_theta = imu.euler[0]
# z_i = iss.measure_state(x, imu_a_x, imu_a_y, imu_theta)
# imu_a_x = imu.linear_acceleration[2]
# imu_a_y = imu.linear_acceleration[1]
l_ticks, r_ticks = enc.get_ticks() # ticks = encoder counts
l_ticks, r_ticks = oss.clean_input(l_ticks,
r_ticks) # removes noise if present
z_o = oss.measure_state(x, l_ticks, r_ticks)
#Kalman Filter
# State Propagation
with state_vector_lock:
x, P = kalman.predict(x, P, rss.A,
rss.Q) # can pass B, u as if you want...
# IMU Update
# with state_vector_lock:
# x, P = kalman.update(x, P, z_i, iss.H, iss.R)
# Odometry Update
with state_vector_lock:
x, P = kalman.update(x, P, z_o, oss.H, oss.R)
# with print_lock:
# print(x)
xx = x[0][0]
xy = x[2][0]
xt = x[4][0]
log_data(write_file, [xx, xy, xt])
# Path correction control code
#with path_planning_lock:
# run control code based on position estimate and current planned path
# does this need to run every loop?
# Actuate motors
#rc_robot.actuate(args)
clock.tick(
freq
) # limits the loop to "freq" fps - stops unnecessary processing power being used
def rts_smoother(self, flt, prd):
"""Rauch-Tung-Striebel smooth using the model"""
# Get system matrices
F = self.transition_matrix()
# Initialise arrays of Gaussian densities and (log-)likelihood
num_time_instants = flt.num_time_instants
smt = GaussianDensityTimeSeries(num_time_instants, self.ds)
# Loop through time instants
for kk in reversed(range(num_time_instants)):
# RTS update
if kk < num_time_instants-1:
smt_kk = kal.update(flt.get_instant(kk),
smt.get_instant(kk+1), prd.get_instant(kk+1), F)
else:
smt_kk = flt.get_instant(kk)
smt.set_instant(kk, smt_kk)
return smt
def rts_smoother(self, flt, prd):
"""Rauch-Tung-Striebel smooth using the model"""
# Get system matrices
F = self.transition_matrix()
# Initialise arrays of Gaussian densities and (log-)likelihood
num_time_instants = flt.num_time_instants
smt = GaussianDensityTimeSeries(num_time_instants, self.ds)
# Loop through time instants
for kk in reversed(range(num_time_instants)):
# RTS update
if kk < num_time_instants - 1:
smt_kk = kal.update(flt.get_instant(kk),
smt.get_instant(kk + 1),
prd.get_instant(kk + 1), F)
else:
smt_kk = flt.get_instant(kk)
smt.set_instant(kk, smt_kk)
return smt
def fast_loop(print_lock, state_vector_lock, imu, enc, iss, oss, rss, path,
ctrl):
clock = Clock()
imu_a_x = 0
imu_a_y = 0
p = path.pop(0)
with print_lock:
print(p)
start = time.time()
global x
global P
while True:
period = time.time() - start
actual_freq = 1 / period
# with print_lock:
# print("{}Hz".format(actual_freq))
start = time.time()
dist_to_point = sqrt((x[0][0] - p[0])**2 + (x[2][0] - p[1])**2)
if dist_to_point < dist_thresh:
if path:
p = path.pop(0) # get the next point on the path
with print_lock:
print(p)
else:
with print_lock:
print("Reached Goal")
ctrl.move('stop')
break
phi = ctrl.get_phi(x, p)
# with print_lock:
# print(np.degrees(phi))
a, b = ctrl.p_control(phi)
# with print_lock:
# print("A: {}, B: {}".format(a, b))
# State Estimation Code
# imu_theta = imu.euler[0]
# z_i = iss.measure_state(x, imu_a_x, imu_a_y, imu_theta)
# imu_a_x = imu.linear_acceleration[2]
# imu_a_y = imu.linear_acceleration[1]
l_ticks, r_ticks = enc.get_ticks() # ticks = encoder counts
l_ticks, r_ticks = oss.clean_input(l_ticks,
r_ticks) # removes noise if present
z_o = oss.measure_state(x, l_ticks, r_ticks)
#Kalman Filter
# State Propagation
with state_vector_lock:
x, P = kalman.predict(x, P, rss.A,
rss.Q) # can pass B, u as if you want...
# IMU Update
# with state_vector_lock:
# x, P = kalman.update(x, P, z_i, iss.H, iss.R)
# Odometry Update
with state_vector_lock:
x, P = kalman.update(x, P, z_o, oss.H, oss.R)
# with print_lock:
# print(x)
xx = x[0][0]
xy = x[2][0]
xt = x[4][0]
log_data(write_file, [xx, xy, xt])
# Path correction control code
#with path_planning_lock:
# run control code based on position estimate and current planned path
# does this need to run every loop?
# Actuate motors
#rc_robot.actuate(args)
clock.tick(
freq
) # limits the loop to "freq" fps - stops unnecessary processing power being used
z_o = oss.measure_state(x, enc_l, enc_r)
data.append(z_o[0][0])
data.append(z_o[2][0])
data.append(z_o[4][0])
imu_theta = imu.euler[0]
z_i = iss.measure_state(
x, imu_a_x, imu_a_y, imu_theta
) # acc provided in m/s^2, theta provided in degrees (model converts these)
imu_a_x = imu.linear_acceleration[
2] # using the z-axis because x-axis is faulty...
imu_a_y = imu.linear_acceleration[1]
data.append(z_i[0][0])
data.append(z_i[2][0])
data.append(z_i[4][0])
x, P = kalman.predict(x, P, rss.A, rss.Q)
data.append(x[0][0])
data.append(x[2][0])
data.append(x[4][0])
x, P = kalman.update(x, P, z_i, iss.H, iss.R)
x, P = kalman.update(x, P, z_o, oss.H, oss.R)
data.append(x[0][0])
data.append(x[2][0])
data.append(x[4][0])
log_data(data_file, data)
clock.tick(freq)
print("Final Estimated State: {}".format(x))
print("Final State Covariance: {}".format(P))
z_o = oss.measure_state(x, enc_l, enc_r)
x_odo_measurements.append(z_o[0][0])
y_odo_measurements.append(z_o[2][0])
theta_odo_measurements.append(z_o[4][0])
imu_theta = imu.euler[0]
z_i = iss.measure_state(x, imu_a_x, imu_a_y, imu_theta) # acc provided in m/s^2, theta provided in degrees (model converts these)
x_imu_measurements.append(z_i[0][0])
y_imu_measurements.append(z_i[2][0])
theta_imu_measurements.append(z_i[4][0])
imu_a_x = imu.linear_acceleration[2] # using the z-axis because x-axis is faulty...
imu_a_y = imu.linear_acceleration[1]
x, P = kalman.predict(x, P, A, Q)
x_predictions.append(x[0][0])
y_predictions.append(x[2][0])
theta_predictions.append(x[4][0])
x, P = kalman.update(x, P, z_i, H_i, R_i)
x, P = kalman.update(x, P, z_o, H_o, R_o)
x_corrections.append(x[0][0])
y_corrections.append(x[2][0])
theta_corrections.append(x[4][0])
clock.tick(freq)
print("Final Estimated State: {}".format(x))
print("Final State Covariance: {}".format(P))
def draw_plot(predictions, odo_meas, imu_meas, corrections, state='x'):
plt.plot(predictions, label='{} State Prediction'.format(state))
plt.plot(odo_meas, label='{} Odometry Measurement'.format(state))
plt.plot(imu_meas, label='{} IMU Measurement'.format(state))
N = 200
imu_a_x = 0
imu_a_y = 0
clock = Clock()
print("Starting...")
for i in range(N):
imu_theta = imu.euler[0]
z_i = iss.measure_state(x, imu_a_x, imu_a_y, imu_theta)
x_measurements.append(z_i[0][0])
y_measurements.append(z_i[2][0])
x, P = kalman.predict(x, P, A, Q)
x_predictions.append(x[0][0])
y_predictions.append(x[2][0])
x, P = kalman.update(x, P, z_i, H_i, R_i)
x_corrections.append(x[0][0])
y_corrections.append(x[2][0])
imu_a_x = imu.linear_acceleration[0]
imu_a_y = imu.linear_acceleration[1]
clock.tick(freq)
plt.plot(x_predictions, label='State Prediction')
plt.plot(x_measurements, label='Odometry Measurement')
plt.plot(x_corrections, label='Odometry Correction')
plt.legend()
plt.title("Kalman Filter - Odometry Test")
save_name = "KF_IMU_test"