cv2.imshow("Threshold Right",frame_right)
colorseg.save_settings()
if(kalman_filter_simulation):
dt = .1
time.sleep(dt)
marker_dist,marker_angle,junk1,junk2 = kalfilter.simulate_camera()
print 'Marker Dist'
print marker_dist
print 'Marker Angle'
print marker_angle
kalfilter.prediction_update(vel,ang_vel,dt,False)
dist,angle,second_dist,second_angle,frame_left,frame_right = colorseg.get_measurements(frame_left,frame_right,colorseg_debug)
if(marker_dist != -1): #-1 means invalid measurements
marker_x,marker_y = kalfilter.guess_marker(marker_dist,marker_angle)
kalfilter.measurement_update(marker_x,marker_y,marker_dist,marker_angle,second_dist,second_angle)
else:
time_elapsed = time.clock() - prior_time + delay_time
print 'Time Elapsed:' + str(time_elapsed)
prior_time = time.clock()
if(vel == 0):
kalfilter.prediction_update(0,.4*ang_vel,time_elapsed,True)
else:
kalfilter.prediction_update(.55*vel,.45*ang_vel,time_elapsed,True)
if(marker_dist != -1): #-1 means invalid measurements
print 'Marker Dist:' + str(marker_dist)
print 'Marker Angle:' + str(marker_angle)
marker_x,marker_y = kalfilter.guess_marker(marker_dist,marker_angle)
if(not colorseg_debug):
