import numpy as np
from matplotlib.patches import Ellipse
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
from gif_animate import animate
import numpy.random as random
from DogSimulation import DogSimulation
import filterpy.stats
from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import KalmanFilter
def dog_tracking_filter(R,Q=0,cov=1.):
dog_filter = KalmanFilter (dim_x=2, dim_z=1)
dog_filter.x = np.array([0, 0])
dog_filter.F = np.array([[1,1],
[0,1]])
dog_filter.H = np.array([[1,0]])
dog_filter.R *= R
dog_filter.P *= cov
if np.isscalar(Q):
dog_filter.Q = Q_discrete_white_noise(2, var=Q)
else:
dog_filter.Q = Q
return dog_filter
R = 5.
Q = .01
noise = 2.
P = 20.
dog = DogSimulation(measurement_variance=R, process_variance=Q)
f = dog_tracking_filter(R=R, Q=Q, cov=P)
random.seed(200)
zs = []
xs = []
def animate_track(frame):
if frame > 30: return
plt.gca().set_xlim(-100,100)
if frame == 0:
stats.plot_covariance_ellipse ((0, f.x[0]), cov=f.P, axis_equal=True,
facecolor='g', edgecolor=None, alpha=0.2)
xs.append (f.x[0])
z = dog.move_and_sense()
zs.append(z)
f.update(z)
xs.append (f.x[0])
stats.plot_covariance_ellipse ((frame+1, f.x[0]), cov=f.P, axis_equal=True,
facecolor='g', edgecolor=None, alpha=0.5,
xlim=(-5,30), ylim=(-5,30))
plt.plot(zs, color='r', linestyle='dashed')
plt.plot(xs, color='b')
f.predict()
animate('multivariate_track1.gif', animate_track, 37, 200, figsize=(5.5, 5.5))