class KalmanTrack:
"""
This class represents the internal state of individual tracked objects observed as bounding boxes.
"""
def __init__(self, initial_state):
"""
Initialises a tracked object according to initial bounding box.
:param initial_state: (array) single detection in form of bounding box [X_min, Y_min, X_max, Y_max]
"""
self.kf = KalmanFilter(dim_x=7, dim_z=4)
# Transition matrix
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
# Transformation matrix (Observation to State)
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0]])
self.kf.R[2:, 2:] *= 10. # observation error covariance
self.kf.P[4:, 4:] *= 1000. # initial velocity error covariance
self.kf.P *= 10. # initial location error covariance
self.kf.Q[-1, -1] *= 0.01 # process noise
self.kf.Q[4:, 4:] *= 0.01 # process noise
self.kf.x[:4] = xxyy_to_xysr(initial_state) # initialize KalmanFilter state
def project(self):
"""
:return: (ndarray) The KalmanFilter estimated object state in [x1,x2,y1,y2] format
"""
return xysr_to_xxyy(self.kf.x)
def update(self, new_detection):
"""
Updates track with new observation and returns itself after update
:param new_detection: (np.ndarray) new observation in format [x1,x2,y1,y2]
:return: KalmanTrack object class (itself after update)
"""
self.kf.update(xxyy_to_xysr(new_detection))
return self
def predict(self):
"""
:return: ndarray) The KalmanFilter estimated new object state in [x1,x2,y1,y2] format
"""
if self.kf.x[6] + self.kf.x[2] <= 0:
self.kf.x[6] *= 0.0
self.kf.predict()
return self.project()
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.
class PoseEstimater:
def __init__(self, N):
self.N = N
self.state_estimater = KalmanFilter(6)
self.sub = rospy.Subscriber("/kinect_head/rgb/ObjectDetection",
ObjectDetection,
self.cb_object_detection,
queue_size=10)
self.pub = rospy.Publisher('fridge_pose', PoseStamped, queue_size=10)
def cb_object_detection(self, msg):
header = copy.deepcopy(msg.header)
def convert_pose2tf(pose):
pos = pose.position
rot = pose.orientation
trans = [pos.x, pos.y, pos.z]
rot = [rot.x, rot.y, rot.z, rot.w]
return (trans, rot)
header = msg.header
assert len(msg.objects) == 1
obj = msg.objects[0]
tf_handle_to_kinect = convert_pose2tf(obj.pose)
tf_kinect_to_map = listener.lookupTransform(
'/map', '/head_mount_kinect_rgb_optical_frame', rospy.Time(0))
rpy = tf.transformations.euler_from_quaternion(tf_handle_to_kinect[1])
dist_to_kinect = np.linalg.norm(tf_handle_to_kinect[0])
print("dist to kinnect is {0}".format(dist_to_kinect))
tf_handle_to_map = convert(tf_handle_to_kinect, tf_kinect_to_map)
trans = tf_handle_to_map[0]
rpy = tf.transformations.euler_from_quaternion(tf_handle_to_map[1])
state = np.array(list(trans) + list(rpy))
std_trans = dist_to_kinect * 0.5
std_rot = dist_to_kinect * 0.1
cov = np.diag([std_trans**2] * 3 + [std_rot**2] * 3)
if self.state_estimater.isInitialized:
self.state_estimater.update(state, cov)
else:
cov_init = cov * 10
self.state_estimater.initialize(state, cov_init)
x_est, cov = self.state_estimater.get_current_est(withCov=None)
print(x_est)
pose_msg = create_posemsg_from_pose3d(x_est)
header.frame_id = "/map"
posestamped_msg = PoseStamped(header=header, pose=pose_msg)
self.pub.publish(posestamped_msg)
class Tracking(object):
def __init__(self):
self.tracking_list = []
self.data_association = DataAssociation()
self.tracking_filter = KalmanFilter()
pass
def update(self,object_list,timeframe):
if not self.tracking_list:
for object in object_list:
self.init_track(object)
else:
self.tracking_filter.predict(self.tracking_list,timeframe)
asso = self.data_association.nearest_neighbor(object_list,self.tracking_list)
self.tracking_filter.update(object_list,self.tracking_list,asso)
def init_track(self,object):
tr = Track(object)
self.tracking_list.append(tr)
def number_of_tracks(self):
return len(self.tracking_list)
def filter_data(data, x0, P, Q, R):
filter1 = KalmanFilter(x0, P, 1, 0, 1, Q, R)
x_out = np.zeros(data.size)
P_out = np.zeros(data.size)
for k in np.arange(1, data.size):
x_out[k], P_out[k] = filter1.update(0, data[k])
return x_out, P_out
class TestKalmanFilter(unittest.TestCase):
def setUp(self):
A = np.eye(2)
H = np.eye(2)
Q = np.eye(2)
R = np.eye(2)
x_0 = np.array([1, 1])
P_0 = np.eye(2)
self.kalman_filter = KalmanFilter(A, H, Q, R, x_0, P_0)
def test_predict(self):
self.kalman_filter.predict()
(x, P) = self.kalman_filter.get_state()
self.assertTrue((x == np.array([1, 1])).all())
self.assertTrue((P == 2 * np.eye(2)).all())
def test_update(self):
self.kalman_filter.update(np.array([0, 0]))
(x, P) = self.kalman_filter.get_state()
self.assertTrue((x == np.array([0.5, 0.5])).all())
self.assertTrue((P == 0.5 * np.eye(2)).all())
class fans_kalman(object):
def __init__(self, initial_x0, sigma_w, sigma_v):
self.sigma_w = sigma_w
self.sigma_v = sigma_v
self.define_kalman_parameters(initial_x0)
self.define_kalman_filter()
def define_kalman_parameters(self, initial_x0):
'initialize the parameters of kalman filter here'
dt = 1
self.F = np.array([[1, dt], [0, 1]])
self.H = np.array([1, 0]).reshape(1, 2)
self.Q = np.array([[self.sigma_w, 0.0], [0.0, self.sigma_w]])
self.R = np.array([self.sigma_v]).reshape(1, 1)
self.P = np.array([[10., 0], [0, 10.]])
'Notice that, the inital_x0 here actually is a state, which is a 2-d array'
self.initial_x0 = np.array([[initial_x0], [0]])
def define_kalman_filter(self):
self.kf = KalmanFilter(F=self.F,
H=self.H,
Q=self.Q,
R=self.R,
P=self.P,
x0=self.initial_x0)
def predict_interface(self, new_measurement):
'predict the value, and update the kalman filter'
if new_measurement is not None:
self.kf.update(new_measurement)
'Otherwise, skip the measurement-update step...just perform the time-update'
res = np.dot(self.H, self.kf.predict())[0]
'res is a scalar here'
return res
class EM:
def __init__(self):
self.kf = KalmanFilter()
def find_Q(self, data=None):
log_likelihoods = []
for i in range(0, 20):
x_posteriors, x_predictions, cov = self.run(data=data)
log_likelihoods.append(self.calculate_log_likelihood(x_posteriors, cov, data))
self.m_step(x_predictions, x_posteriors, data)
return log_likelihoods
def run(self, data=None):
x_posteriors, x_predictions, cov = [], [], []
for z in data:
self.kf.predict()
x_predictions.append(self.kf.x)
self.kf.update(z)
x_posteriors.append(self.kf.x)
cov.append(self.kf.P)
x_posteriors, x_predictions, cov = np.array(x_posteriors), np.array(x_predictions), np.array(cov)
return x_posteriors, x_predictions, cov
def calculate_log_likelihood(self, x_posteriors, cov, measurements):
log_likelihood = 0
for i in range(0, len(cov)):
S = np.dot(np.dot(self.kf.H, cov[i]), self.kf.H.T) + self.kf.R
state_posterior_in_meas_space = np.dot(self.kf.H, x_posteriors[i]).squeeze()
distribution = multivariate_normal(mean=state_posterior_in_meas_space, cov=S)
log_likelihood += np.log(distribution.pdf(measurements[i]))
return log_likelihood
def m_step(self, x_predictions, x_posteriors, measurements):
self.kf = KalmanFilter()
self.kf.Q = np.cov((x_posteriors - x_predictions).squeeze().T, bias=True)
self.kf.R = np.cov((measurements.T - np.dot(self.kf.H, x_posteriors.squeeze().T)), bias=True)
def run(filename):
arrays = []
filtered_arrays = []
sums = []
current = None
with open(filename, "rU") as csvfile:
spamreader = csv.reader(csvfile, delimiter=",", quotechar="|")
readings = list(spamreader)
for index, row in enumerate(readings):
if "#" in row[0]:
arrays.append([])
filtered_arrays.append([])
sums.append((0, 0, 0))
current = arrays[-1]
current_filtered = filtered_arrays[-1]
initial_values = list((float(i) for i in readings[index + 1]))
x_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[0])
y_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[1])
z_Kalman = KalmanFilter(p=1000, r=50, q=0.1, k=0, initial_value=initial_values[2])
else:
x, y, z = tuple(float(i) for i in row)
current.append(tuple((x, y, z)))
x_filt = x_Kalman.update(x)
y_filt = y_Kalman.update(y)
z_filt = z_Kalman.update(z)
current_filtered.append(tuple((x_filt, y_filt, z_filt)))
x += sums[-1][0] + x_filt
y += sums[-1][1] + y_filt
z += sums[-1][2] + z_filt
sums[-1] = (x, y, z)
display_graphs(arrays, filtered_arrays, sums)
def run(filename):
current_filtered = []
sums = []
current = []
x_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0)
x_Kalmanq=KalmanFilter(p=1000, r=100, q=1, k=0)
x_Kalmanr=KalmanFilter(p=1000, r=5000, q=1, k=0)
y_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0)
y_Kalmanq = KalmanFilter(p=1000, r=100, q=10, k=0)
y_Kalmanr = KalmanFilter(p=1000, r=500, q=1, k=0)
z_Kalman = KalmanFilter(p=1000, r=100, q=10, k=0)
z_Kalmanq = KalmanFilter(p=1000, r=100, q=1, k=0)
z_Kalmanr = KalmanFilter(p=1000, r=500, q=10, k=0)
with open(filename, "rU") as csvfile:
spamreader = csv.reader(csvfile, delimiter=',', quotechar='|')
readings = list(spamreader)
x_Kalman.x,y_Kalman.x,z_Kalman.z=[float(i) for i in readings[0]]
x_Kalman.x=x_Kalmanr.x=x_Kalman.x
y_Kalman.x=y_Kalmanr.x=y_Kalman.x
z_Kalman.x=z_Kalmanr.x=z_Kalman.x
for index, row in enumerate(readings):
if(index >= START and index <= END):
x, y, z = tuple(float(i) for i in row)
current.append(tuple((x, y, z)))
x_filt = x_Kalman.update(x)
x_filtq = x_Kalmanq.update(x)
x_filtr = x_Kalmanq.update(x)
y_filt = y_Kalman.update(y)
z_filt = z_Kalman.update(z)
y_filtq = y_Kalmanq.update(y)
y_filtr = y_Kalmanq.update(y)
z_filtq = z_Kalmanq.update(z)
z_filtr = z_Kalmanq.update(z)
current_filtered.append(tuple(((x_filt,x_filtq,x_filtr), (y_filt,y_filtq,y_filtr), (z_filt,z_filtq,z_filtr))))
display_graphs(current, current_filtered)
x_estimated = [x_init]
cov_estimated = [Sigma_init]
x_real_list = [np.array([[0, 0, 5, 10]]).transpose()]
x_real = x_real_list[0]
z_list = []
for i in xrange(100):
kf.predict(u)
# simulate noisy observation
x_real = A.dot(x_real)+u
z = x_real[0:2, :] + np.random.multivariate_normal(np.zeros((2,)), R).reshape((2,1))
kf.update(z)
x_estimated.append(kf.x)
cov_estimated.append(kf.Sigma)
x_real_list.append(x_real)
z_list.append(z)
plt.figure()
px = np.array([ pv[0, 0] for pv in x_real_list])
py = np.array([ pv[1, 0] for pv in x_real_list])
plt.plot(px, py, 'r', label="True position")
gx = np.array([ pv[0, 0] for pv in x_estimated])
gy = np.array([ pv[1, 0] for pv in x_estimated])
plt.plot(gx, gy, 'b', label="KF position estimate")
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count1 = 0
count2 = 0
def __init__(self, bbox, channel, reset_id):
"""
Initialises a tracker using initial bounding box.
"""
self.channel = channel
self.F = np.array(
[[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]])
self.H = np.array(
[[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]])
self.kf = KalmanFilter(F=self.F, H=self.H)
self.kf.R[2:, 2:] *= 10.
self.kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities
self.kf.P *= 10.
self.kf.Q[-1, -1] *= 0.01
self.kf.Q[4:, 4:] *= 0.01
self.kf.x[:4], self.score, self.class_type = convert_bbox_to_z(bbox)
self.time_since_update = 0
if channel == 1:
if reset_id:
# print('channel1 is reset to 0')
KalmanBoxTracker.count1 = 0
self.id = KalmanBoxTracker.count1
KalmanBoxTracker.count1 += 1
elif channel == 2:
if reset_id:
# print('channel2 is reset to 0')
KalmanBoxTracker.count2 = 0
self.id = KalmanBoxTracker.count2
KalmanBoxTracker.count2 += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox)[0])
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if (self.kf.x[6] + self.kf.x[2]) <= 0:
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if self.time_since_update > 0:
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
def get_area(self):
if self.class_type == 0:
return 1.2
elif self.class_type == 1:
return 4.48
elif self.class_type == 2:
return 5.36
elif self.class_type == 3:
return 24.74
def get_velocity(self):
velocity_x = self.kf.x[4]
velocity_y = self.kf.x[5]
magnitude = int(np.sqrt(velocity_x ** 2 + velocity_y ** 2))
if (velocity_x != 0) & (velocity_y != 0):
direction = int(np.arctan(velocity_y / velocity_x) * (180 / np.pi))
return magnitude, direction
else:
return 0, 0
class FusionEKF:
def __init__(self):
self.kalman_filter = KalmanFilter()
self.is_initialized = False
self.previous_timestamp = 0
self.noise_ax = 1
self.noise_ay = 1
self.noise_az = 1
self.noise_vector = np.diag(
[self.noise_ax, self.noise_ay, self.noise_az])
self.kalman_filter.P = np.array([[1, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0,
0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
self.kalman_filter.x = np.zeros((6, 1))
# self.uwb_std = 23.5 / 1000
self.uwb_std = 23.5 / 10
self.R_UWB = np.array([[self.uwb_std**2]])
self.kalman_filter.R = self.R_UWB
def process_measurement(self, anchor_pose, anchor_distance):
if not self.is_initialized:
self.is_initialized = True
self.previous_timestamp = time.time()
return
current_time = time.time()
dt = current_time - self.previous_timestamp
self.previous_timestamp = current_time
self.calulate_F_matrix(dt)
self.calculate_Q_matrix(dt)
self.kalman_filter.predict()
self.kalman_filter.update(anchor_pose, anchor_distance)
# print("X:", self.kalman_filter.x)
# print("P:", self.kalman_filter.P)
# print(self.kalman_filter.Q)
def calulate_F_matrix(self, dt):
self.kalman_filter.F = np.array([[1, 0, 0, dt, 0, 0],
[0, 1, 0, 0, dt, 0],
[0, 0, 1, 0, 0,
dt], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
def calculate_Q_matrix(self, dt):
a = [[(dt**4) / 4, (dt**3) / 2], [(dt**3) / 2, dt**2]]
self.kalman_filter.Q = np.kron(a, self.noise_vector)
log.new_log('covariance')
log.new_log('moving average')
# Moving average for measurements
moving_avg = MovingAverage(15)
# Number of iterations to perform
iterations = 100
for i in range(0, iterations):
# Generate a random acceleration
w = matrix(random.multivariate_normal([0.0, 0.0], Q)).T
# Predict
(X, P) = kf.predict(X, P, w)
# Update
(X, P) = kf.update(X, P, Z)
# Update the actual position
A = F * A + w
# Synthesise a new noisy measurement distributed around the real position
Z = matrix([[random.normal(A[0, 0], sigma_z)], [0.0]])
# Update the moving average with the latest measured position
moving_avg.update(Z[0, 0])
# Update the log for plotting later
log.log('measurement', Z[0, 0])
log.log('estimate', X[0, 0])
log.log('actual', A[0, 0])
log.log('time', i * dt)
log.log('covariance', P[0, 0])
log.log('moving average', moving_avg.getAvg())
# Plot the system behaviour
log.new_log('covariance')
log.new_log('moving average')
# Moving average for measurements
moving_avg = MovingAverage(15)
# Number of iterations to perform
iterations = 100
for i in range(0, iterations):
# Generate a random acceleration
w = matrix(random.multivariate_normal([0.0, 0.0], Q)).T
# Predict
(X, P) = kf.predict(X, P, w)
# Update
(X, P) = kf.update(X, P, Z)
# Update the actual position
A = F * A + w
# Synthesise a new noisy measurement distributed around the real position
Z = matrix([[random.normal(A[0, 0], sigma_z)], [0.0]])
# Update the moving average with the latest measured position
moving_avg.update(Z[0, 0])
# Update the log for plotting later
log.log('measurement', Z[0, 0])
log.log('estimate', X[0, 0])
log.log('actual', A[0, 0])
log.log('time', i * dt)
log.log('covariance', P[0, 0])
log.log('moving average', moving_avg.getAvg())
# Plot the system behaviour
delta_t = 0.5 # predictions are going to be made for 0.5 seconds in the future
A = np.array([[1, delta_t], [0, 1]])
B = np.zeros((2, 2))
G = np.identity(2)
H = np.array([[1, 0]])
sigma_w = 0.1 # 10cm = standard deviation of position prediction noise for one time step of dt seconds
Q = (sigma_w**2) * np.identity(2)
sigma_n = 0.2 # 20cm = standard deviation of position measurement noise
R = (sigma_n**2) * np.identity(1)
x_init = np.array([[0, 5]]).transpose()
sigma_p = 1 # 10cm = uncertainty about initial position
sigma_v = 20 # 20m/s = uncertainty about initial velocity
Sigma_init = np.array([[sigma_p**2, 0], [0, sigma_v**2]])
kf = KalmanFilter(A, B, G, H, Q, R, x_init, Sigma_init)
plot_mean_and_covariance(kf.x, kf.Sigma)
kf.predict()
plot_mean_and_covariance(kf.x, kf.Sigma)
kf.update(z=5)
plot_mean_and_covariance(kf.x, kf.Sigma)
class SensorFusion:
X_INDEX = 0
Y_INDEX = 1
VX_INDEX = 2
VY_INDEX = 3
AX_INDEX = 4
AY_INDEX = 5
def __init__(self):
self.resetup([True] * 6)
self._kf = KalmanFilter()
def resetup(self, maintain_state_flag):
self._maintain_state_flag = copy.copy(maintain_state_flag)
self._maintain_state_size = self._maintain_state_flag.count(True)
print(self._maintain_state_size)
print(self._maintain_state_flag)
# lidar H will not change even if we change the state we maintain
self._lidar_H = np.zeros(self._maintain_state_size * 2).reshape(2, -1)
self._lidar_H[0, 0], self._lidar_H[1, 1] = 1.0, 1.0
'''setup F matrix, if we maintain acceleration,
it should be reflected in F matrix'''
def setup_F(self, dt):
self._F = np.identity(self._maintain_state_size)
self._F[SensorFusion.X_INDEX, SensorFusion.VX_INDEX] = dt
self._F[SensorFusion.Y_INDEX, SensorFusion.VY_INDEX] = dt
if self._maintain_state_flag[SensorFusion.AX_INDEX]:
self._F[SensorFusion.VX_INDEX, SensorFusion.AX_INDEX] = dt
if self._maintain_state_flag[SensorFusion.AY_INDEX]:
self._F[SensorFusion.VY_INDEX, SensorFusion.AY_INDEX] = dt
def setup_lidar_H(self):
self._kf.setH(self._lidar_H)
def update_with_lidar_obs(self, lidar_obs):
z = np.array([lidar_obs.get_local_px(),
lidar_obs.get_local_py()]).reshape(-1, 1)
self._kf.setMeasurement(z)
self._kf.setH(self._lidar_H)
print("lidar_H ", self._lidar_H)
self._kf.update()
return self._kf.getState(), self._kf.getP()
def predict(self, dt, warp):
rotation = warp[:2, :2]
translation = warp[:2, 2]
self._kf.warp(rotation, translation)
self.setup_F(dt)
self._kf.setF(self._F)
self._kf.predict()
def update_with_radar_obs(self, radar_obs, use_lat_v=False):
z = radar_obs.get_radar_measurement().reshape(-1, 1)
if not use_lat_v:
z = z[:3, :].reshape(-1, 1)
print("radar z", z)
self._kf.setMeasurement(z)
self.setup_radar_H(use_lat_v)
print("radar H", self._kf.getH())
self._kf.update()
return self._kf.getState(), self._kf.getP()
def setup_radar_H(self, use_lat_v=False):
if use_lat_v:
self.setup_radar_H_w_lat(self.get_state())
else:
self.setup_radar_H_wo_lat(self.get_state())
def cal_radar_observe_wo_lat(self, state):
state = copy.copy(state)
x, y, vx, vy = state[0], state[1], state[2], state[3]
R = np.sqrt(x**2 + y**2)
theta = np.arctan2(y, x)
dRdt = vx * np.cos(theta) + vy * np.sin(theta)
return np.array([R, theta, dRdt]).reshape(-1, 1)
def setup_radar_H_wo_lat(self, state):
state = copy.copy(state)
state.reshape(-1, 1)
x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0]
obs = self.cal_radar_observe_wo_lat(state)
R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0]
dR_dx = x / R
dR_dy = y / R
dR_dvx = 0.0
dR_dvy = 0.0
dtheta_dx = -y / (R**2)
dtheta_dy = x / (R**2)
dtheta_dvx = 0.0
dtheta_dvy = 0.0
dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \
vy * np.cos(theta) * dtheta_dx
dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \
vy * np.cos(theta) * dtheta_dy
dRdt_dvx = np.sin(theta)
dRdt_dvy = np.cos(theta)
H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy],
[dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy],
[dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy]])
self._kf.setH(H)
def cal_radar_observe_w_lat(self, state_):
state = copy.copy(state_)
x, y, vx, vy = state[0], state[1], state[2], state[3]
R = np.sqrt(x**2 + y**2)
theta = np.arctan2(y, x)
dRdt = vx * np.cos(theta) + vy * np.sin(theta)
dLdt = -vx * np.sin(theta) + vy * np.cos(theta)
return np.array([R, theta, dRdt, dLdt], dtype=float).reshape(-1, 1)
def setup_radar_H_w_lat(self, state):
state = copy.copy(state)
state.reshape(-1, 1)
x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0]
obs = self.cal_radar_observe_w_lat(state)
R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0]
dR_dx = x / R
dR_dy = y / R
dR_dvx = 0.0
dR_dvy = 0.0
dtheta_dx = -y / (R**2)
dtheta_dy = x / (R**2)
dtheta_dvx = 0.0
dtheta_dvy = 0.0
dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \
vy * np.cos(theta) * dtheta_dx
dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \
vy * np.cos(theta) * dtheta_dy
dRdt_dvx = np.sin(theta)
dRdt_dvy = np.cos(theta)
dLdt_dx = -vx * np.cos(theta) * dtheta_dx - \
vy * np.sin(theta) * dtheta_dx
dLdt_dy = -vx * np.cos(theta) * dtheta_dy - \
vy * np.sin(theta) * dtheta_dy
dLdt_dvx = -np.sin(theta)
dLdt_dvy = np.cos(theta)
H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy],
[dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy],
[dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy],
[dLdt_dx, dLdt_dy, dLdt_dvx, dLdt_dvy]])
self._kf.setH(H)
def set_maintain_state(self, state_flags):
self.resetup(state_flags)
def set_P(self, P):
self._kf.setP(
P[:self._maintain_state_size, :self._maintain_state_size])
def get_P(self):
return self._kf.getP()
def set_Q(self, Q):
self._kf.setQ(
Q[:self._maintain_state_size, :self._maintain_state_size])
def get_Q(self):
return self._kf.getQ()
def set_R(self, R):
self._kf.setR(R)
def get_R(self):
return self._kf.getR()
def set_state(self, state):
print("state:", state)
if len(state) != self._maintain_state_size:
print(" STATE dimension not equals to maintain state size")
s = []
for i in range(len(self._maintain_state_flag)):
if self._maintain_state_flag[i]:
s.append(state[i])
s = np.array(s).reshape(-1, 1)
self._kf.setState(s)
def get_state(self):
return self._kf.getState()
def get_state_in_lidar_format(self, Twl):
state = self.get_state()
px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0]
if len(state) == 6:
ax, ay = state[4, 0], state[5, 0]
else:
ax, ay = 0, 0
return LidarObservation(px,
py,
vx,
vy,
Twl=Twl,
local_ax=ax,
local_ay=ay)
def get_state_in_radar_format(self, Twr):
state = self.get_state()
px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0]
R = np.sqrt(px**2 + py**2)
theta = np.arctan2(py, px)
range_v = vx * np.cos(theta) + vy * np.sin(theta)
lat_v = -vx * np.sin(theta) + vy * np.cos(theta)
return RadarObservation(R, theta, range_v, lat_v, Twr)
class MainWindow(QtGui.QMainWindow):
# Signals:
connect_signal = QtCore.pyqtSignal(str, int)
disconnect_signal = QtCore.pyqtSignal()
def __init__(self, parent=None):
super().__init__(parent)
self._initialise_gui_widgets()
self._setup_gui()
self.connected = False
self.time_zero = time.time()
def _initialise_gui_widgets(self):
"""
Create all the widgets for the window.
"""
# # Main widget
self.main_widget = QtGui.QWidget()
# # Address label
self.address_label = QtGui.QLabel("Server:")
self.address_label.setAlignment(QtCore.Qt.AlignRight
| QtCore.Qt.AlignVCenter)
# # Address bar
self.address_bar = QtGui.QLineEdit()
# Create a regex validator for the IP address
ip_values = "(?:[0-1]?[0-9]?[0-9]|2[0-4][0-9]|25[0-5])"
ip_regex = QtCore.QRegExp("^" + ip_values + "\\." + ip_values + "\\." +
ip_values + "\\." + ip_values + "$")
ip_validator = QtGui.QRegExpValidator(ip_regex, self)
self.address_bar.setValidator(ip_validator)
self.address_bar.setText("192.168.42.1")
# # Port bar
self.port_bar = QtGui.QLineEdit()
self.port_bar.setValidator(QtGui.QIntValidator())
self.port_bar.setText("12345")
# # Connect button
self.connect_button = QtGui.QPushButton("&Connect")
self.connect_button.clicked.connect(self._toggle_connect)
# # Plot widgets
self.plot_widgets = {
'accelerometer': plt.UpdatingPlotWidget(title='Accelerometer'),
'magnetometer': plt.UpdatingPlotWidget(title='Magnetometer'),
'gyroscope': plt.UpdatingPlotWidget(title='Gyroscope'),
'barometer': plt.UpdatingPlotWidget(title='Barometer'),
'gps-pos': plt.UpdatingPlotWidget(title='GPS Position')
}
for name, widget in self.plot_widgets.items():
if name == 'barometer':
widget.add_item('altitude', pen='k')
widget.add_item('filtered', pen='r')
widget.add_item('altitude_gps', pen='b')
self.filter1 = KalmanFilter(x_prior=0,
P_prior=2,
A=1,
B=0,
H=1,
Q=0.005,
R=1.02958)
elif name == 'gps-pos':
widget.add_item('position', symbol='o')
else:
widget.add_item('x', pen='r')
widget.add_item('y', pen='g')
widget.add_item('z', pen='b')
def _setup_gui(self):
"""
Add all the widgets to the window.
Layout:
0 1 2 3 4 5
|-----------------------------------------------------|
0 | | | Label | Address | Port | Connect |
|-----------------------------------------------------|
1 | Accelerometer | Gyroscope |
|-----------------------------------------------------|
2 | Magnetometer | Barometer |
|-----------------------------------------------------|
3 | GPS XY | | | | | |
|-----------------------------------------------------|
"""
# # Window setup
self.setWindowTitle('Remote Display')
self.resize(450, 800)
# # Main widget setup
self.main_widget = QtGui.QWidget()
self.setCentralWidget(self.main_widget)
# # Layout setup
self.grid = QtGui.QGridLayout()
self.main_widget.setLayout(self.grid)
self.grid.setRowMinimumHeight(0, 50)
self.grid.setRowMinimumHeight(1, 200)
self.grid.setRowMinimumHeight(2, 200)
self.grid.setRowMinimumHeight(3, 200)
self.grid.setColumnMinimumWidth(0, 200)
self.grid.setColumnMinimumWidth(1, 200)
self.grid.setColumnMinimumWidth(2, 200)
self.grid.setColumnMinimumWidth(3, 200)
self.grid.setColumnMinimumWidth(4, 200)
self.grid.setColumnMinimumWidth(5, 200)
self.grid.setSpacing(10)
self.main_widget.setLayout(self.grid)
# # Add widgets to layout
self.grid.addWidget(self.address_label, 0, 2)
self.grid.addWidget(self.address_bar, 0, 3)
self.grid.addWidget(self.port_bar, 0, 4)
self.grid.addWidget(self.connect_button, 0, 5)
for name, widget in self.plot_widgets.items():
if name == 'accelerometer':
self.grid.addWidget(widget, 1, 0, 1, 3)
elif name == 'gyroscope':
self.grid.addWidget(widget, 1, 3, 1, 3)
elif name == 'magnetometer':
self.grid.addWidget(widget, 2, 0, 1, 3)
elif name == 'barometer':
self.grid.addWidget(widget, 2, 3, 1, 3)
elif name == 'gps-pos':
self.grid.addWidget(widget, 3, 0, 1, 1)
@QtCore.pyqtSlot()
def _toggle_connect(self):
"""
Slot for when connect button is clicked
"""
if self.connected:
self.disconnect_signal.emit()
self.connect_button.setText("&Connect")
self.address_bar.setEnabled(True)
self.port_bar.setEnabled(True)
self.connected = False
else:
address = self.address_bar.text()
port = int(self.port_bar.text())
self.connect_signal.emit(address, port)
self.connect_button.setText("Dis&connect")
self.address_bar.setEnabled(False)
self.port_bar.setEnabled(False)
self.connected = True
@QtCore.pyqtSlot(list)
def data_received(self, received_data):
"""
Slot for when data is received - updates the plots
"""
data_source = received_data[0]
values = received_data[1]
t = time.time() - self.time_zero
if data_source == com.BAROMETER_UNFILTERED_ID:
self.plot_widgets['barometer'].get_item('altitude').update_data(
t, values[0])
self.plot_widgets['barometer'].get_item('filtered').update_data(
t,
self.filter1.update(u_input=0, z_measurement=values[0])[0])
elif data_source == com.ACCELEROMETER_ID:
if len(values) != 3:
print(
"Error: Expected 3 values for accelerometer data, got {}".
format(len(values)))
else:
self.plot_widgets['accelerometer'].get_item('x').update_data(
t, values[0])
self.plot_widgets['accelerometer'].get_item('y').update_data(
t, values[1])
self.plot_widgets['accelerometer'].get_item('z').update_data(
t, values[2])
elif data_source == com.MAGNETOMETER_ID:
if len(values) != 3:
print("Error: Expected 3 values for magnetometer data, got {}".
format(len(values)))
else:
self.plot_widgets['magnetometer'].get_item('x').update_data(
t, values[0])
self.plot_widgets['magnetometer'].get_item('y').update_data(
t, values[1])
self.plot_widgets['magnetometer'].get_item('z').update_data(
t, values[2])
elif data_source == com.GYROSCOPE_ID:
if len(values) != 3:
print("Error: Expected 3 values for gyroscope data, got {}".
format(len(values)))
else:
self.plot_widgets['gyroscope'].get_item('x').update_data(
t, values[0])
self.plot_widgets['gyroscope'].get_item('y').update_data(
t, values[1])
self.plot_widgets['gyroscope'].get_item('z').update_data(
t, values[2])
elif data_source == com.GPS_POS_ID:
if len(values) != 2:
print(
"Error: Expected 2 values for GPS position, got {}".format(
len(values)))
else:
self.plot_widgets['gps-pos'].get_item('position').update_data(
values[0], values[1])
elif data_source == com.GPS_ALT_ID:
if len(values) != 1:
print(
"Error: Expected 1 value for GPS altitude, got {}".format(
len(values)))
else:
self.plot_widgets['barometer'].get_item(
'altitude_gps').update_data(t, values[0])
np.random.seed(21)
(A, H, Q, R) = create_model_parameters()
K = 20
# initial state
x = np.array([0, 0.5, 0, 0.5])
P = 0 * np.eye(4)
(state, meas) = simulate_system(K, x)
kalman_filter = KalmanFilter(A, H, Q, R, x, P)
est_state = np.zeros((K, 4))
est_cov = np.zeros((K, 4, 4))
for k in range(K):
kalman_filter.predict()
kalman_filter.update(meas[k, :])
(x, P) = kalman_filter.get_state()
est_state[k, :] = x
est_cov[k, ...] = P
plt.figure(figsize=(7, 5))
plt.plot(state[:, 0], state[:, 2], '-bo')
plt.plot(est_state[:, 0], est_state[:, 2], '-ko')
plt.plot(meas[:, 0], meas[:, 1], ':rs')
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.legend(['true state', 'inferred state', 'observed measurement'])
plt.axis('square')
plt.tight_layout(pad=0)
plt.show()
sift = Sift(gray_frame)
while is_frame_good:
cv2.imshow('video 0', frame)
prev_f = frame
is_frame_good, frame = video.read()
# Redraw frame with new bounding box
cv2.rectangle(frame, (bounding_box[0][0][0], bounding_box[0][1][0]),
(bounding_box[0][2][0], bounding_box[0][3][0]),
(255, 0, 0), 2)
"""
Correct filter with new bounding box on previous frame
Predict bounding box in new frame
"""
bounding_box[0] = kf.update(bounding_box[0])
"""
Extract features using sift in previous image
Match features extracted in previous image with current image (updates bounding box too)
"""
pdb.set_trace()
features = sift.extract_features(bounding_box[0])
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
matches, bounding_box[0] = sift.match_features(gray_frame)
# Update previous frame
cv2.waitKey(25) # delays next video frams (ms)
video.release()
cv2.destroyAllWindows()
class Track:
def __init__(self, bb, id_, max_miss_):
self.track = [bb]
self.alive = True
self.kf = KalmanFilter()
self.id_track = id_
self.mean, self.cov = self.kf.initiate(bb.asp_ratio())
self.num_miss = 0
self.num_max_miss = max_miss_
self.project_mean = 0
self.color = (int(random.random() * 256), int(random.random() * 256),
int(random.random() * 256))
def last(self):
return self.track[-1]
def get_last_mean(self):
return self.mean[0:4]
def append(self, bb):
self.kalman_steps(bb.asp_ratio())
aproxBB = BoudingBox(coord=_restoreStandardValue(self.get_last_mean()),
frame=bb.getIDFrame())
self.track.append(aproxBB)
def kalman_steps(self, dets):
self.mean, self.cov = self.kf.predict(self.mean, self.cov)
self.mean, self.cov = self.kf.update(self.mean, self.cov, dets)
def getTrack(self):
return self.track
def getBBFrame(self, frame_id):
for bb in self.track:
if bb.getIDFrame() == frame_id:
return bb
def __len__(self):
return len(self.track)
def is_alive(self):
return self.alive
def ressurect(self):
self.alive = True
def project_position(self):
self.append(
BoudingBox(_restoreStandardValue(self.mean[:4]),
self.last().getIDFrame() + 1))
def missed(self):
self.num_miss += 1
self.project_position()
if self.num_miss > self.num_max_miss:
self.kill()
def kill(self):
self.alive = False
def getID(self):
return self.id_track
def getcolor(self):
return self.color
tx= range(0,len(samples),25) for i in tx: tens.append(samples[i:(i+25)]) m=max(samples) print (m,samples.index(m)) m=min(samples) print (m,samples.index(m)) t=[mean(i) for i in tens ] s=[pstdev(i) for i in tens ] v=[pvariance(i) for i in tens] print(s) print(max(s),sum(s)) print(v) print(max(v),sum(v)) print(pstdev(samples)) kf = KalmanFilter(p=1000, r=100, q=0.5, k=0, initial_value=samples[0]) kf2 = KalmanFilter(p=300, r=100, q=0.1, k=0, initial_value=samples[0]) kf3 = KalmanFilter(p=1000, r=100, q=0.1, k=0, initial_value=samples[0]) f = [kf.update(i) for i in samples] f2 = [kf2.update(i) for i in samples] f3 = [kf3.update(i) for i in samples] plt.plot(x,samples,x,f,x,f2,tx,t,x,f3) plt.show()
class EKF:
def __init__(self):
self.is_initialized = False
self.previous_timestamp = 0
# Initialize measurement covariance matrix - laser
self.R_laser = np.array([[0.0225, 0.0], [0.0, 0.0225]],
dtype=np.float32)
# Initialize measurement covariance matrix - radar
self.R_radar = np.array(
[[0.09, 0.0, 0.0], [0.0, 0.0009, 0.0], [0.0, 0.0, 0.09]],
dtype=np.float32)
# Initialize state variable
x_init = np.zeros(4, dtype=np.float32)
# Initialize state covariance matrix
P_init = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# System model - dt not yet taken into account
F_init = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# Transformation from state variable to measurement
H_init = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float32)
# Covariance matrix of process noise - dt not yet taken into account
Q_init = np.array([
[1, 0, 1, 0],
[0, 1, 0, 1],
[1, 0, 1, 0],
[0, 1, 0, 1],
],
dtype=np.float32)
# Initialize our Kalman filter
self.ekf = KalmanFilter(x_init, P_init, F_init, H_init, self.R_laser,
Q_init)
# Set the process noise constants
self.noise_ax = 9.0
self.noise_ay = 9.0
def process_measurement(self, m):
if not self.is_initialized:
# First measurement
if m['sensor_type'] == 'R':
# Convert radar data in polar to cartesian coordinates
# (Note that rho_dot from the very first measurement is
# not used))
px = m['rho'] * np.cos(m['phi'])
py = m['rho'] * np.sin(m['phi'])
self.ekf.x = np.array([px, py, 0.0, 0.0])
self.previous_timestamp = m['timestamp']
elif m['sensor_type'] == 'L':
# Initialize state variable
px, py = m['x'], m['y']
self.ekf.x = np.array([px, py, 0, 0])
self.previous_timestamp = m['timestamp']
self.is_initialized = True
return
# Prediction
# Update state transition matrix (time measured in seconds)
dt = (m['timestamp'] - self.previous_timestamp) / 1000000.0
self.previous_timestamp = m['timestamp']
self.ekf.F[0][2] = dt
self.ekf.F[1][3] = dt
# Set the process noise covariance
dt2 = dt * dt
dt3 = dt2 * dt
dt4 = dt3 * dt
self.ekf.Q = np.array(
[[dt4 * self.noise_ax / 4.0, 0.0, dt3 * self.noise_ax / 2.0, 0.0],
[0.0, dt4 * self.noise_ay / 4.0, 0.0, dt3 * self.noise_ay / 2.0],
[dt3 * self.noise_ax / 2.0, 0.0, dt2 * self.noise_ax, 0.0],
[0.0, dt3 * self.noise_ay / 2.0, 0.0, dt2 * self.noise_ay]],
dtype=np.float32)
self.ekf.predict()
# Measurement update
if m['sensor_type'] == 'R':
# Radar updates
z = np.array([m['rho'], m['phi'], m['rho_dot']], dtype=np.float32)
self.ekf.R = self.R_radar
self.ekf.update_ekf(z)
elif m['sensor_type'] == 'L':
# Laser updates
z = np.array([m['x'], m['y']], dtype=np.float32)
self.ekf.R = self.R_laser
self.ekf.update(z)
def AMM_predict(self,
interpolated_track_dict,
x_in=np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose()):
# ========== parameter setting ========== #
x_in = np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose()
# print('x_in',x_in)
P_in = np.mat([[100.0, 0, 0, 0], [0, 100.0, 0, 0], [0, 0, 100.0, 0],
[0, 0, 0, 100.0]])
H_in = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]])
R_in = np.mat([[0.1, 0], [0, 0.1]])
## tuning ?
t = 0.5
t_2 = t * t
t_3 = t_2 * t
t_4 = t_3 * t
alpha_ax_2 = 2.25 * 2.25
alpha_ay_2 = 2.25 * 2.25
Q_in = np.mat([[t_4 / 4 * alpha_ax_2, t_3 / 2 * alpha_ax_2, 0, 0],
[t_3 / 2 * alpha_ax_2, t_2 * alpha_ax_2, 0, 0],
[0, 0, t_4 / 4 * alpha_ay_2, t_3 / 2 * alpha_ay_2],
[0, 0, t_3 / 2 * alpha_ay_2, t_2 * alpha_ay_2]])
# ========== parameter setting ========== #
# print('interpolated_track_dict', interpolated_track_dict)
for track_id in interpolated_track_dict.keys():
track_ids = []
# print('track_id', track_id)
track_ids.append(track_id)
history_trajectory_x = interpolated_track_dict[track_id][0]
# print('history_trajectory_x',history_trajectory_x)
history_trajectory_y = interpolated_track_dict[track_id][1]
# print('history_trajectory_y', history_trajectory_y)
#x_in = np.mat([[history_trajectory_x[0], history_trajectory_y[0], 0.0, 0.0]]).transpose()
kf0 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf1 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf2 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf3 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf4 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf5 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf6 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf7 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf8 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
for i in range(len(history_trajectory_x) - 1, -1, -1):
#===================== estimation done =================== #
pose_xy = np.mat(
[history_trajectory_x[i], history_trajectory_y[i]])
# print('pose_xy', pose_xy, i)
kf0.predict('CV', t)
kf0.update(pose_xy.transpose()) #need to transpose?
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf0.S_)
L_cv = gaussain_distribution.pdf(kf0.y_.transpose())
L_cv = max(1e-30, L_cv)
kf1.predict('CTA_a1', t)
kf1.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf1.S_)
L_cta_a1 = gaussain_distribution.pdf(kf1.y_.transpose())
L_cta_a1 = max(1e-30, L_cta_a1)
kf2.predict('CTA_a2', t)
kf2.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf2.S_)
L_cta_a2 = gaussain_distribution.pdf(kf2.y_.transpose())
L_cta_a2 = max(1e-30, L_cta_a2)
kf3.predict('CTA_d1', t)
kf3.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf3.S_)
L_cta_d1 = gaussain_distribution.pdf(kf3.y_.transpose())
L_cta_d1 = max(1e-30, L_cta_d1)
kf4.predict('CTA_d2', t)
kf4.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf4.S_)
L_cta_d2 = gaussain_distribution.pdf(kf4.y_.transpose())
L_cta_d2 = max(1e-30, L_cta_d2)
kf5.predict('CT_l1', t)
kf5.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf5.S_)
L_ct_l1 = gaussain_distribution.pdf(kf5.y_.transpose())
L_ct_l1 = max(1e-30, L_ct_l1)
kf6.predict('CT_l2', t)
kf6.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf6.S_)
L_ct_l2 = gaussain_distribution.pdf(kf6.y_.transpose())
L_ct_l2 = max(1e-30, L_ct_l2)
kf7.predict('CT_r1', t)
kf7.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf7.S_)
L_ct_r1 = gaussain_distribution.pdf(kf7.y_.transpose())
L_ct_r1 = max(1e-30, L_ct_r1)
kf8.predict('CT_r2', t)
kf8.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf8.S_)
L_ct_r2 = gaussain_distribution.pdf(kf8.y_.transpose())
L_ct_r2 = max(1e-30, L_ct_r2)
prob_sum = self.AMM_prob_dict['CV'] * L_cv + \
self.AMM_prob_dict['CTA_a1'] * L_cta_a1 + self.AMM_prob_dict['CTA_a2'] * L_cta_a2 + \
self.AMM_prob_dict['CTA_d1'] * L_cta_d1 + self.AMM_prob_dict['CTA_d2'] * L_cta_d2 + \
self.AMM_prob_dict['CT_l1'] * L_ct_l1 + self.AMM_prob_dict['CT_l2'] * L_ct_l2 + \
self.AMM_prob_dict['CT_r1'] * L_ct_r1 + self.AMM_prob_dict['CT_r2'] * L_ct_r2
#print("+++++++++++++++++++++")
#print(self.AMM_prob_dict)
self.AMM_prob_dict[
'CV'] = self.AMM_prob_dict['CV'] * L_cv / prob_sum
self.AMM_prob_dict['CTA_a1'] = self.AMM_prob_dict[
'CTA_a1'] * L_cta_a1 / prob_sum
self.AMM_prob_dict['CTA_a2'] = self.AMM_prob_dict[
'CTA_a2'] * L_cta_a2 / prob_sum
self.AMM_prob_dict['CTA_d1'] = self.AMM_prob_dict[
'CTA_d1'] * L_cta_d1 / prob_sum
self.AMM_prob_dict['CTA_d2'] = self.AMM_prob_dict[
'CTA_d2'] * L_cta_d2 / prob_sum
self.AMM_prob_dict[
'CT_l1'] = self.AMM_prob_dict['CT_l1'] * L_ct_l1 / prob_sum
self.AMM_prob_dict[
'CT_l2'] = self.AMM_prob_dict['CT_l2'] * L_ct_l2 / prob_sum
self.AMM_prob_dict[
'CT_r1'] = self.AMM_prob_dict['CT_r1'] * L_ct_r1 / prob_sum
self.AMM_prob_dict[
'CT_r2'] = self.AMM_prob_dict['CT_r2'] * L_ct_r2 / prob_sum
current_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \
self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \
self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \
self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \
self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_
# print('current_x_estimation', current_x_estimation)
current_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_x_estimation - kf0.x_) * (current_x_estimation - kf0.x_).transpose()) + \
self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \
self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \
self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\
self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \
self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \
self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \
self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \
self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose())
#===================== estimation done =================== #
# now lets predict:
current_pose_xy = np.mat(
[history_trajectory_x[0], history_trajectory_y[0]])
# print('current_pose_xy',current_pose_xy)
kf0.predict('CV', self.period_of_predict)
kf1.predict('CTA_a1', self.period_of_predict)
kf2.predict('CTA_a2', self.period_of_predict)
kf3.predict('CTA_d1', self.period_of_predict)
kf4.predict('CTA_d2', self.period_of_predict)
kf5.predict('CT_l1', self.period_of_predict)
kf6.predict('CT_l2', self.period_of_predict)
kf7.predict('CT_r1', self.period_of_predict)
kf8.predict('CT_r2', self.period_of_predict)
prob_sum_p = self.AMM_prob_dict['CV'] + \
self.AMM_prob_dict['CTA_a1'] + self.AMM_prob_dict['CTA_a2'] + \
self.AMM_prob_dict['CTA_d1'] + self.AMM_prob_dict['CTA_d2'] + \
self.AMM_prob_dict['CT_l1'] + self.AMM_prob_dict['CT_l2'] + \
self.AMM_prob_dict['CT_r1'] + self.AMM_prob_dict['CT_r2'] + 0.00000000000001
self.AMM_prob_dict['CV'] = self.AMM_prob_dict['CV'] / prob_sum_p
self.AMM_prob_dict[
'CTA_a1'] = self.AMM_prob_dict['CTA_a1'] / prob_sum_p
self.AMM_prob_dict[
'CTA_a2'] = self.AMM_prob_dict['CTA_a2'] / prob_sum_p
self.AMM_prob_dict[
'CTA_d1'] = self.AMM_prob_dict['CTA_d1'] / prob_sum_p
self.AMM_prob_dict[
'CTA_d2'] = self.AMM_prob_dict['CTA_d2'] / prob_sum_p
self.AMM_prob_dict[
'CT_l1'] = self.AMM_prob_dict['CT_l1'] / prob_sum_p
self.AMM_prob_dict[
'CT_l2'] = self.AMM_prob_dict['CT_l2'] / prob_sum_p
self.AMM_prob_dict[
'CT_r1'] = self.AMM_prob_dict['CT_r1'] / prob_sum_p
self.AMM_prob_dict[
'CT_r2'] = self.AMM_prob_dict['CT_r2'] / prob_sum_p
predict_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \
self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \
self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \
self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \
self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_
predict_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_pose_xy - kf0.x_) * (current_pose_xy - kf0.x_).transpose()) + \
self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \
self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \
self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\
self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \
self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \
self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \
self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \
self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose())
self.predict_dict[track_id] = [
predict_x_estimation, predict_P_estimation
]
def main():
R_values = [0.001, 0.1, 1, 10, 1000]
Q_values = [0.001, 0.1, 1, 10, 1000]
parser = argparse.ArgumentParser(
description="Run SSD on input folder and show result in popup window")
parser.add_argument("object_detector", choices=['ssd', 'yolo_full', 'yolo_tiny'],
help="Specify which object detector network should be used")
parser.add_argument("test", choices=['Q', 'R'],
help="Which noise matrix should be tested")
args = parser.parse_args()
if args.object_detector == 'ssd':
fd = SSDObjectDetection(frozen, pbtxt)
elif args.object_detector == 'yolo_full':
fd = YOLOObjectDetection('full')
elif args.object_detector == 'yolo_tiny':
fd = YOLOObjectDetection('tiny')
# Config
should_plot = False
# Get images list from dataset
dataset = "data/TinyTLP/"
for path, directories, files in os.walk(dataset):
all_directories = directories
break
results = {}
if args.test == 'R':
k = 2
Q_value = 1
for l, R_value in enumerate(R_values):
for dir in all_directories:
results[dir] = []
ground_truth_file = dataset + dir + "/groundtruth_rect.txt"
images_wildcard = dataset + dir + "/img/*.jpg"
images_filelist = glob(images_wildcard)
# Sort them in ascending order
# images_filelist = sorted(images_filelist, key=lambda xx: int(
# xx.split('/')[-1].split('.')[0]))
# Extract all ground truths
ground_truth = list(csv.reader(open(ground_truth_file)))
gt_measurements = []
for row in ground_truth:
gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)]))
initial_position = ground_truth[0]
frame_id, x, y, w, h, is_lost = initial_position
x = int(x)
y = int(y)
w = int(w)
h = int(h)
kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value)
# Iterate of every image
features = {}
t = tqdm(images_filelist[1:], desc="Processing")
da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1)
plt.ion()
for i, im in enumerate(t):
img = plt.imread(images_filelist[i])
height, width, _ = img.shape
# Compute features
features[i] = np.array(fd.compute_features(img))
# Do prediction
mu_bar, Sigma_bar = kf.predict()
# Do data association
da.update_prediction(mu_bar, Sigma_bar)
m = da.associate(features[i])
kf.update(m)
gt = list(map(int, ground_truth[i]))
kf_x = kf.get_x()
if should_plot:
x, y = np.mgrid[0:width, 0:height]
pos = np.empty(x.shape + (2,))
pos[:, :, 0] = x
pos[:, :, 1] = y
rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]])
f = rv.pdf(pos)
f[f < 1e-5] = np.nan
plt.gca()
plt.cla()
plt.imshow(img)
plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5)
if m is not None:
plt.plot(m[0], m[1], marker='o', color='yellow')
plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red')
plt.pause(0.0001)
print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}")
results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2]))
print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}")
with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp:
pickle.dump(results, fp)
elif args.test == 'Q':
l = 2
R_value = 1
for k, Q_value in enumerate(Q_values):
for dir in all_directories:
results[dir] = []
ground_truth_file = dataset + dir + "/groundtruth_rect.txt"
images_wildcard = dataset + dir + "/img/*.jpg"
images_filelist = glob(images_wildcard)
# Sort them in ascending order
# images_filelist = sorted(images_filelist, key=lambda xx: int(
# xx.split('/')[-1].split('.')[0]))
# Extract all ground truths
ground_truth = list(csv.reader(open(ground_truth_file)))
gt_measurements = []
for row in ground_truth:
gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)]))
initial_position = ground_truth[0]
frame_id, x, y, w, h, is_lost = initial_position
x = int(x)
y = int(y)
w = int(w)
h = int(h)
kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value)
# Iterate of every image
features = {}
t = tqdm(images_filelist[1:], desc="Processing")
da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1)
plt.ion()
for i, im in enumerate(t):
img = plt.imread(images_filelist[i])
height, width, _ = img.shape
# Compute features
features[i] = np.array(fd.compute_features(img))
# Do prediction
mu_bar, Sigma_bar = kf.predict()
# Do data association
da.update_prediction(mu_bar, Sigma_bar)
m = da.associate(features[i])
kf.update(m)
gt = list(map(int, ground_truth[i]))
kf_x = kf.get_x()
if should_plot:
x, y = np.mgrid[0:width, 0:height]
pos = np.empty(x.shape + (2,))
pos[:, :, 0] = x
pos[:, :, 1] = y
rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]])
f = rv.pdf(pos)
f[f < 1e-5] = np.nan
plt.gca()
plt.cla()
plt.imshow(img)
plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5)
if m is not None:
plt.plot(m[0], m[1], marker='o', color='yellow')
plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red')
plt.pause(0.0001)
print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}")
results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2]))
print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}")
with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp:
pickle.dump(results, fp)
# While testing, don't run on too many frames
iteration = 0
while is_frame_good and iteration <= 200:
# Read a frame
is_frame_good, frame = video.read()
if not is_frame_good:
break
# To generate static image directory for use with StaticCapture
# cv2.imwrite(f'outimgs/out{iteration}.jpg', frame)
# iteration += 1
# continue
# Compute Sift predicted
bounding_box = sift.ProcessImg(frame, bounding_box)
# Apply Kalman Filter
bounding_box = kf.update(bounding_box)
# Convert to int
bounding_box = bounding_box.astype(np.int64)
# Draw bounding box
frame = DrawRectangle(frame, bounding_box)
# Show image
cv2.imshow('Frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
iteration += 1
yg += d_yg * 1000
if img_id < 2:
angle = np.arctan2(xg, yg)
if np.rad2deg(angle) < 0:
sign = -1
else:
sign = 1
continue
prev_lat = lat
prev_lon = lon
xg_rot, yg_rot = rotate(xg, yg, -(np.pi / 2 - angle))
kalman_y.update(xg_rot)
kalman_x.update(yg_rot * sign)
trajectory.append([
kalman_x.x, kalman_y.x, -model.yc, model.xc, yg_rot * sign, xg_rot, x,
z, xg, yg
])
draw_xg, draw_yg = int(xg_rot + 90), int(yg_rot * sign + 290)
draw_x, draw_y = int(x) + 290, int(z) + 90
draw_xb, draw_yb = -int(model.yc) + 290, int(model.xc) + 90
draw_kalman_x, draw_kalman_y = int(kalman_x.x) + 290, int(kalman_y.x) + 90
#print(kalman_y.P, kalman_x.P)
print(sign)
#cv2.circle(traj, (draw_x, draw_y), 1, (255, 0, 0), 1)
# init
x = np.array([0, 0])
P = np.array([[0, 0], [0, 0]])
kf = KalmanFilter(F, H, Q, R)
cart = Cart()
vs = [0] # filterd v
ws = [0] # true v
xs = [0] # filterd x
ys = [0] # true x
zs = [0] # observed x
for _ in range(100):
y, z, w = cart()
x, P = kf.predict(x, P)
x, P = kf.update(x, P, z)
vs.append(x[1])
ws.append(w)
xs.append(x[0])
ys.append(y)
zs.append(z)
# plot graph
figs, axes = plt.subplots(1, 2)
axes[1].plot(vs, label='filterd v', marker='.')
axes[1].plot(ws, label='true v', marker='x')
axes[0].plot(xs, label='filterd x', marker='.')
axes[0].plot(ys, label='true x', marker='x')
axes[0].plot(zs, label='observed x', marker='+')
axes[0].set_ylabel('x')
# plt.draw()
# plt.pause(0.1)
# input()
kf = KalmanFilter(dim=3, x=mu, P=sig, R=measurement_sig, Q=motion_sig)
## TODO: Loop through all measurements/motions
# this code assumes measurements and motions have the same length
# so their updates can be performed in pairs
for n in range(measurements.shape[0]):
# motion update, with uncertainty
mu, sig = kf.predict(Q=motion_sig)
print('Predict: {} \n{}\n\n'.format(mu, sig))
# measurement update, with uncertainty
mu, sig = kf.update(y=measurements[n], R=measurement_sig)
print('Update: {} \n{}\n\n'.format(mu, sig))
# print (mu)
measurement_sig -= np.eye(
3
) * 0.25 # Assumes noise reduces; keep this constant by commenting out this line if needed.
if PLOT:
plt.cla()
ax.axvline(c='grey', lw=1)
ax.axhline(c='grey', lw=1)
# g = []
# for x in x_axis:
# g.append(f(mu, sig, x).flatten())
# ax.plot(np.linspace(-1,1,10),np.linspace(-1,1,10))
for labeled_object in tracked_objects[track_id]:
z = labeled_object.bbox
print("z:")
print(z)
top_left_x = float(z[0])
top_left_y = float(z[1])
bottom_right_x = float(z[2])
bottom_right_y = float(z[3])
centre_x = (top_left_x + bottom_right_x) / 2
centre_y = (top_left_y + bottom_right_y) / 2
measurements = np.zeros((2, 1))
measurements[0] = centre_x
measurements[1] = centre_y
print("measurements:")
print(z)
x, P = filter.predict()
gt_points_x.append(measurements[0])
gt_points_y.append(measurements[1])
kalman_points_x.append(x[0])
kalman_points_y.append(x[1])
#print("x:")
print(labeled_object.bbox)
filter.update(measurements)
plt.scatter(gt_points_x, gt_points_y)
plt.scatter(kalman_points_x, kalman_points_y)
plt.show()
break
