class Tracks(object):
"""docstring for Tracks"""
def __init__(self, detection, trackId):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict()
self.KF.correct(np.matrix(detection).reshape(2, 1))
self.trace = deque(maxlen=20)
self.prediction = detection.reshape(1, 2)
self.trackId = trackId
self.skipped_frames = 0
def predict(self, detection):
self.prediction = np.array(self.KF.predict()).reshape(1, 2)
self.KF.correct(np.matrix(detection).reshape(2, 1))
class TrackingObject(object): """ TrackingObject: maintains and updates all the information for one tracked object """ def __init__(self, detection, trackId): super(TrackingObject, self).__init__() self.KF = KalmanFilter() self.KF.predict() self.KF.correct(np.matrix(detection).reshape(2,1)) self.trace = deque(maxlen=20) self.prediction = detection.reshape(1,2) self.trackId = trackId self.skipped_frames = 0 def predict_and_correct(self, measurement): self.prediction = np.array(self.KF.predict()).reshape(1,2) self.KF.correct(np.matrix(measurement).reshape(2,1))
x = est_rbt_x[:,
est_k] # last state estimate vector from robot frame
P = est_rbt_P[:, :, est_k] # last covariance matrix
Q = est_rbt_Q # process noise covariance matrix
R = est_rbt_R # measurement noise covariance
H = est_rbt_H # observation matrix
z = est_rbt_m[:, est_k] # measurement vector
u = matrix(
[[0], [0], [0]]
) # control input vector (don't give kalman filter knowledge about thruster inputs)
A = est_rbt_A
B = est_rbt_B
state = KalmanFilter(x, P, z, u, A, B, Q, R, H)
x, P = state.predict(x, P, u)
x, K, P = state.update(x, P, z)
# print('x', x)
# # print('P', P)
# # print('Q', Q)
# # print('R', R)
# # print('H', H)
# # print('z', z)
# # print('u', u)
# # print('K', K)
est_rbt_x[:, est_k + 1] = x
est_rbt_L[:, :, est_k + 1] = K
est_rbt_P[:, :, est_k + 1] = P
no_of_state_measurements=no_of_state_measurements)
step = 0 # step counter, used for simulation of abscent measurements only
# Loop that runs throughout the time array defined previously
for t in time:
#Simulate real speed
real_speed_x = initial_speed_x + np.random.randn() * np.sqrt(
acceleration_variance)
real_speed_y = initial_speed_y + np.random.randn() * np.sqrt(
acceleration_variance)
# Simulate real position
real_pos_x = real_pos_x + dt * real_speed_x
real_pos_y = real_pos_y + dt * real_speed_y
# Simulate measurements with noise
meas_value_x = real_pos_x + np.random.randn() * np.sqrt(meas_variance)
meas_value_y = real_pos_y + np.random.randn() * np.sqrt(meas_variance)
# Simulation of abscent measurements
if step > 70 and step < 110:
meas_value_x, meas_value_y = 0, 0
# Assign obtained measurements to measurement array
z = np.array([meas_value_x, meas_value_y])
kf.predict(dt=dt)
kf.update(state_measurement=z)
step += 1 # increment step
