def ball_filter6(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=6)
g = 10
f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0],
[0, 1., dt, 0, 0, 0],
[0, 0, 1., 0, 0, 0],
[0, 0, 0., 1., dt, -0.5*dt*dt*g],
[0, 0, 0, 0, 1., -g*dt],
[0, 0, 0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,1,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0]])
f1.R = np.mat(np.eye(6)) * R
f1.Q = np.zeros((6,6))
f1.Q[2,2] = Q
f1.Q[5,5] = Q
f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
f1.P = np.eye(6) * 50.
f1.B = 0.
f1.u = 0
return f1
def test_procedural_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0])
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.eye(2) * 1000.
f.R = np.eye(1) * 5
f.Q = Q_discrete_white_noise(2, 1., 0.0001)
f.test_matrix_dimensions()
x = np.array([2., 0])
F = np.array([[1., 1.], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2) * 1000.
R = np.eye(1) * 5
Q = Q_discrete_white_noise(2, 1., 0.0001)
zs = [13., None, 1., 2.] * 10
m, c, _, _ = f.batch_filter(zs, update_first=False)
n = len(zs)
mp, cp, _, _ = batch_filter(x, P, zs, [F] * n, [Q] * n, [H] * n, [R] * n)
for x1, x2 in zip(m, mp):
assert np.allclose(x1, x2)
for p1, p2 in zip(c, cp):
assert np.allclose(p1, p2)
def ball_filter4(dt, R=1., Q=0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat([[
1.,
dt,
0,
0,
], [0, 1., 0, 0], [0, 0, 1., dt], [0, 0, 0., 1.]])
f1.H = np.mat([[1, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0]])
f1.B = np.mat([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0], [0, 0, 0, 1.]])
f1.u = np.mat([[0], [0], [-0.5 * g * dt**2], [-g * dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4, 4))
f1.Q[1, 1] = Q
f1.Q[3, 3] = Q
f1.x = np.mat([0, 0, 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def tracker1():
#
#
# Design 2D filter
#
#
# 1. Choose state vars - x
# 2. Design state trans. Function - F
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.
# time step 1 second
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
# 3. Design Process Noise Mat - Q
v = 0.05
q = Q_discrete_white_noise(dim=2, dt=dt, var=v)
tracker.Q = block_diag(q, q)
# 4. B
# 5. Design measurement function
tracker.H = np.array([[1/0.3048, 0, 0, 0],
[0, 0, 1/0.3048, 0]])
# 6. Design Meas. Noise Mat - R
tracker.R = np.array([[5, 0],[0, 5]])
# Init conditions
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
def _build_kf(self, init_box, Q_scale=1.0, R_scale=400.0):
kf = KalmanFilter(dim_x=self._N_STATE,
dim_z=self._N_MEAS)
kf.F = np.array([[1,0,0,0,1,0,0],
[0,1,0,0,0,1,0],
[0,0,1,0,0,0,1],
[0,0,0,1,0,0,0],
[0,0,0,0,1,0,0],
[0,0,0,0,0,1,0],
[0,0,0,0,0,0,1]])
kf.H = np.array([[1,0,0,0,0,0,0],
[0,1,0,0,0,0,0],
[0,0,1,0,0,0,0],
[0,0,0,1,0,0,0]])
Q = np.zeros_like(kf.F)
for i in range(self._N_MEAS, self._N_STATE):
Q[i, i] = Q_scale
R = np.eye(self._N_MEAS) * R_scale
kf.Q = Q
kf.R = R
kf.x = np.zeros((self._N_STATE, 1))
kf.x[:self._N_MEAS,:] = init_box.get_z()
return kf
def createLegKF(x, y):
# kalman_filter = KalmanFilter(dim_x=4, dim_z=2)
kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
KF_F = np.array([[1.0, dt, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, dt], [0, 0, 0, 1.0]])
KF_q = 0.7 # 0.3
KF_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q), np.zeros((2, 2)))),
np.hstack((np.zeros((2, 2)), Q_discrete_white_noise(2, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_P = np.diag([KF_pd, KF_pv, KF_pd, KF_pv])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
# KF_R = np.diag([KF_rd,KF_rd])
KF_R = np.diag([KF_rd, KF_rd, KF_rv, KF_rv])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
KF_H = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0], [0, 1.0, 0, 0], [0, 0, 0, 1.0]])
kalman_filter.x = np.array([x, 0, y, 0])
kalman_filter.F = KF_F
kalman_filter.H = KF_H
kalman_filter.Q = KF_Q
kalman_filter.B = 0
kalman_filter.R = KF_R
kalman_filter.P = KF_P
return kalman_filter
def test_rts():
fk = KalmanFilter(dim_x=2, dim_z=1)
fk.x = np.array([-1., 1.]) # initial state (location and velocity)
fk.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
fk.H = np.array([[1.,0.]]) # Measurement function
fk.P = .01 # covariance matrix
fk.R = 5 # state uncertainty
fk.Q = 0.001 # process uncertainty
zs = [t + random.randn()*4 for t in range(40)]
mu, cov, _, _ = fk.batch_filter (zs)
mus = [x[0] for x in mu]
M, P, _, _ = fk.rts_smoother(mu, cov)
if DO_PLOT:
p1, = plt.plot(zs,'cyan', alpha=0.5)
p2, = plt.plot(M[:,0],c='b')
p3, = plt.plot(mus,c='r')
p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["measurement", "RKS", "KF output", "ideal"], loc=4)
plt.show()
def ball_filter4(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat ([[1., dt, 0, 0,],
[0, 1., 0, 0],
[0, 0, 1., dt],
[0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0],
[0,0,0,0],
[0,0,1,0],
[0,0,0,0]])
f1.B = np.mat([[0,0,0,0],
[0,0,0,0],
[0,0,1.,0],
[0,0,0,1.]])
f1.u = np.mat([[0],
[0],
[-0.5*g*dt**2],
[-g*dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4,4))
f1.Q[1,1] = Q
f1.Q[3,3] = Q
f1.x = np.mat([0, 0 , 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def template():
kf = KalmanFilter(dim_x=2, dim_z=1)
# x0
kf.x = np.array([[.0], [.0]])
# P - change over time
kf.P = np.diag([500., 49.])
# F - state transition matrix
dt = .1
kf.F = np.array([[1, dt], [0, 1]])
## now can predict
## дисперсия растет и становится видна корреляция
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='r')
#kf.predict()
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='k', ls='dashed')
#show()
# Control information
kf.B = 0
# !!Attention!! Q! Process noise
kf.Q = Q_discrete_white_noise( dim=2, dt=dt, var=2.35)
# H - measurement function
kf.H = np.array([[1., 0.]])
# R - measure noise matrix
kf.R = np.array([[5.]])
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.0], [0.0]]) # initial state (location and velocity)
f.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
f.H = np.array([[1.0, 0.0]]) # Measurement function
f.P *= 1000.0 # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
fsq = SquareRootKalmanFilter(dim_x=2, dim_z=1)
fsq.x = np.array([[2.0], [0.0]]) # initial state (location and velocity)
fsq.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
fsq.H = np.array([[1.0, 0.0]]) # Measurement function
fsq.P = np.eye(2) * 1000.0 # covariance matrix
fsq.R = 5 # state uncertainty
fsq.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn() * 20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
fsq.update(z)
fsq.predict()
assert abs(f.x[0, 0] - fsq.x[0, 0]) < 1.0e-12
assert abs(f.x[1, 0] - fsq.x[1, 0]) < 1.0e-12
# save data
results.append(f.x[0, 0])
measurements.append(z)
p = f.P - fsq.P
print(f.P)
print(fsq.P)
for i in range(f.P.shape[0]):
assert abs(f.P[i, i] - fsq.P[i, i]) < 0.01
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2.0, 0]]).T
f.P = np.eye(2) * 100.0
m, c, _, _ = f.batch_filter(zs, update_first=False)
def multivariate_pos_vel_filter(x, P, R, Q, dt, F, H):
kf = KalmanFilter(dim_x=4, dim_z=4)
kf.x = np.array([x[0], x[1], x[2], x[3]])
kf.F = F
kf.H = H
kf.R = R
kf.Q = Q
return kf
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):
dt = 0.1
kf2 = KalmanFilter(dim_x=2, dim_z=2)
kf2.F = array ([[1., dt], [0., 1.]])
kf2.H = array ([[1., 0.], [1., 0.]])
kf2.x = array ([[0.], [0.]])
kf2.Q = array ([[dt**3/3, dt**2/2],
[dt**2/2, dt]]) * 0.02
kf2.P *= 100
kf2.R[0,0] = wheel_sigma**2
kf2.R[1,1] = gps_sigma**2
random.seed(SEED)
xs = []
zs = []
nom = []
for i in range(1, 100):
m0 = i + randn()*wheel_sigma
m1 = i + randn()*gps_sigma
if gps_sigma >1e40:
m1 = -1e40
z = array([[m0], [m1]])
kf2.predict()
kf2.update(z)
xs.append(kf2.x.T[0])
zs.append(z.T[0])
nom.append(i)
xs = asarray(xs)
zs = asarray(zs)
nom = asarray(nom)
res = nom-xs[:,0]
std_dev = np.std(res)
print('fusion std: {:.3f}'.format (np.std(res)))
if DO_PLOT:
plt.subplot(211)
plt.plot(xs[:,0])
#plt.plot(zs[:,0])
#plt.plot(zs[:,1])
plt.subplot(212)
plt.axhline(0)
plt.plot(res)
plt.show()
print(kf2.Q)
print(kf2.K)
return std_dev
def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.):
dt = 0.1
kf2 = KalmanFilter(dim_x=2, dim_z=2)
kf2.F = array ([[1., dt], [0., 1.]])
kf2.H = array ([[1., 0.], [1., 0.]])
kf2.x = array ([[0.], [0.]])
kf2.Q = array ([[dt**3/3, dt**2/2],
[dt**2/2, dt]]) * 0.02
kf2.P *= 100
kf2.R[0,0] = wheel_sigma**2
kf2.R[1,1] = gps_sigma**2
random.seed(SEED)
xs = []
zs = []
nom = []
for i in range(1, 100):
m0 = i + randn()*wheel_sigma
m1 = i + randn()*gps_sigma
if gps_sigma >1e40:
m1 = -1e40
z = array([[m0], [m1]])
kf2.predict()
kf2.update(z)
xs.append(kf2.x.T[0])
zs.append(z.T[0])
nom.append(i)
xs = asarray(xs)
zs = asarray(zs)
nom = asarray(nom)
res = nom-xs[:,0]
std_dev = np.std(res)
print('fusion std: {:.3f}'.format (np.std(res)))
if DO_PLOT:
plt.subplot(211)
plt.plot(xs[:,0])
#plt.plot(zs[:,0])
#plt.plot(zs[:,1])
plt.subplot(212)
plt.axhline(0)
plt.plot(res)
plt.show()
print(kf2.Q)
print(kf2.K)
return std_dev
def test_1d_0P():
global inf
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1., 1.],
[0., 1.]])) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q = np.eye(2)*0.0001
inf.P_inv = 0.000000000000000000001
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (50):
# create measurement = t plus white noise
z = t + random.randn()* np.sqrt(5)
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
try:
print(t, inf.P)
except:
pass
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.array([[1., 1.],
[0., 1.]]) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0, 0])
measurements.append(z)
# test mahalanobis
a = np.zeros(f.y.shape)
maha = scipy_mahalanobis(a, f.y, f.SI)
assert f.mahalanobis == approx(maha)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2., 0]]).T
f.P = np.eye(2) * 100.
s = Saver(f)
m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
s.to_array()
assert len(s.x) == len(zs)
assert len(s.x) == len(s)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, 'r', alpha=0.5)
p2, = plt.plot(results, 'b')
p4, = plt.plot(m[:, 0], 'm')
p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"], loc=4)
plt.show()
def initialize_filter(self, x_inicial):
filtro = KalmanFilter(dim_x=2, dim_z=1)
filtro.x = np.array([[x_inicial],[0.]])
filtro.F = np.array([[1.,1.],[0.,1.]])
filtro.H = np.array([[1.,0.]])
filtro.P = np.array([[10000.,0.],[0.,100.]])
filtro.R = 5.**2
filtro.Q = np.array([[0.0001,0.],[0.,0.00001]])
return filtro
def make_cv_filter(dt, noise_factor):
cvfilter = KalmanFilter(dim_x=2, dim_z=1)
cvfilter.x = array([0., 0.])
cvfilter.P *= 3
cvfilter.R *= noise_factor**2
cvfilter.F = array([[1, dt], [0, 1]], dtype=float)
cvfilter.H = array([[1, 0]], dtype=float)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
return cvfilter
def init_ca_filter(dt, std):
cafilter = KalmanFilter(dim_x=3, dim_z=1)
cafilter.x = np.array([0., 0., 0.])
cafilter.P *= 3
cafilter.R *= std
cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
cafilter.F = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]])
cafilter.H = np.array([[1., 0, 0]])
return cafilter
def tracker1():
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1. # time step 1 second
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
tracker.Q = block_diag(q, q)
tracker.H = np.array([[1, 0, 0, 0],[0, 0, 1, 0]])
tracker.R = np.eye(2) * R_std ** 2
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std ** 2)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
def init_cv_filter(dt, std):
cvfilter = KalmanFilter(dim_x=2, dim_z=1)
cvfilter.x = np.array([0., 0.])
cvfilter.P *= 3
cvfilter.R *= std**2
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
cvfilter.F = np.array([[1, dt], [0, 1]], dtype=float)
cvfilter.H = np.array([[1, 0]], dtype=float)
return cvfilter
def dog_tracking_filter(R, Q=0, cov=1.):
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.matrix([[0], [0]]) # initial state (location and velocity)
f.F = np.matrix([[1, 1], [0, 1]]) # state transition matrix
f.H = np.matrix([[1, 0]]) # Measurement function
f.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
def pos_vel_filter(x, P, R, Q=0., dt=1.0):
""" Returns a KalmanFilter which implements a
constant velocity model for a state [x dx].T
"""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([x[0], x[1]]) # location and velocity6.4. TRACKING A DOG
kf.F = np.array([[1, dt], [0, 1]]) # state transition matrix
kf.H = np.array([[1, 0]]) # Measurement function
kf.R *= R # measurement uncertainty
if np.isscalar(P):
kf.P *= P # covariance matrix
else:
kf.P[:] = P
if np.isscalar(Q):
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q)
else:
kf.Q = Q
return kf
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.array([[1., 1.],
[0., 1.]]) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0, 0])
measurements.append(z)
# test mahalanobis
a = np.zeros(f.y.shape)
maha = scipy_mahalanobis(a, f.y, f.SI)
assert f.mahalanobis == approx(maha)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2., 0]]).T
f.P = np.eye(2) * 100.
s = Saver(f)
m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
s.to_array()
assert len(s.x) == len(zs)
assert len(s.x) == len(s)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, 'r', alpha=0.5)
p2, = plt.plot(results, 'b')
p4, = plt.plot(m[:, 0], 'm')
p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"], loc=4)
plt.show()
def test_against_kf():
inv = np.linalg.inv
dt = 1.0
IM = np.eye(2)
Q = np.array([[.25, 0.5], [0.5, 1]])
F = np.array([[1, dt], [0, 1]])
#QI = inv(Q)
P = inv(IM)
from filterpy.kalman import InformationFilter
from filterpy.common import Q_discrete_white_noise
#f = IF2(2, 1)
r_std = .2
R = np.array([[r_std*r_std]])
RI = inv(R)
'''f.F = F.copy()
f.H = np.array([[1, 0.]])
f.RI = RI.copy()
f.Q = Q.copy()
f.IM = IM.copy()'''
kf = KalmanFilter(2, 1)
kf.F = F.copy()
kf.H = np.array([[1, 0.]])
kf.R = R.copy()
kf.Q = Q.copy()
f0 = InformationFilter(2, 1)
f0.F = F.copy()
f0.H = np.array([[1, 0.]])
f0.R_inv = RI.copy()
f0.Q = Q.copy()
#f.IM = np.zeros((2,2))
for i in range(1, 50):
z = i + (np.random.rand() * r_std)
f0.predict()
#f.predict()
kf.predict()
f0.update(z)
#f.update(z)
kf.update(z)
print(f0.x.T, kf.x.T)
assert np.allclose(f0.x, kf.x)
def dog_tracking_filter(R,Q=0,cov=1.):
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.matrix([[0], [0]]) # initial state (location and velocity)
f.F = np.matrix([[1,1],[0,1]]) # state transition matrix
f.H = np.matrix([[1,0]]) # Measurement function
f.R = R # measurement uncertainty
f.P *= cov # covariance matrix
f.Q = Q
return f
def make_cp_filter(self, dt, R_std):
cpfilter = KalmanFilter(dim_x=2, dim_z=2)
cpfilter.x = np.array([0., 0.])
cpfilter.P *= 3
cpfilter.R *= np.eye(2) * R_std**2
cpfilter.F = np.array([[1., 0], [0, 1.]], dtype=float)
cpfilter.H = np.array([[1., 0], [0, 1.]], dtype=float)
cpfilter.Q = np.eye(2) * 1
return cpfilter
def get_kalman():
ls_filter = KalmanFilter(dim_x=2, dim_z=1)
ls_filter.x = np.array([0., 0.]) # intial states: memory pressure, memory pressure gradient
ls_filter.F = np.array([[1., 1.], [0.,1.]]) # state transition matrix
ls_filter.H = np.array([[1., 0.]]) # measurement function
ls_filter.P *= 0. # initial uncertainty of states
ls_filter.R = 1 # measurement noise (larger, smoothier)
# refelcts uncertainly from higher-order unknow input (model noise)
ls_filter.Q = Q_discrete_white_noise(dim=2, dt=0.5, var=0.2)
return ls_filter
def make_cv_filter(dt, noise_factor):
cvfilter = KalmanFilter(dim_x = 2, dim_z=1)
cvfilter.x = array([0., 0.])
cvfilter.P *= 3
cvfilter.R *= noise_factor**2
cvfilter.F = array([[1, dt],
[0, 1]], dtype=float)
cvfilter.H = array([[1, 0]], dtype=float)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
return cvfilter
def init_kalman_filter(initValue):
my_filter = KalmanFilter(dim_x=2, dim_z=1)
# initial state (location and velocity)
my_filter.x = np.array([[initValue], [0.]])
my_filter.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix
my_filter.H = np.array([[1., 0.]]) # Measurement function
my_filter.P = np.array([[1, 0.], [0., 1]]) # covariance matrix
my_filter.R = np.array([[1000.]]) # Measurement noise
my_filter.Q = np.array([[1, 1], [1, 1]]) # process noise
return my_filter
def make_ca_filter(dt, noise_factor):
cafilter = KalmanFilter(dim_x=3, dim_z=1)
cafilter.x = array([0., 0., 0.])
cafilter.P *= 3
cafilter.R *= noise_factor**2
cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
cafilter.F = array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]],
dtype=float)
cafilter.H = array([[1, 0, 0]], dtype=float)
return cafilter
def test_1d_0P():
global inf
f = KalmanFilter(dim_x=2, dim_z=1)
inf = InformationFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.], [0.]]) # initial state (location and velocity)
f.F = (np.array([[1., 1.], [0., 1.]])) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2) * 0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1., 0.]]) # Measurement function
inf.R_inv *= 1. / 5 # state uncertainty
inf.Q = np.eye(2) * 0.0001
inf.P_inv = 0.000000000000000000001
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range(50):
# create measurement = t plus white noise
z = t + random.randn() * np.sqrt(5)
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
try:
print(t, inf.P)
except:
pass
# save data
r.append(f.x[0, 0])
r2.append(inf.x[0, 0])
m.append(z)
#assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def pos_vel_filter(x, P, R, Q=0., dt=1.0):
""" Returns a KalmanFilter which implements a
constant velocity model for a state [x dx].T
"""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([x[0], x[1]]) # location and velocity
kf.F = np.array([[1, dt],
[0, 1]]) # state transition matrix
kf.H = np.array([[1, 0]]) # Measurement function
kf.R *= R # measurement uncertainty
if np.isscalar(P):
kf.P *= P # covariance matrix
else:
kf.P[:] = P
if np.isscalar(Q):
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q)
else:
kf.Q = Q
return kf
def kalman_initialise(self, dim_x=3, dim_z=2, dt=0.5):
kfilter = KalmanFilter(dim_x, dim_z)
kfilter.x = np.array([88., 40., 0.])
kfilter.F = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]],
dtype=float)
kfilter.H = np.array([[1, 0, 0], [0, 1, 0]], dtype=float)
kfilter.P = Q_discrete_white_noise(dim_x, 1, 1)
kfilter.Q = 0.01 * np.array([[0.1, 0, 0], [0, 10, 0], [0, 0, 100]],
dtype=float)
kfilter.R = np.array([[1, 0], [0, 1]], dtype=float)
return kfilter
def getKalmanFilter(m):
kalman = KalmanFilter(len(m) * 2, len(m))
kalman.x = np.hstack((m, [0.0, 0.0])).astype(np.float)
kalman.F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0],
[0, 0, 0, 1]])
kalman.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
kalman.P *= 1000
kalman.R = 0.00001
kalman.Q = Q_discrete_white_noise(dim=4, dt=1, var=5)
kalman.B = 0
return kalman
def make_ca_filter(dt, noise_factor):
cafilter = KalmanFilter(dim_x=3, dim_z=1)
cafilter.x = array([0., 0., 0.])
cafilter.P *= 3
cafilter.R *= noise_factor**2
cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
cafilter.F = array([[1, dt, 0.5*dt*dt],
[0, 1, dt],
[0, 0, 1]], dtype=float)
cafilter.H = array([[1, 0, 0]], dtype=float)
return cafilter
def create_linear_kalman_filter_1d(dt, discrete_white_noise_var, r, x, p):
f = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]])
q = Q_discrete_white_noise(dim=3, dt=dt, var=discrete_white_noise_var)
tracker = KalmanFilter(dim_x=3, dim_z=1)
tracker.F = f
tracker.Q = q
tracker.x = np.array([x]).T
tracker.P = p
tracker.R = np.eye(1) * r
tracker.H = np.array([[1.0, 0.0, 0.0]])
return tracker
def setUp(self):
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.F = np.array([[1., 1.], [0., 1.]])
kf.H = np.eye(2)
kf.Q = np.eye(2)
kf.R = np.eye(2)
kf.x = np.array([[0., 0.]]).T
self.kf = kf
self.attacker = MoAttacker(self.kf)
self.data_count = 100
self.zs = [(i + randn()) for i in range(self.data_count)]
def run_tracker():
# parameters
filter_misestimation_factor = 1.0
sample_size = 100
used_taps = int(sample_size * 0.5)
measurement_std = 3.5
# set up sensor simulator
dt = 0.1
measurement_std_list = np.asarray([measurement_std] * sample_size)
sim = SensorSim(0, 0.1, measurement_std_list, 1, timestep=dt)
# set up kalman filter
tracker = KalmanFilter(dim_x=2, dim_z=1)
tracker.F = np.array([[1, dt], [0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
tracker.Q = q
tracker.H = np.array([[1, 0]])
tracker.R = measurement_std**2 * filter_misestimation_factor
tracker.x = np.array([[0, 0]]).T
tracker.P = np.eye(2) * 500
# perform sensor simulation and filtering
readings = []
truths = []
mu = []
residuals = []
for _ in measurement_std_list:
reading, truth = sim.read()
readings.extend(reading.flatten())
truths.extend(truth.flatten())
tracker.predict()
tracker.update(reading)
mu.extend(tracker.x[0])
residual_posterior = reading - np.dot(tracker.H, tracker.x)
residuals.extend(residual_posterior[0])
# error = np.asarray(truths) - mu
# plot_results(readings, mu, error, residuals)
# perform estimation
cor = Correlator(residuals)
correlation = cor.autocorrelation(used_taps)
R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P,
'posterior')
abs_err = measurement_std**2 - R_approx
rel_err = abs_err / measurement_std**2
print("True: %.3f" % measurement_std**2)
print("Filter: %.3f" % tracker.R)
print("Estimated (approximation): %.3f" % R_approx)
print("Absolute error: %.3f" % abs_err)
print("Relative error: %.3f %%" % (rel_err * 100))
print("-" * 15)
return rel_err
def FirstOrderKF(R, Q, dt):
""" Create first order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.zeros(2)
kf.P *= np.array([[100, 0], [0, 1]])
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, Q)
kf.F = np.array([[1., dt],
[0., 1]])
kf.H = np.array([[1., 0]])
return kf
def rot_box_kalman_filter(initial_state, Q_std, R_std):
"""
Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
position, centroid velocity, dimensions, and rotation angle.
Parameters
----------
initial_state : sequence of floats
[x, vx, y, vy, w, h, phi]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=7, dim_z=5)
dt = 1.0 # time step
# state mean and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.eye(kf.dim_x)
kf.F[0, 1] = kf.F[2, 3] = dt
# measurement matrix - maps from state space to observation space, so
# shape is dim_z x dim_x.
kf.H = np.zeros([kf.dim_z, kf.dim_x])
# z = Hx. H has nonzero coefficients for the following components of kf.x:
# x y w h phi
kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance for x-vx or y-vy pairs
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
# diagonal process noise sub-matrix for width, height, and phi
qq = Q_std**2*np.eye(3)
# process noise covariance matrix for full state
kf.Q = block_diag(q, q, qq)
return kf
def make_cv_filter(self, dt, R_std):
cvfilter = KalmanFilter(dim_x=4, dim_z=2)
cvfilter.x = np.array([0., 0., 0., 0.])
cvfilter.P *= 3
cvfilter.R *= np.eye(2) * R_std**2
cvfilter.F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
cvfilter.H = np.array([[1., 0, 0, 0], [0, 0, 1., 0]], dtype=float)
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
cvfilter.Q = block_diag(q, q)
return cvfilter
def FirstOrderKF(self, R, Q, dt):
""" Create first order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.zeros(2)
kf.P *= np.array([[100, 0], [0, 1]])
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, Q)
kf.F = np.array([[1., dt],
[0., 1]])
kf.H = np.array([[1., 0]])
return kf
def rot_box_kalman_filter(initial_state, Q_std, R_std):
"""
Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
position, centroid velocity, dimensions, and rotation angle.
Parameters
----------
initial_state : sequence of floats
[x, vx, y, vy, w, h, phi]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=7, dim_z=5)
dt = 1.0 # time step
# state mean and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.eye(kf.dim_x)
kf.F[0, 1] = kf.F[2, 3] = dt
# measurement matrix - maps from state space to observation space, so
# shape is dim_z x dim_x.
kf.H = np.zeros([kf.dim_z, kf.dim_x])
# z = Hx. H has nonzero coefficients for the following components of kf.x:
# x y w h phi
kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance for x-vx or y-vy pairs
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
# diagonal process noise sub-matrix for width, height, and phi
qq = Q_std**2 * np.eye(3)
# process noise covariance matrix for full state
kf.Q = block_diag(q, q, qq)
return kf
def kf(s, r, q):
'''Perform Kalman Filter on pandas Series
Inputs:
s -- Pandas series with data to filter, index is datetime
r -- measurement noise variance (use 10*quiet_period_variance)
q -- process noise variance (use 20)
Returns:
f -- Kalman Filter model
'''
estimate_columns = ['z', 'x', 'dx', 'r', 'v11', 'v21', 'v12', 'v22']
estimate_df = pd.DataFrame(index=s.index, columns=estimate_columns)
estimate_df = estimate_df.fillna(0)
# NCV model
f = KalmanFilter(dim_x=2, dim_z=1)
# Initial condition
f.x = np.array([s[0], s.diff()[1:5].mean()])
# State transition matrix
f.F = np.array([[
1.,
1.,
], [
0,
1.,
]])
# Measurement Function
f.H = np.array([[1., 0]])
# Covariance Matrix
f.P *= 1
# Measurement Noise Covariance
f.R = r
# Add noise profile
f.Q = Q_discrete_white_noise(dim=2, dt=1, var=q)
for k in range(0, len(estimate_df)):
z = s[k]
f.predict()
f.update(z)
estimate_df.loc[estimate_df.index[k], 'z'] = z
estimate_df.loc[estimate_df.index[k], ['x', 'dx']] = list(f.x)
estimate_df.loc[estimate_df.index[k], 'r'] = f.y**2
estimate_df.loc[estimate_df.index[k],
['v11', 'v21', 'v12', 'v22']] = list(np.reshape(
f.P, 4))
return (estimate_df, f)
def test_1d_0P():
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1.,1.],
[0.,1.]])) # state transition matrix
f.H = np.array([[1.,0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q *= 0.0001
inf.P_inv = 0
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def test_1d_0P():
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1.,1.],
[0.,1.]])) # state transition matrix
f.H = np.array([[1.,0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q *= 0.0001
inf.P_inv = 0
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def tracker1():
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
tracker.u = 0
tracker.H = np.array([[1 / 0.3048, 0, 0, 0], [0, 0, 1 / 0.3048, 0]])
tracker.R = np.eye(2) * R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500
return tracker
def test_univariate():
f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
f.x = np.array([[0]])
f.P *= 50
f.H = np.array([[1.]])
f.F = np.array([[1.]])
f.B = np.array([[1.]])
f.Q = .02
f.R *= .1
for i in range(50):
f.predict()
f.update(i)
def test_univariate():
f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
f.x = np.array([[0]])
f.P *= 50
f.H = np.array([[1.]])
f.F = np.array([[1.]])
f.B = np.array([[1.]])
f.Q = .02
f.R *= .1
for i in range(50):
f.predict()
f.update(i)
