def update(self,measurement):
'''
The update takes in account any anomaly detector if one of the model has
a probability above 0.8. This feature allows the anomaly to not be taken
in consideration when the system is doing a maneuver. Switching model is
not breaking the estimation that way.
Parameters
----------
measurement: float numpy array
The measurement added to the update cycle of the filter.
'''
if max(self.mu)>0.8:
for filter in self.filters:
filter.activate_detection()
IMMEstimator.update(self,measurement)
def test_misshapen():
"""Ensure we get a ValueError if the filter banks are not designed
properly
"""
ca = KalmanFilter(3, 1)
cv = KalmanFilter(2, 1)
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
try:
IMMEstimator([ca, cv], (0.5, 0.5), trans)
assert "IMM should raise ValueError on filter banks with filters of different sizes"
except ValueError:
pass
try:
IMMEstimator([], (0.5, 0.5), trans)
assert "Should raise ValueError on empty bank"
except ValueError:
pass
def __init__(self, pfa=1e-6, sn0=50, beamwidth=0.02, n_max=20, x0=np.array([30e3, -150, 0, 30e3, 150, 0]),
theta_accuracy=0.002):
# Trackers
probs = np.ones(2) / 2
p_switch = 1.0 / 1000.0
M = np.array([
[1 - p_switch, p_switch],
[p_switch, 1 - p_switch]])
F_ca = kinematic_state_transition(self.DT, self.ORDER, self.DIM)
F_cv = kinematic_state_transition(self.DT, self.ORDER, self.DIM)
F_cv[:, 2::3] = 0
g = 9.81
var = 4 * g ** 2
Q = filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var, block_size=self.ORDER + 1)
kf_cv = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM)
kf_cv.F = F_cv
kf_cv.Q = Q
kf_cv.H = position_measurement_matrix(self.DIM, self.ORDER)
kf_ca = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM)
kf_ca.F = F_ca
kf_ca.Q = Q
kf_ca.H = position_measurement_matrix(self.DIM, self.ORDER)
filters = [kf_cv, kf_ca]
tracker = IMMEstimator(filters, probs, M)
# Target
st_models = {
0: F_cv,
1000: F_ca,
2000: F_cv
}
p_noises = {
0: Q
}
target = DefinedTargetProcess(x0, st_models, p_noises, self.MAX_STEPS, self.ORDER, self.DIM)
# Radar
radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM, self.ORDER, angle_accuracy=theta_accuracy)
# Computer
P0 = np.zeros((x0.size,) * 2)
computer = TrackingComputer(tracker, radar, n_max, P0=P0)
super().__init__(computer)
def __init__(self, var_cv=737.5, var_ca=73.7, p_switch=0.028, traj_idx=0):
F_ca = kinematic_state_transition(self.DT, self.ORDER, self.DIM)
F_cv = F_ca.copy()
F_cv[:, 2::3] = 0
G = np.array([[1 / 2 * self.DT**2, self.DT, 1]]).T
A = G @ G.T
Q_ca = block_diag(A, A) * var_ca
Q_cv = block_diag(A, A) * var_cv
Q_cv[:, 2::3] = 0
Q_cv[2::3, :] = 0
kf_ca = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM)
kf_ca.F = F_ca
kf_ca.Q = Q_ca
kf_ca.H = position_measurement_matrix(self.DIM, self.ORDER)
kf_cv = KalmanFilter(dim_x=self.DIM * (self.ORDER + 1), dim_z=self.DIM)
kf_cv.F = F_cv
kf_cv.Q = Q_cv
kf_cv.H = position_measurement_matrix(self.DIM, self.ORDER)
filters = [kf_cv, kf_ca]
mu = [0.5, 0.5]
M = np.array([[1 - p_switch, p_switch], [p_switch, 1 - p_switch]])
tracker = IMMEstimator(filters, mu, M)
target = load_benchmark_target(traj_idx,
self.ORDER,
self.DIM,
skip_k=10)
sn0 = 50.0
pfa = 1e-6
beamwidth = 0.02
radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM,
self.ORDER)
n_max = 20
P0 = np.zeros((6, 6), dtype=float)
target.reset()
x0 = target.x
computer = TrackingComputer(tracker, radar, n_max, x0=x0, P0=P0)
super().__init__(computer=computer)
def __init__(self, pfa=1e-6, sn0=50, beamwidth=0.02, n_max=20, x0=np.array([10e3, -200.0, 10e3, 0]),
P0=np.eye(4) * 1000, theta_accuracy=0.002):
var = 0.1*9.81**2
tr1 = -0.08
tr2 = 0.1
probs = np.ones(3) / 3
p_switch = 1.0 / 100.0
M = np.array([
[1 - p_switch, p_switch / 2, p_switch / 2],
[p_switch / 2, 1 - p_switch, p_switch / 2],
[p_switch / 2, p_switch / 2, 1 - p_switch]
])
filters = list()
for i in range(3):
filters.append(filterpy.common.kinematic_kf(self.DIM, self.ORDER, self.DT))
filters[i].x = x0
filters[i].Q = filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var=var, block_size=self.ORDER+1)
filters[1].F = constant_turn_rate_matrix(tr1, self.DT)
filters[2].F = constant_turn_rate_matrix(tr2, self.DT)
st_models = {
0: kinematic_state_transition(self.DT, self.ORDER, self.DIM),
200: constant_turn_rate_matrix(tr1, self.DT),
300: kinematic_state_transition(self.DT, self.ORDER, self.DIM),
400: constant_turn_rate_matrix(tr2, self.DT),
500: kinematic_state_transition(self.DT, self.ORDER, self.DIM)
}
p_noises = {
0: filterpy.common.Q_discrete_white_noise(self.DIM, self.DT, var=var, block_size=self.ORDER + 1)
}
target = DefinedTargetProcess(x0, st_models, p_noises, self.MAX_STEPS, self.ORDER, self.DIM)
tracker = IMMEstimator(filters, probs, M)
radar = PositionRadar(target, sn0, pfa, beamwidth, self.DIM, self.ORDER, angle_accuracy=theta_accuracy)
computer = TrackingComputer(tracker, radar, n_max, P0=P0)
super().__init__(computer)
def gen_env():
trajectory = Track()
states = trajectory.gen_landing()
x0 = states[0, 0]
y0 = states[0, 3]
z0 = states[0, 6]
radar = Radar(x=0, y=2000)
radar_filter_cv = RadarFilterCV(dim_x=9,
dim_z=3,
q=1.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
radar_filter_ca = RadarFilterCA(dim_x=9,
dim_z=3,
q=400.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
radar_filter_ct = RadarFilterCT(dim_x=9,
dim_z=3,
q=350.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
filters = [radar_filter_cv, radar_filter_ca, radar_filter_ct]
mu = [0.33, 0.33, 0.33]
trans = np.array([[0.998, 0.001, 0.001], [0.050, 0.900, 0.050],
[0.001, 0.001, 0.998]])
imm = IMMEstimator(filters, mu, trans)
benchmark_imm3 = Benchmark(radars=radar, radar_filter=imm, states=states)
benchmark_imm3.launch_benchmark(with_nees=True)
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 = 1.
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)
#x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',')
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)
xs, probs = [], []
cvxs, caxs = [], []
s = Saver(bank)
for i, z in enumerate(zs[0:10]):
z = np.array([z]).T
bank.update(z)
#print(ca.likelihood, cano.likelihood)
#print(ca.x.T)
xs.append(bank.x.copy())
cvxs.append(ca.x.copy())
caxs.append(cano.x.copy())
#print(i, ca.likelihood, cano.likelihood, bank.w)
#print('p', bank.p)
probs.append(bank.mu.copy())
s.save()
s.to_array()
if DO_PLOT:
xs = np.array(xs)
cvxs = np.array(cvxs)
caxs = np.array(caxs)
probs = np.array(probs)
plt.subplot(121)
plt.plot(xs[:, 0], xs[:, 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(probs[:, 0])
plt.plot(probs[:, 1])
plt.ylim(-1.5, 1.5)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
plt.plot(cvxs, label='CV')
plt.plot(caxs, label='CA')
plt.plot(xs[:, 0], label='GT')
plt.legend()
plt.figure()
plt.plot(xs)
plt.plot(xs[:, 0])'''
return bank
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.
"""
dt = 0.1
pos, zs = generate_data(120, noise_factor=0.6)
z_xs = zs[:, 0]
t = np.arange(0, len(z_xs) * dt, dt)
dt = 0.1
ca = make_ca_filter(dt, noise_factor=0.6)
cv = make_ca_filter(dt, noise_factor=0.6)
cv.F[:,2] = 0 # remove acceleration term
cv.P[2,2] = 0
cv.Q[2,2] = 0
filters = [cv, ca]
trans = np.array([[0.97, 0.03],
[0.03, 0.97]])
trans = np.array([[0.8, 0.2],
[0.05, 0.95]])
bank = IMMEstimator(filters, (0.5, 0.5), trans, dim_x=3)
xs, probs = [], []
cvxs, caxs = [], []
for i, z in enumerate(z_xs):
bank.update(z)
xs.append(bank.x[0])
cvxs.append(cv.x[0])
caxs.append(ca.x[0])
#print(i, cv.likelihood, ca.likelihood, bank.w)
#print('p', bank.p)
probs.append(bank.w[0] / bank.w[1])
if DO_PLOT:
plt.subplot(121)
plt.plot(xs)
plt.plot(pos[:, 0])
plt.subplot(122)
plt.plot(probs)
plt.title('probability ratio p(cv)/p(ca)')
plt.figure()
plt.plot(cvxs, label='CV')
plt.plot(caxs, label='CA')
plt.plot(pos[:, 0], label='GT')
plt.legend()
plt.figure()
plt.plot(xs)
plt.plot(pos[:, 0])
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.8, 0.2],
[0.05, 0.95]])
trans = np.array([[0.97, 0.03],
[0.03, 0.97]])
bank = IMMEstimator((6, 1), filters, (0.5, 0.5), trans)
xs, probs = [], []
cvxs, caxs = [], []
for i, z in enumerate(zs):
print("\ni=", i+1)
if i == 10000:
break
#print(z)
z = np.array([z]).T
bank.update(z)
#print(ca.likelihood, cano.likelihood)
#print(ca.x.T)
xs.append(bank.x.copy())
cvxs.append(ca.x.copy())
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 = 1.
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)
#x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',')
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)
xs, probs = [], []
cvxs, caxs = [], []
for i, z in enumerate(zs[0:10]):
z = np.array([z]).T
bank.update(z)
#print(ca.likelihood, cano.likelihood)
#print(ca.x.T)
xs.append(bank.x.copy())
cvxs.append(ca.x.copy())
caxs.append(cano.x.copy())
#print(i, ca.likelihood, cano.likelihood, bank.w)
#print('p', bank.p)
probs.append(bank.mu.copy())
radars = [radar1, radar2]
radar_filter_cv = MultipleRadarsFilterCV(dim_x=9,
dim_z=6,
q=1.,
radars=radars,
x0=x0,
y0=y0,
z0=z0)
radar_filter_ca = MultipleRadarsFilterCA(dim_x=9,
dim_z=6,
q=400.,
radars=radars,
x0=x0,
y0=y0,
z0=z0)
radar_filter_ct = MultipleRadarsFilterCT(dim_x=9,
dim_z=6,
q=350.,
radars=radars,
x0=x0,
y0=y0,
z0=z0)
filters = [radar_filter_cv, radar_filter_ca, radar_filter_ct]
mu = [0.33, 0.33, 0.33]
trans = np.array([[0.998, 0.001, 0.001], [0.050, 0.900, 0.050],
[0.001, 0.001, 0.998]])
imm = IMMEstimator(filters, mu, trans)
benchmark_imm3 = Benchmark(radars=radars, radar_filter=imm, states=states)
benchmark_imm3.launch_benchmark(with_nees=True)
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 = 1.
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)
#x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',')
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)
ca.F = np.array(
[[1, dt, dt**2/2, 0, 0, 0],
[0, 1, dt, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, dt, dt**2/2],
[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, .16666666666666666666666666667],
[.125, .333333333333333333333333333333333333, .5],
[.166666666666666666666666667, .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.8, 0.2],
[0.05, 0.95]])
trans = np.array([[0.97, 0.03],
[0.03, 0.97]])
bank = IMMEstimator(filters, (0.5, 0.5), trans)
xs, probs = [], []
cvxs, caxs = [], []
for i, z in enumerate(zs):
z = np.array([z]).T
bank.update(z)
#print(ca.likelihood, cano.likelihood)
#print(ca.x.T)
xs.append(bank.x.copy())
cvxs.append(ca.x.copy())
caxs.append(cano.x.copy())
#print(i, ca.likelihood, cano.likelihood, bank.w)
#print('p', bank.p)
probs.append(bank.mu.copy())
if DO_PLOT:
xs = np.array(xs)
cvxs = np.array(cvxs)
caxs = np.array(caxs)
probs = np.array(probs)
plt.subplot(121)
plt.plot(xs[:, 0], xs[:, 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(probs[:, 0])
plt.plot(probs[:, 1])
plt.ylim(-1.5, 1.5)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
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()
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)
from filterpy.common import kinematic_kf
from filterpy.kalman import ExtendedKalmanFilter
import cv2
import time
import filterpy as fp
from filterpy.kalman import IMMEstimator
np.set_printoptions(suppress=True)
order = 1
kf1 = kinematic_kf(dim=2, order=order)
kf2 = ExtendedKalmanFilter(4, 2)
# do some settings of x, R, P etc. here, I'll just use the defaults
kf2.Q *= 0 # no prediction error in second filter
filters = [kf1, kf2]
mu = [0.5, 0.5] # each filter is equally likely at the start
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
imm = IMMEstimator(filters, mu, trans)
bg_image = cv2.imread('bg.png') # 500,1000
cv2.imshow('KalmanFilterDemo', bg_image)
cv2.moveWindow("KalmanFilterDemo", 0, 0)
window_ul = (8, 30) # x,y
boxhsize = (20, 10)
lastul = (0, 0)
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()
