def test(x, y, z, qcov, rcov): # Initializations dt = 1 # Measurement interval (s) I = lambda x: identity(x) A = zeros((9, 9)) # Transition matrix A[0:3, 0:3] = I(3) A[0:3, 3:6] = I(3) * dt A[0:3, 6:9] = I(3) * 0.5 * dt * dt A[3:6, 3:6] = I(3) A[3:6, 6:9] = I(3) * dt A[6:9, 6:9] = I(3) H = zeros((3, 9)) # Measurement matrix H[0:3, 0:3] = I(3) Q = identity(9) * qcov # Transition covariance R = identity(3) * rcov # Noise covariance B = identity(9) # Control matrix kf = KalmanFilter(A, H, Q, R, B) # Run through the dataset n = len(x) xkf, ykf, zkf = zeros(n), zeros(n), zeros(n) for i in xrange(n): kf.correct(array([x[i], y[i], z[i]])) kf.predict(array([])) Skf = kf.getCurrentState() xkf[i], ykf[i], zkf[i] = Skf[0], Skf[1], Skf[2] return xkf, ykf, zkf
class UltraSoundFilter(object): def __init__(self, us): self.ultra_sound = us self.kalman_filter = None self.last_update_time = None self.calibration_time = 3.0 self.calibration_range = None self.calibration_std = None def calibrate(self): """ Run initialization procedure on empty scene. Determine mean and std of dists within calibration_time. Can block for a while.""" ranges = [] start_time = time.time() while time.time() - start_time <= self.calibration_time: dist = self.ultra_sound.get_distance() time.sleep(0.05) if dist is not None: ranges.append(dist) if len(ranges) <= 1: raise RuntimeError("No valid ranges during calibration.") return self.calibration_range = sum(ranges) / len(ranges) sqr_error = [(dist - self.calibration_range)**2 for dist in ranges] self.calibration_std = math.sqrt(sum(sqr_error) / (len(ranges) - 1)) logger.info( f"Calibrated range as {self.calibration_range:.2f}m +- {self.calibration_std:.3f}m" ) self.kalman_filter = KalmanFilter(self.calibration_range, self.calibration_std) def update(self): """ Update the internal feature based on sensor data. """ cur_dist = self.ultra_sound.get_distance() now = time.time() if cur_dist is None: return if self.last_update_time is None: delta_t = 0 else: delta_t = now - self.last_update_time self.kalman_filter.predict(delta_t) self.kalman_filter.correct(cur_dist) self.last_update_time = now def get_distance(self): if self.kalman_filter is None: return None return self.kalman_filter.distance()
class Tracklet: STATUS_BORN = 0 STATUS_TRACKING = 1 STATUS_LOST = 2 def __init__(self, global_id, x, y, confidence, timestamp): self.id = global_id self.last_update = timestamp self.estimator = KalmanFilter(x, y, 1 - confidence) self.status = Tracklet.STATUS_BORN def get_tracklet_info(self): return { 'id': self.id, 'x': self.estimator.mu[0], 'y': self.estimator.mu[2], 'v': sqrt(self.estimator.mu[1]**2 + self.estimator.mu[3]**2), 'phi': atan2(self.estimator.mu[1], self.estimator.mu[3]), 'sigma_x': self.estimator.sigma[0][0], 'sigma_y': self.estimator.sigma[2][2], 'status': self.status } def estimate_position_at(self, timestamp): estimate = self.estimator.estimate_at(timestamp - self.last_update)[0] return estimate[0], estimate[2] def predict(self, timestamp): self.estimator.predict(math.fabs(self.last_update - timestamp)) self.last_update = timestamp def update(self, detection, confidence): self.estimator.correct(detection, 1 - confidence) def similarity(self, coordinates, timestamp): return euclidean(self.estimate_position_at(timestamp), coordinates) def confidence(self): return self.estimator.sigma[0][0] * self.estimator.sigma[2][2]
class FilterNode(): def __init__(self): # Number of states self.n = 12 # System state self.X = np.matrix(np.zeros((self.n, 1))) # Initial State Transition Matrix self.F = np.asmatrix(np.eye(self.n)) self.F[3:6, 3:6] = 0.975 * np.matrix(np.eye(3)) self.F[9:12, 9:12] = 0.975 * np.matrix(np.eye(3)) # Initial Process Matrix self.P = np.asmatrix(1.0e3 * np.eye(self.n)) # Process Noise Level self.N = 1.0e-3 # Initialize H and R matrices for optitrack pose self.Hopti = np.matrix(np.zeros((6, self.n))) self.Hopti[0:3, 0:3] = np.matrix(np.eye(3)) self.Hopti[3:6, 6:9] = np.matrix(np.eye(3)) self.Ropti = np.matrix(np.zeros((6, 6))) self.Ropti[0:3, 0:3] = 1.0 * 1.0e-3 * np.matrix(np.eye(3)) self.Ropti[3:6, 3:6] = 1.0 * 1.0e-2 * np.matrix(np.eye(3)) # Initialize Kalman Filter self.kalmanF = KalmanFilter() self.isInit = False self.lastTime = None def state_update(self, T, R, mTime): attiQ = Quaternion(R[0], R[1], R[2], R[3]) rpy = quat2rpy(attiQ) pose = np.matrix([[T[0]], [T[1]], [T[2]], [rpy[0]], [rpy[1]], [rpy[2]]]) if self.isInit: dt = mTime - self.lastTime # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Correction self.kalmanF.correct(pose, self.Hopti, self.Ropti) # Update state self.X = self.kalmanF.getState() self.lastTime = mTime else: # Initialize state self.X[0] = pose[0] self.X[1] = pose[1] self.X[2] = pose[2] self.X[6] = pose[3] self.X[7] = pose[4] self.X[8] = pose[5] # Initialize filter self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.lastTime = mTime self.isInit = True def get_state(self): if self.isInit: timeNow = time.time() dt = timeNow - self.lastTime self.lastTime = timeNow # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Update state self.X = self.kalmanF.getState() return self.X else: return None
def correct(self, y): KalmanFilter.correct(self, y, self.v.var)
red_rgb = (255, 0, 0) # light_blue_rgb = (173,216,230) draw_rectangle(rgb_frame, x1, y1, x2, y2, green_rgb, 1) cx, cy = get_center_rect(x1, y1, x2, y2) tracked_points.append([cx, cy]) if len(tracked_points) % nth_point_fade == 1 and len( tracked_points) > nth_point_fade: tracked_points.popleft() for _point in tracked_points: current_state_measurement = np.array([[_point[0]], [_point[1]]]) tracker_point = kf.predict() _ = kf.correct(current_state_measurement) # draw_circle(rgb_frame, _point[0], _point[1], 3, red_rgb) draw_circle(rgb_frame, tracker_point[0], tracker_point[1], 3, red_rgb) bgr_frame = cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR) cv2.imshow('frame', bgr_frame) if cv2.waitKey(28) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()
class FusionEKF: """ Gets inputs from multiple sources and uses a Kalman Filter to estimate the state of the system The state of the system is: X = {pD, dotpD, attD, dotattD, pJ, dotpJ, headJ, GSPoffsetJ, compassOffsetJ} The inputs are: - Drone's position in local ENU - Drone's velocity in local ENU - Drone's attitude - Drone's angular velocities - Jetyak's position in local ENU - Jetyak's heading - Jetyak's position in the Drone's body frame """ def __init__(self, F, P, N, rate): self.kalmanF = KalmanFilter() self.F = F self.P = P self.N = N self.n = np.shape(F)[0] self.X = np.matrix(np.zeros((self.n, 1))) self.dt = 1 / rate self.isInit = False self.X[0:3] = np.nan # Drone's position self.X[14:17] = np.nan # Jetyak's position self.X[19:24] = np.nan # Jetyak's GPS and heading offset def initialize(self, dataPoint): if dataPoint.getID() == 'dgps': self.X[0] = dataPoint.getZ().item(0) self.X[1] = dataPoint.getZ().item(1) self.X[2] = dataPoint.getZ().item(2) elif dataPoint.getID() == 'jgps': self.X[6] = dataPoint.getZ().item(0) self.X[7] = dataPoint.getZ().item(1) self.X[8] = dataPoint.getZ().item(2) elif dataPoint.getID() == 'tag': if (not (np.isnan(self.X[0]) or np.isnan(self.X[6]))): self.X[11] = self.X[6] - self.X[0] - dataPoint.getZ().item(0) self.X[12] = self.X[7] - self.X[1] - dataPoint.getZ().item(1) self.X[13] = self.X[8] - self.X[2] - dataPoint.getZ().item(2) self.X[14] = dataPoint.getZ().item(3) self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.kalmanF.updateF(self.dt) self.kalmanF.updateQ() self.kalmanF.predict() self.isInit = True print 'Colocalization filter initialized' def process(self, dataPoint): if not self.isInit: self.initialize(dataPoint) return None, None else: r = None P = None # KF Correction Step r, P = self.kalmanF.correct(dataPoint.getZ(), dataPoint.getH(), dataPoint.getR(), dataPoint.getChi()) if r is None: print "A ", dataPoint.getID(), " measurement was rejected" self.X = self.kalmanF.getState() return r, P def getState(self): if self.isInit: self.kalmanF.predict() return self.X else: return None def resetFilter(self): self.isInit = False self.X = np.matrix(np.zeros((self.n, 1))) self.X[0:3] = np.nan # Drone's position self.X[14:17] = np.nan # Jetyak's position self.X[19:24] = np.nan # Jetyak's GPS and heading offset
def main(): # open vid file cap = cv.VideoCapture('resources/benchmarkV9.mp4') # calculate time between frames of video dt = 1 / cap.get(cv.CAP_PROP_FPS) # initialize necessary objects cascade = cv.CascadeClassifier() backSub = cv.createBackgroundSubtractorKNN() detection = HAAR_Detection(cascade, 'detection', COLOR.BLUE, SHAPE.RECTANGLE) tracking = Tracking('tracking', COLOR.GREEN, SHAPE.RECTANGLE, TM.MOSSE) kalman_filter = KalmanFilter(dt, 'kalman_filter', COLOR.WHITE, SHAPE.RECTANGLE) # necessary variables tracking_counter = 0 frame_counter = 0 ticks = 0 # flags is_object_moving = None was_object_moving = None is_tracking_active = False # variables to adjust program detection_frame_rate = 8 tracking_detection_diff = 10 # check video vid_end, frame = cap.read() if not vid_end: print('Error during loading video') sys.exit(0) # define window name cv.namedWindow('vid_frame') # check if cascade is loading properly if not cascade.load( cv.samples.findFile( 'resources/cascades/haarcascade_frontalface_alt.xml')): print('Error loading cascade') exit(0) # main program loop while True: # read vid frame vid_end, frame = cap.read() # Check for vid end if vid_end: frame_to_draw = frame.copy() # detect movement on frame for kalman filter porpoise is_object_moving = detect_move(frame, backSub) if is_object_moving: cv.putText(frame_to_draw, "Object is moving", (20, 60), cv.FONT_HERSHEY_COMPLEX_SMALL, 1, COLOR.RED) else: cv.putText(frame_to_draw, "Object is not moving", (20, 60), cv.FONT_HERSHEY_COMPLEX_SMALL, 1, COLOR.RED) if not is_tracking_active: # activate tracking if there is object something detected detection.detect_and_draw(frame, frame_to_draw) detected_objects = detection.get_detected_objects() if len(detected_objects) > 0: roi_x, roi_y, roi_width, roi_height = detected_objects[0] tracking.init_tracker( frame, (roi_x, roi_y, roi_width, roi_height)) is_tracking_active = True else: # if tracking is active # preforming kalman filter prediction kalman_filter.predict_and_draw(frame_to_draw) # preforming tracking tracking.track_and_draw(frame, frame_to_draw) # preforming kalman filter prediction update kalman_filter.correct(tracking.get_trackedROI(), is_object_moving) # if tracking is active for indicated amount of frames perform detection to correct tracking error if tracking_counter is detection_frame_rate - 1: detected_ROIs = (detection.detect(frame)) # check if there is any object detected, if not pass if len(detected_ROIs) > 0: # to ensure that detected and tracked objects are the same object, choose detected # ROI that is the nearest to the tracked object nearest_ROI_dict = tracking.find_nearest(detected_ROIs) # if error is larger than maximal value reinitialise tracker (because there is no # other option to reset tracker) if nearest_ROI_dict[ 'distance'] > tracking_detection_diff: tracking.init_tracker(frame, tuple(detected_ROIs[0])) # count frames without detection tracking_counter = (tracking_counter + 1) % detection_frame_rate # checking for frame when object not in sight if is_object_moving and not was_object_moving: if check_roi(tuple(kalman_filter.get_predicted_bb())): tracking.init_tracker( frame, tuple(kalman_filter.get_predicted_bb())) was_object_moving = is_object_moving frame_counter += 1 cv.imshow('vid_frame', frame_to_draw) key = cv.waitKey(30) # press ecc to exit if key == 27: print("User ended program with ESC button press") break else: print("Video ended") break cap.release() cv.destroyAllWindows()
lastYTilt = evDevValue elif evDevCode == WACOM_EVCODE_DISTANCE: lastDistance = evDevValue elif evDevCode == WACOM_EVCODE_PRESSURE: if not ONLY_DEBUG: if not mouseButtonPressed and evDevValue > SCREEN_DRAW_BUTTON_PRESSURE: mouse.press(Button.left) mouseButtonPressed = True elif mouseButtonPressed and evDevValue <= SCREEN_DRAW_BUTTON_PRESSURE: mouse.release(Button.left) mouseButtonPressed = False lastPressure = evDevValue if ONLY_DEBUG: print('XPos: %5d | YPos: %5d | XTilt: %5d | YTilt: %5d | Distance: %3d | Pressure: %4d' % (lastXPos, lastYPos, lastXTilt, lastYTilt, lastDistance, lastPressure)) else: screenY = SCREEN_DRAW_AREA_FROM_X + (WACOM_WIDTH - lastXPos) * ratioX # (X doesn't need to invert) screenX = SCREEN_DRAW_AREA_FROM_Y + (WACOM_HEIGHT - lastYPos) * ratioY # (Y has to be inverted) currentMeasurement = np.array([[np.float32(screenX)], [np.float32(screenY)]]) currentPrediction = kalman.predict() cmx, cmy = currentMeasurement[0], currentMeasurement[1] cpx, cpy = currentPrediction[0], currentPrediction[1] kalman.correct(currentMeasurement) mouseMoveAbs(int(np.round(cpx)), int(np.round(cpy)))
class FilterNode(): def __init__(self, ): rp.init_node("filter") # Get rate of state publisher self.rate = rp.get_param("rate", 100) # Get VRPN client topic self.vrpn_topic = rp.get_param("vrpn_topic") # Number of states self.n = 12 # System state self.X = np.matrix(np.zeros((self.n, 1))) # Initial State Transition Matrix self.F = np.asmatrix(np.eye(self.n)) # Initial Process Matrix self.P = np.asmatrix(1.0e3 * np.eye(self.n)) # Process Noise Level self.N = 1.0e-3 # Initialize H and R matrices for optitrack pose self.Hopti = np.matrix(np.zeros((6, self.n))) self.Hopti[0:3, 0:3] = np.matrix(np.eye(3)) self.Hopti[3:6, 6:9] = np.matrix(np.eye(3)) self.Ropti = np.matrix(np.zeros((6, 6))) self.Ropti[0:3, 0:3] = 1.0e-3 * np.matrix(np.eye(3)) self.Ropti[3:6, 3:6] = 1.0e-5 * np.matrix(np.eye(3)) # Initialize Kalman Filter self.kalmanF = KalmanFilter() self.isInit = False self.lastTime = None # Set up Subscriber self.vrpn_sub = rp.Subscriber(self.vrpn_topic, PoseStamped, self.state_callback, queue_size=1) # Set up Publisher self.state_pub = rp.Publisher("opti_state/pose", PoseStamped, queue_size=1) self.state_rate_pub = rp.Publisher("opti_state/rates", TwistStamped, queue_size=1) # Create thread for publisher t = threading.Thread(target=self.statePublisher) t.start() rp.spin() def state_callback(self, msg): attiQ = Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w) rpy = quat2rpy(attiQ) pose = np.matrix([[msg.pose.position.x], [msg.pose.position.y], [msg.pose.position.z], [rpy[0]], [rpy[1]], [rpy[2]]]) if self.isInit: timeNow = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs dt = timeNow - self.lastTime # Prediction self.kalmanF.updateF(dt) self.kalmanF.updateQ() self.kalmanF.predict() # Correction self.kalmanF.correct(pose, self.Hopti, self.Ropti) # Update state self.X = self.kalmanF.getState() self.lastTime = timeNow else: # Initialize state self.X[0] = pose[0] self.X[1] = pose[1] self.X[2] = pose[2] self.X[6] = pose[3] self.X[7] = pose[4] self.X[8] = pose[5] # Initialize filter self.kalmanF.initialize(self.X, self.F, self.P, self.N) self.lastTime = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs self.isInit = True def statePublisher(self): r = rp.Rate(self.rate) while not rp.is_shutdown(): if self.isInit: timeNow = rp.Time.now() stateMsg = PoseStamped() stateMsg.header.stamp = timeNow stateMsg.header.frame_id = 'optitrack' stateMsg.pose.position.x = self.X.item(0) stateMsg.pose.position.y = self.X.item(1) stateMsg.pose.position.z = self.X.item(2) q = rpy2quat(self.X.item(6), self.X.item(7), self.X.item(8)) stateMsg.pose.orientation.x = q.x stateMsg.pose.orientation.y = q.y stateMsg.pose.orientation.z = q.z stateMsg.pose.orientation.w = q.w twistMsg = TwistStamped() twistMsg.header.stamp = timeNow twistMsg.header.frame_id = 'optitrack' twistMsg.twist.linear.x = self.X.item(3) twistMsg.twist.linear.y = self.X.item(4) twistMsg.twist.linear.z = self.X.item(5) twistMsg.twist.angular.x = self.X.item(9) twistMsg.twist.angular.y = self.X.item(10) twistMsg.twist.angular.z = self.X.item(11) self.state_pub.publish(stateMsg) self.state_rate_pub.publish(twistMsg) r.sleep()