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 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, 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 predict_boundaries(curr_speed, curr_bearing, dt):
#print 'Previous left: ', sorted(LEFT_BOUNDARY)
#print 'Previous right: ', sorted(RIGHT_BOUNDARY)
predicted_left, predicted_right = predict(LEFT_BOUNDARY, RIGHT_BOUNDARY,
curr_speed, curr_bearing, dt)
if predicted_left and predicted_right:
set_boundaries(predicted_left, predicted_right)
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 kalman_filter(self, observ):
"""Kalman filter using the model on a set of observations"""
# Get system matrices
F = self.transition_matrix()
Q = self.transition_covariance()
H = self.observation_matrix()
R = self.observation_covariance()
# Initialise arrays of Gaussian densities and (log-)likelihood
num_time_instants = len(observ)
flt = GaussianDensityTimeSeries(num_time_instants, self.ds)
prd = GaussianDensityTimeSeries(num_time_instants, self.ds)
lhood = 0
# Loop through time instants
for kk in range(num_time_instants):
# Prediction
if kk > 0:
prd_kk = kal.predict(flt.get_instant(kk - 1), F, Q)
else:
prd_kk = self.initial_state_prior
prd.set_instant(kk, prd_kk)
# Correction - handles misisng data indicated by NaNs
y = observ[kk]
if not np.any(np.isnan(y)):
# Nothing missing - full update
flt_kk, innov = kal.correct(prd.get_instant(kk), y, H, R)
lhood = lhood + mvn.logpdf(observ[kk], innov.mn, innov.vr)
elif np.all(np.isnan(y)):
# All missing - no update
flt_kk = prd_kk
else:
# Partially missing - delete missing elements
missing = np.where(np.isnan(y))
yp = np.delete(y, missing, axis=0)
Hp = np.delete(H, missing, axis=0)
Rp = np.delete(np.delete(R, missing, axis=0), missing, axis=1)
flt_kk, innov = kal.correct(prd.get_instant(kk), yp, Hp, Rp)
lhood = lhood + mvn.logpdf(yp, innov.mn, innov.vr)
flt.set_instant(kk, flt_kk)
return flt, prd, lhood
def kalman_filter(self, observ):
"""Kalman filter using the model on a set of observations"""
# Get system matrices
F = self.transition_matrix()
Q = self.transition_covariance()
H = self.observation_matrix()
R = self.observation_covariance()
# Initialise arrays of Gaussian densities and (log-)likelihood
num_time_instants = len(observ)
flt = GaussianDensityTimeSeries(num_time_instants, self.ds)
prd = GaussianDensityTimeSeries(num_time_instants, self.ds)
lhood = 0
# Loop through time instants
for kk in range(num_time_instants):
# Prediction
if kk > 0:
prd_kk = kal.predict(flt.get_instant(kk-1), F, Q)
else:
prd_kk = self.initial_state_prior
prd.set_instant(kk, prd_kk)
# Correction - handles misisng data indicated by NaNs
y = observ[kk]
if not np.any(np.isnan(y)):
# Nothing missing - full update
flt_kk,innov = kal.correct(prd.get_instant(kk), y, H, R)
lhood = lhood + mvn.logpdf(observ[kk], innov.mn, innov.vr)
elif np.all(np.isnan(y)):
# All missing - no update
flt_kk = prd_kk
else:
# Partially missing - delete missing elements
missing = np.where( np.isnan(y) )
yp = np.delete(y, missing, axis=0)
Hp = np.delete(H, missing, axis=0)
Rp = np.delete(np.delete(R, missing, axis=0), missing, axis=1)
flt_kk,innov = kal.correct(prd.get_instant(kk), yp, Hp, Rp)
lhood = lhood + mvn.logpdf(yp, innov.mn, innov.vr)
flt.set_instant(kk, flt_kk)
return flt, prd, lhood
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1]])
u = np.array([0, 0, random.uniform(0, 2), 0, 0, random.uniform(0, 2)])
#u = np.array([0, 0, 1, 0, 0, 1])
#Q = np.eye(x.shape[0])
sq2 = 0.1**2
Q = np.array([[0.5 * (dt**2) * sq2, 0, 0, 0, 0, 0], [0, dt * sq2, 0, 0, 0, 0],
[0, 0, sq2, 0, 0, 0], [0, 0, 0, 0.5 * (dt**2) * sq2, 0, 0],
[0, 0, 0, 0, dt * sq2, 0], [0, 0, 0, 0, 0, sq2]])
R = 0.01 * np.eye(z.shape[0])
N = 100
predictions, corrections, measurements = [], [], []
for k in np.arange(0, N):
x, P = kalman.predict(x, P, A, B, u, Q)
predictions.append(x)
x, P = kalman.correct(x, C, z, P, R)
corrections.append(x)
measurements.append(z)
z = np.array([[x[0, 0] + random.uniform(0, 1)],
[x[1, 0] + random.uniform(0, 1)]]) # test measurement
measurements_plot_x = []
predictions_plot_x = []
corrections_plot_x = []
measurements_plot_y = []
predictions_plot_y = []
corrections_plot_y = []
for x in range(10):
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
def fast_loop(print_lock, state_vector_lock, path_planning_lock, imu, enc, freq):
clock = Clock()
dt = 0
while True:
start = time.time()
A = iss.get_A(dt) # update state transition matrix
Q = iss.get_Q(dt) # update covariance matrix
a_x = imu.linear_acceleration[0]
a_y = imu.linear_acceleration[1]
theta = imu.euler[0]
u = iss.get_u(a_x, a_y) # get control vector (actually this is for the next loop, but this was the best way of handling the state...)
with state_vector_lock: # required so we lidar doesn't write at the same time
x, P = kalman.predict(x, P, A, B, u, Q) # predict state from imu
ticks_l, ticks_r = enc.get_ticks() # get 'measured' position
with state_vector_lock:
x, P = kalman.correct(x, C, z, P, R)
with path_planning_lock:
# run control code based on position estimate and current planned path
with print_lock:
print("This is dummy text")
# read imu data (to give starting acceleration)
# while True:
# time since clock began = clock.time()?
# state space model to estimate position from previous imu data (with time)
# WITH STATE_VECTOR_LOCK:
# predict belief of position vector, with covariance vector in KF
# read imu data for this moment
# read encoder ticks
# state space model to estimate position, with covariance vector
# WITH STATE_VECTOR_LOCK:
# update belief of position vector in KF
# WITH PATH_PLANNING_LOCK:
# run control code based on current position estimate and planned path
# actuate motors based on the control code
time.sleep(0.01)
clock.tick(freq) # limits the loop to "freq" fps - stops unnecessary processing power being used
dt = time.time() - start # make sure this doesn't break / clash with clock.tick()
def slow_loop(print_lock, text):
freq = 10
clock = Clock()
while True:
with print_lock:
print("This {} will be taken by the Lidar scanning and Path Planning".format(text))
time.sleep(0.01)
clock.tick(freq)
# while True:
# listen for Lidar scan from arduino
# if lidar_scan:
# infer_position using one of the methods you know
# WITH STATE_VECTOR_LOCK:
# update belief of position vector in KF
# run path planning code
# WITH PATH_PLANNING_LOCK (and maybe state_vector_lock?):
# update path plan based on current position and end goal
def main():
imu = imu_sensor.IMU()
enc = encoders.Encoders()
fast_loop_freq = Config['pygame']['fast_loop_freq'] # 60 Hz loop speed
print_lock = threading.Lock()
state_vector_lock = threading.Lock()
path_planning_lock = threading.Lock()
fast_thread = threading.Thread(target=fast_loop, args=(print_lock, state_vector_lock, path_planning_lock, imu, enc, fast_loop_freq), name="fast_loop")
fast_thread.start()
slow_thread = threading.Thread(target=slow_loop, args=(print_lock, 'dummy_text'), name="slow_loop")
slow_thread.start()
if __name__ == "__main__":
main()
enc_l, enc_r = enc.get_ticks()
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))
oldState = modelFreeFall.state[0].reshape((2,1))
z = np.zeros((N,2))
H = np.eye(2)
measurementNoise = 0.9
sensor = kalman.MeasurementModel(2*H,measurementNoise,z)
kalman = kalman.KalmanFilter(np.zeros((2,2)))
stateEstimates = []
detP = []
for i in range(1,N):
oldState = modelFreeFall.state[i-1].reshape((2,1))
modelFreeFall.state[i] = np.transpose(modelFreeFall.calcNewState(oldState))
trueState = modelFreeFall.state[i].reshape((2,1))
measure = sensor.calcNewMeasurement(trueState)
kalman.predict(oldState,modelFreeFall,sensor)
kalman.calcKGain(sensor)
stateEstimates.append(kalman.correct(sensor,measure))
detP.append(np.linalg.det(kalman.P))
z[i] = measure.reshape(2)
t[i] = t[i-1] + dt
detP = np.array(detP)
stateEstimates = np.array(stateEstimates).reshape((99,2))
print detP.shape,t.shape ,np.transpose(stateEstimates)[0].shape
plt.figure()
#plt.scatter( t, np.transpose(sensor.z)[0])
plt.scatter(t,np.transpose(modelFreeFall.state)[0])
plt.scatter(t[1:100],np.transpose(stateEstimates)[0])
#plt.scatter(t[1:100],detP)
clock = Clock()
print("Starting...")
for i in range(N):
enc_l, enc_r = enc.get_ticks()
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))
### Part 3 ### # Supposing we only have the given observations, estimate the state of the system at # iteration 200, using the average of the measured velocities from iteration 200 to 210. # Estimate the position of the projectile given this initial state estimate, using P = 10^6 * Q. # Add to the plot the estimated path of the projectile as a green curve. vel = sp.array([sp.mean(sp.diff(observations[0,200:210])/.1),sp.mean(sp.diff(observations[1,200:210])/.1)]) x_est_initial = sp.concatenate([observations[:,200],vel]) estimation = kalman.kalmanFilter(Fk,Q,U,H,R,x_est_initial,Q*(10**6),observations[:,200:800]) plt.plot(estimation[0,:],estimation[1,:],'g') ### Part 4 ### # Given the final state estimate at iteration 800, iterate forward predictively to find the # projectile's point of impact. Plot this with a yellow curve. prediction = kalman.predict(Fk,U,estimation[:,599],500) temp = sp.array([x > -50 for x in prediction[1,:]]) plt.plot(prediction[0,temp],prediction[1,temp],'y') ### Part 4 ### # Given the state estimate at iteration 250, rewind the system to identify the # projectile's point of origin. Plot this with a cyan curve, and display the results. rewound = kalman.rewind(Fk,U,estimation[:,50],300) temp = sp.array([x > -50 for x in rewound[1,:]]) plt.plot(rewound[0,temp],rewound[1,temp],'c') plt.ylim([0,20000]) plt.show()
