def update_landmark_estimates(nn_output): global landmark_est for key in nn_output.keys(): x_estimate = landmark_est[key][0] cov = landmark_est[key][1] obs = nn_output[key][1] x_estimate, cov = kf_update(cov, x_estimate, obs, sensor_noise_cov) landmark_est[key][0] = x_estimate landmark_est[key][1] = cov
def update_landmark_estimates(nn_output): global workspace for key in nn_output.keys(): x_estimate = workspace.landmark[key][0] cov = workspace.landmark[key][1] obs = nn_output[key][1] x_estimate, cov = kf_update(cov, x_estimate, obs, sensor_noise_cov) workspace.landmark[key][0] = x_estimate workspace.landmark[key][1] = cov workspace.generate_samples_for_lm(key) workspace.update_covariance_shape_for_lm(key)
def main(): # locations of doors (todo, read from file/cmd line) doors = [2, 4, 9] # initial setup commands k = 0 # initial setup x = 500 vel = 0 xmin = 15 xmax = 1000 - 15 mean = 500 u = np.matrix([[500.0, 0.0]]).T sigma = np.diag([200.0, 50.0]) door_dist = [0.5] * 10 #door_dist[3] = 0.25 random.seed() while k != 27: # Create a black image img = np.zeros((200, xmax + 30, 3), np.uint8) # draw environment drawDoors(img, doors) drawProbDist(img, u.item((0, 0)), sigma.item((0, 0)), (0, 255, 0)) # draw robot drawRobot(img, x) # draw sensor measurement z = np.random.normal(x, 5, 3) drawProbDist(img, int(z[0]), 10, (0, 255, 255)) #drawSensor(img, z[0]) drawDoorDist(img, door_dist) # simulate measurement door_pos = int(x / 100) door_meas = False num = random.random() if door_pos in doors: if num <= 0.6: door_meas = True # random num < 0.6 is true else: if num > 0.8: door_meas = True # random num < 0.8 is false if door_meas: drawDoorSensor(img, x) # show sim cv2.namedWindow('image', cv2.WINDOW_AUTOSIZE) cv2.imshow('image', img) door_dist = door_update(u, sigma, door_meas, door_dist) # arrow keys for control (left, right, space to stop, Esc to quit) k = cv2.waitKey(30) & 0xff if k == 81: vel -= 1 elif k == 83: vel += 1 elif k == 32: vel = 0 # add velocity noise vel_rnd = vel if vel > 0: vel_rnd += (vel + 1) * int(np.random.normal(0, 1, 1000)[25]) x += vel_rnd if x > xmax: vel = 0 x = xmax if x < xmin: vel = 0 x = xmin # add current velocity (intended) to state matrix u[1] = vel_rnd + (vel + 1) * int(np.random.normal(0, 1, 1000)[25]) # do kf update [u, sigma] = kf_update(u, sigma, z[0]) cv2.destroyAllWindows()