def kalman_angle(): global kAngle_g T = 0.01 lamC = 1.0 #variance for compass lamGPS = 1.0 #variance for gps bearing lamGyro = 1.0 #variance for gyroscope sig = 1.0 #variance for model sig2 = sig**2 lamC2 = lamC**2 lamGPS2 = lamGPS**2 lamGyro2 = lamGyro**2 (A, H, Q, R) = create_model_parameters(T, sig2, lamC2) #initialize evolution, measurement, model error, and measurement error # initial state x = np.array([0, 0.1]) #starting velocity and position P = 0 * np.eye(2) #starting probability 100% kalman_filter = KalmanFilter(A, H, Q, R, x, P) #create the weights filter lastTime = time.time() startTime = lastTime lastOmega = 0 lastAngle = 0 thetaCoord_l = 0 #local variable to prevent race conditions lastBearing = 0 gpsTrust = 1.0 while True: while (omega_g == lastOmega or thetaCoord_g == lastAngle): #check if there were updates to the compass time.sleep(0.01) #if no updates then wait a bit thetaCoord_l = thetaCoord_g lastAngle = thetaCoord_l if (kalman_filter._x[0] - thetaCoord_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (thetaCoord_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 kalman_filter.predict() #evolve the state and the error kalman_filter.update((thetaCoord_l,omega_g),(lamC2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that. # while (omega_g == lastOmega or gpsTheta_g == lastBearing): #check if there were updates to the gps bearing # time.sleep(0.01) #if no updates then wait a bit gpsTheta_l = gpsTheta_g lastBearing = gpsTheta_l if (kalman_filter._x[0] - gpsTheta_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (gpsTheta_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 lamGPS2 = gpsTrust/gpsDistance_g kalman_filter.predict() #evolve the state and the error kalman_filter.update((gpsTheta_l,omega_g),(lamGPS2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that.
def compute_track(video): track = [] proposals = [] object_detected = False kalman_filter = None for t, im in enumerate(video): scores, bboxes = im_detect(sess, net, im) bboxes, scores = filter_proposals(scores, bboxes, nms_thresh=NMS_THRESH, conf_thresh=CONF_THRESH) affinities = np.zeros(scores.shape) if not object_detected: if bboxes.shape[0] == 0: track.append(np.zeros(6)) proposals.append(np.zeros((1, 6))) continue else: object_detected = True i = np.argmax(scores) xhat_0 = bboxes[i] kalman_filter = KalmanFilter(xhat_0) track.append( np.concatenate((bboxes[i], scores[i], affinities[i]))) proposals.append( np.concatenate([bboxes, scores, affinities], axis=1)) continue xhat, P = kalman_filter.update_time() score, affinity = np.zeros(1), np.zeros(1) if bboxes.shape[0] > 0: track_im = crop_and_resize(im, track[-1][:4]) bbox_ims = [crop_and_resize(im, bbox) for bbox in bboxes] affinities = [ get_affinity(track_im, bbox_im) for bbox_im in bbox_ims ] affinities = np.reshape(affinities, scores.shape) i, aff = np.argmax(affinities), np.max(affinities) if aff > AFF_THRESH: xhat, P = kalman_filter.update_measurement(bboxes[i][:4]) score, affinity = scores[i], affinities[i] track.append(np.concatenate((xhat, score, affinity))) proposals.append(np.concatenate((bboxes, scores, affinities), axis=1)) return track, proposals