cv2.imshow("Threshold Left",frame_left) 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)