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
