def demo():
img = np.zeros((768, 1024, 3), np.uint8)
trajs = deque(maxlen=200)
trajs_filterd = deque(maxlen=200)
def draw(event, x, y, flags, param):
kf, fm, fo = param
if event == cv2.EVENT_MOUSEMOVE:
fm[:], fo[:] = kf.filter_update(fm, fo, np.array([x, y]))
fx = int(fm[0])
fy = int(fm[1])
trajs.append((x, y))
trajs_filterd.append((fx, fy))
cv2.namedWindow("image")
kf = KalmanFilter(n_dim_state=4, n_dim_obs=2)
kf.transition_matrices = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.observation_matrices = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
kf.transition_covariance = 1e-3 * np.eye(4)
kf.observation_covariance = 10 * np.eye(2)
kf.initial_state_mean = np.array([0, 0, 0, 0])
kf.initial_state_covariance = np.eye(4)
fm = kf.initial_state_mean
fo = kf.initial_state_covariance
cv2.setMouseCallback("image", draw, (kf, fm, fo))
while True:
img = np.zeros((768, 1024, 3), np.uint8)
for i in range(len(trajs) - 1):
cv2.line(img, trajs[i], trajs[i + 1], (0, 255, 0), 1)
cv2.line(img, trajs_filterd[i], trajs_filterd[i + 1], (0, 0, 255), 1)
map(lambda pt: cv2.circle(img, pt, 3, (0, 255, 0), 1), trajs)
map(lambda pt: cv2.circle(img, pt, 3, (0, 0, 255), 1), trajs_filterd)
cv2.imshow("image", img)
if cv2.waitKey(5) & 0xFF == ord("q"):
return
for cnum, cpos in posDF.groupby("id"):
if len(cpos) == 1:
continue
ft = np.arange(cpos["frame"].values[0], cpos["frame"].values[-1] + 1)
# obs = np.vstack((cpos['x'].values, cpos['y'].values)).T
obs = np.zeros((len(ft), 2))
obs = np.ma.array(obs, mask=np.zeros_like(obs))
for f in range(len(ft)):
if len(cpos[cpos["frame"] == ft[f]].x.values) > 0:
obs[f][0] = cpos[cpos["frame"] == ft[f]].x.values[0] * px_to_m
obs[f][1] = cpos[cpos["frame"] == ft[f]].y.values[0] * px_to_m
else:
obs[f] = np.ma.masked
kf.initial_state_mean = [cpos["x"].values[0] * px_to_m, cpos["y"].values[0] * px_to_m, 0, 0, 0, 0]
sse = kf.smooth(obs)[0]
ani = cpos["animal"].values[0]
xSmooth = sse[:, 0]
ySmooth = sse[:, 1]
xv = sse[:, 2] / 0.1
yv = sse[:, 3] / 0.1
xa = sse[:, 4] / 0.01
ya = sse[:, 5] / 0.01
headings = np.zeros_like(xSmooth)
dx = np.zeros_like(xSmooth)
dy = np.zeros_like(xSmooth)
for i in range(len(headings)):
start = max(0, i - 5)
def main(argv):
config = read_parser(argv, Inputs, InputsOpt_Defaults)
if config['mode'] == 'normal':
print('nothing')
n = 500
x_pure = np.arange(n) * 0.1
x = x_pure + np.random.normal(scale=0.25, size=n)
y = np.cos(x)
# y = 2*x+ 6
kf = KalmanFilter(initial_state_mean=0, n_dim_obs=1, random_state=1)
# measurements = [[x[i], y[i]] for i in range(int(n))]
measurements = [[y[i]] for i in range(int(n / 2))]
# measurements_full = [[y[i]] for i in range(int(n))]
kf.em(measurements)
# print(kf.initial_state_mean)
# print(kf.initial_state_covariance)
# print(type(kf.initial_state_mean))
# print(type(kf.initial_state_covariance))
# exit()
z = kf.filter(measurements)
means = z[0]
covs = z[1]
it_mean = means[len(means) - 1]
it_cov = covs[len(covs) - 1]
h = list(means)
r = h
# kf2 = KalmanFilter(initial_state_mean=it_mean, initial_state_covariance=it_cov, n_dim_obs=1)
# kf2.em(measurements)
f = list(
kf.sample(int(n / 2), initial_state=it_mean, random_state=1)[0])
# f = h + list(np.array(f) + it_mean)
f = h + f
for i in range(int(n / 2)):
it_z = kf.filter_update(filtered_state_mean=it_mean,
filtered_state_covariance=it_cov,
observation=[y[i + int(n / 2)]])
it_mean = it_z[0]
it_cov = it_z[1]
h.append(it_mean)
plt.plot(x_pure, h, 'r')
plt.plot(x_pure, y)
plt.plot(x_pure, f, 'g')
plt.show()
elif config['mode'] == 'ukalman':
print('Ukalman')
n = 1000
x_pure = np.arange(n) * 0.1
x = x_pure + np.random.normal(scale=0.5, size=n)
y = np.cos(x)
# y = 2*x
kf = AdditiveUnscentedKalmanFilter(n_dim_obs=1)
measurements = [[y[i]] for i in range(int(n / 2))]
kf.initial_state_mean = np.array([0.])
kf.initial_state_covariance = np.array([[0.1]])
it_mean = kf.initial_state_mean
it_cov = kf.initial_state_covariance
print(kf.initial_state_mean)
print(kf.initial_state_covariance)
# exit()
h = []
for element in measurements:
it_z = kf.filter_update(filtered_state_mean=it_mean,
filtered_state_covariance=it_cov,
observation=element)
it_mean = it_z[0]
it_cov = it_z[1]
h.append(it_mean)
f = list(kf.sample(int(n / 2), initial_state=it_mean)[0])
# f = h + list(np.array(f) + it_mean)
f = h + f
plt.plot(x_pure, y)
plt.plot(x_pure, f, 'g')
plt.show()
return
#yobs = np.ma.array(ymes, mask=np.zeros_like(ymes))
#xobs = np.ma.empty_like(ft)
#yobs = np.ma.empty_like(ft)
for f in range(len(ft)):
if len(cpos[cpos['frame']==ft[f]].x.values)>0:
obs[f][0]=cpos[cpos['frame']==ft[f]].x.values[0]*px_to_m
obs[f][1]=cpos[cpos['frame']==ft[f]].y.values[0]*px_to_m
else:
obs[f]=np.ma.masked
#yobs[f]=np.ma.masked
#if cnum==49:
# break
#obs = np.vstack((cpos['x'].values*px_to_m, cpos['y'].values*px_to_m)).T
#obs=np.vstack((xobs,yobs)).T
#obs = np.vstack((cpos['x'].values, cpos['y'].values)).T
kf.initial_state_mean=[cpos['x'].values[0]*px_to_m,cpos['y'].values[0]*px_to_m,0,0,0,0]
#kf.initial_state_mean=[cpos['x'].values[0],cpos['y'].values[0],0,0,0,0]
sse = kf.smooth(obs)[0]
xSmooth = sse[:,0]
ySmooth = sse[:,1]
xv = sse[:,2]/0.1
yv = sse[:,3]/0.1
xa = sse[:,4]/0.01
ya = sse[:,5]/0.01
dx = np.zeros_like(xSmooth)
dy = np.zeros_like(xSmooth)
headings = np.zeros_like(xSmooth)
# calculate change in position for 5 second intervals
