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