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()