def test_imm():
""" This test is drawn from Crassidis [1], example 4.6.
** References**
[1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press,
Second edition.
"""
r = 100.
dt = 1.
phi_sim = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
gam = np.array([[dt**2 / 2, 0], [dt, 0], [0, dt**2 / 2], [0, dt]])
x = np.array([[2000, 0, 10000, -15.]]).T
simxs = []
N = 600
for i in range(N):
x = np.dot(phi_sim, x)
if i >= 400:
x += np.dot(gam, np.array([[.075, .075]]).T)
simxs.append(x)
simxs = np.array(simxs)
zs = np.zeros((N, 2))
for i in range(len(zs)):
zs[i, 0] = simxs[i, 0] + randn() * r
zs[i, 1] = simxs[i, 2] + randn() * r
'''
try:
# data to test against crassidis' IMM matlab code
zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1]
zs = zs_tmp
except:
pass
'''
ca = KalmanFilter(6, 2)
cano = KalmanFilter(6, 2)
dt2 = (dt**2) / 2
ca.F = np.array([[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0],
[0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2],
[0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]])
cano.F = ca.F.copy()
ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T
cano.x = ca.x.copy()
ca.P *= 1.e-12
cano.P *= 1.e-12
ca.R *= r**2
cano.R *= r**2
cano.Q *= 0
q = np.array([[.05, .125, 1. / 6], [.125, 1 / 3, .5], [1. / 6, .5, 1.]
]) * 1.e-3
ca.Q[0:3, 0:3] = q
ca.Q[3:6, 3:6] = q
ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
cano.H = ca.H.copy()
filters = [ca, cano]
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
bank = IMMEstimator(filters, (0.5, 0.5), trans)
# ensure __repr__ doesn't have problems
str(bank)
s = Saver(bank)
ca_s = Saver(ca)
cano_s = Saver(cano)
for i, z in enumerate(zs):
z = np.array([z]).T
bank.update(z)
bank.predict()
s.save()
ca_s.save()
cano_s.save()
if DO_PLOT:
s.to_array()
ca_s.to_array()
cano_s.to_array()
plt.figure()
plt.subplot(121)
plt.plot(s.x[:, 0], s.x[:, 3], 'k')
#plt.plot(cvxs[:, 0], caxs[:, 3])
#plt.plot(simxs[:, 0], simxs[:, 2], 'g')
plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2)
plt.subplot(122)
plt.plot(s.mu[:, 0])
plt.plot(s.mu[:, 1])
plt.ylim(0, 1)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
class Tracker_SingleTarget_MultipleModel(object):
"""
"""
def __init__(self, deltaT, measurementNoiseStd):
self.updatedPredictions = []
self.mus = []
"""
First init constant linear model
"""
points1 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.constantLinearModel = UnscentedKalmanFilter(
dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_linearModel,
hx=h_unscented_linearModel,
points=points1)
self.constantLinearModel.x = np.array([0.01, 0.01, 0.01, 0.01, 0])
self.constantLinearModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.constantLinearModel.R = np.eye(2) * (measurementNoiseStd**2)
self.constantLinearModel.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0])
"""
Second init constant turn rate model
"""
points2 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.constantTurnRateModel = UnscentedKalmanFilter(
dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_turnRateModel,
hx=h_unscented_turnRateModel,
points=points2)
self.constantTurnRateModel.x = np.array(
[0.01, 0.01, 0.01, 0.001, 1e-5])
self.constantTurnRateModel.P = np.eye(5) * (measurementNoiseStd**
2) / 2.0
self.constantTurnRateModel.R = np.eye(2) * (measurementNoiseStd**2)
self.constantTurnRateModel.Q = np.diag(
[1e-24, 1e-24, 1e-3, 4e-3, 1e-10])
"""
Third init random motion model
"""
points3 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.randomModel = UnscentedKalmanFilter(dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_randomModel,
hx=h_unscented_randomModel,
points=points3)
self.randomModel.x = np.array([0.01, 0.01, 0.01, 0.001, 1e-5])
self.randomModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.randomModel.R = np.eye(2) * (measurementNoiseStd**2)
self.randomModel.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24])
#############################33
if (1):
filters = [self.constantLinearModel, self.constantTurnRateModel]
mu = [0.5, 0.5]
trans = np.array([[0.9, 0.1], [0.1, 0.9]])
self.imm = IMMEstimator(filters, mu, trans)
else:
filters = [
self.constantLinearModel, self.constantTurnRateModel,
self.randomModel
]
mu = [0.34, 0.33, 0.33]
trans = np.array([[0.9, 0.05, 0.05], [0.05, 0.9, 0.05],
[0.05, 0.05, 0.9]])
self.imm = IMMEstimator(filters, mu, trans)
def predictAndUpdate(self, measurement):
self.imm.P = 1 / 2.0 * (self.imm.P + self.imm.P.T)
self.imm.predict()
self.imm.update(measurement)
self.updatedPredictions.append(
np.array((self.imm.x_post[0], self.imm.x_post[1])))
self.mus.append(self.imm.mu)
def test_imm():
""" This test is drawn from Crassidis [1], example 4.6.
** References**
[1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press,
Second edition.
"""
r = 100.
dt = 1.
phi_sim = np.array(
[[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
gam = np.array([[dt**2/2, 0],
[dt, 0],
[0, dt**2/2],
[0, dt]])
x = np.array([[2000, 0, 10000, -15.]]).T
simxs = []
N = 600
for i in range(N):
x = np.dot(phi_sim, x)
if i >= 400:
x += np.dot(gam, np.array([[.075, .075]]).T)
simxs.append(x)
simxs = np.array(simxs)
zs = np.zeros((N, 2))
for i in range(len(zs)):
zs[i, 0] = simxs[i, 0] + randn()*r
zs[i, 1] = simxs[i, 2] + randn()*r
'''
try:
# data to test against crassidis' IMM matlab code
zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1]
zs = zs_tmp
except:
pass
'''
ca = KalmanFilter(6, 2)
cano = KalmanFilter(6, 2)
dt2 = (dt**2)/2
ca.F = np.array(
[[1, dt, dt2, 0, 0, 0],
[0, 1, dt, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, dt, dt2],
[0, 0, 0, 0, 1, dt],
[0, 0, 0, 0, 0, 1]])
cano.F = ca.F.copy()
ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T
cano.x = ca.x.copy()
ca.P *= 1.e-12
cano.P *= 1.e-12
ca.R *= r**2
cano.R *= r**2
cano.Q *= 0
q = np.array([[.05, .125, 1./6],
[.125, 1/3, .5],
[1./6, .5, 1.]])*1.e-3
ca.Q[0:3, 0:3] = q
ca.Q[3:6, 3:6] = q
ca.H = np.array([[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
cano.H = ca.H.copy()
filters = [ca, cano]
trans = np.array([[0.97, 0.03],
[0.03, 0.97]])
bank = IMMEstimator(filters, (0.5, 0.5), trans)
# ensure __repr__ doesn't have problems
str(bank)
s = Saver(bank)
ca_s = Saver(ca)
cano_s = Saver(cano)
for i, z in enumerate(zs):
z = np.array([z]).T
bank.update(z)
bank.predict()
s.save()
ca_s.save()
cano_s.save()
if DO_PLOT:
s.to_array()
ca_s.to_array()
cano_s.to_array()
plt.figure()
plt.subplot(121)
plt.plot(s.x[:, 0], s.x[:, 3], 'k')
#plt.plot(cvxs[:, 0], caxs[:, 3])
#plt.plot(simxs[:, 0], simxs[:, 2], 'g')
plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2)
plt.subplot(122)
plt.plot(s.mu[:, 0])
plt.plot(s.mu[:, 1])
plt.ylim(0, 1)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
lastbr = (0, 0)
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
videof = cv2.VideoWriter('video.avi', fourcc, 1, (1000, 500))
while True:
start_time = time.time() # start time of the loop
frame = bg_image.copy()
x, y = win32api.GetCursorPos()
x = x - window_ul[0]
y = y - window_ul[1]
cv2.rectangle(frame, (x - boxhsize[0], y - boxhsize[1]),
(x + boxhsize[0], y + boxhsize[1]), (0, 255, 0), 3)
z = np.array([[x], [y]])
imm.update(z)
imm.predict()
print(imm.x.T)
# if order==1:
# xp=imm.x_prior[0]
# yp=imm.x_prior[2]
# elif order==2:
# xp=imm.x_prior[0]
# yp=imm.x_prior[3]
new_vals = imm.x.T[0]
if order == 1:
xp = int(new_vals[0])
yp = int(new_vals[2])
elif order == 2:
xp = int(new_vals[0])
yp = int(new_vals[3])
