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 _KF_init(self): # para: Center of box used for prediction
KF = KalmanFilter(4,2)
# KF.x = location + [0,0,0,0]
# KF.F = np.array([
# [1,0,0,0,1,0,0,0],
# [0,1,0,0,0,1,0,0],
# [0,0,1,0,0,0,1,0],
# [0,0,0,1,0,0,0,1],
# [0,0,0,0,1,0,0,0],
# [0,0,0,0,0,1,0,0],
# [0,0,0,0,0,0,1,0],
# [0,0,0,0,0,0,0,1]])
# KF.H = np.array([
# [1,0,0,0,0,0,0,0],
# [0,1,0,0,0,0,0,0],
# [0,0,1,0,0,0,0,0],
# [0,0,0,1,0,0,0,0]])
KF.x = self.KF_center + [0,0] # can be improved for accuracy e.g. from which edge
KF.F = np.array([
[1,0,1,0],
[0,1,0,1],
[0,0,1,0],
[0,0,0,1]])
KF.H = np.array([
[1,0,0,0],
[0,1,0,0]])
KF.P *= 100
KF.R *= 100
# KF.Q *= 2
# KF.predict()
return KF
def create_kalman_filter(self, det):
"""(x, y, s(area), r(aspect ratio), x', y', s')
"""
model = KalmanFilter(dim_x=7, dim_z=4)
model.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],
], 'float32')
model.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],
], 'float32')
model.R[2:,2:] *= 10.
model.P[4:,4:] *= 1000. # high uncertainty of initial volocity
model.P *= 10.
model.Q[-1,-1] *= 0.01
model.Q[4:,4:] *= 0.01
model.x[:4] = np.array(xywh_to_xysr(*det), 'float32').reshape(4, 1)
return model
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 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 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 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 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 kf_circle():
from filterpy.kalman import KalmanFilter
from math import radians
import math
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1]+x[2], x[2]])
def hx_inv(x, y):
angle = math.atan2(y,x)
radius = math.sqrt(x*x + y*y)
return np.array([radius, angle])
std_noise = .1
kf = KalmanFilter(dim_x=3, dim_z=2)
kf.x = np.array([50., 0., 0.])
F = np.array([[1., 0, 0.],
[0., 1., 1.,],
[0., 0., 1.,]])
kf.F = F
kf.P*= 100
kf.H = np.array([[1,0,0],
[0,1,0]])
kf.R = np.eye(2)*(std_noise**2)
#kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
zs = []
kfxs = []
for t in range (0,2000):
a = t / 30 + 90
x = cos(radians(a)) * 50.+ randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
z = hx_inv(x,y)
zs.append(z)
kf.predict()
kf.update(z)
# save data
kfxs.append(kf.x)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
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 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 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 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 one_run_test_fls():
fls = FixedLagSmoother(dim_x=2, dim_z=1)
fls.x = np.array([0., .5])
fls.F = np.array([[1.,1.],
[0.,1.]])
fls.H = np.array([[1.,0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([0., .5])
kf.F = np.array([[1.,1.],
[0.,1.]])
kf.H = np.array([[1.,0.]])
kf.P *= 2000
kf.R *= 1.
kf.Q *= 0.001
N = 4 # size of lag
nom = np.array([t/2. for t in range (0,40)])
zs = np.array([t + random.randn()*1.1 for t in nom])
xs, x = fls.smooth_batch(zs, N)
M, P, *_ = kf.batch_filter(zs)
rts_x, *_ = kf.rts_smoother(M, P)
xfl = xs[:,0].T[0]
xkf = M[:,0].T[0]
fl_res = abs(xfl-nom)
kf_res = abs(xkf-nom)
if DO_PLOT:
plt.cla()
plt.plot(zs,'o', alpha=0.5, marker='o', label='zs')
plt.plot(x[:,0], label='FLS')
plt.plot(xfl, label='FLS S')
plt.plot(xkf, label='KF')
plt.plot(rts_x[:,0], label='RTS')
plt.legend(loc=4)
plt.show()
print(fl_res)
print(kf_res)
print('std fixed lag:', np.mean(fl_res[N:]))
print('std kalman:', np.mean(kf_res[N:]))
return np.mean(fl_res) <= np.mean(kf_res)
def makeLinearKF(A, B, C, P, F, x, R = None, dim_x = 4, dim_z = 4):
kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
kf.x = x
kf.P = P
kf.F = F
kf.H = C
kf.R = R
return kf
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 test_1d():
global inf
f = KalmanFilter(dim_x=2, dim_z=1)
inf = InformationFilter(dim_x=2, dim_z=1)
# ensure __repr__ doesn't assert
str(inf)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
inf.x = f.x.copy()
f.F = (np.array([[1.,1.],
[0.,1.]])) # state transition matrix
inf.F = f.F.copy()
f.H = np.array([[1.,0.]]) # Measurement function
inf.H = np.array([[1.,0.]]) # Measurement function
f.R *= 5 # state uncertainty
inf.R_inv *= 1./5 # state uncertainty
f.Q *= 0.0001 # process uncertainty
inf.Q *= 0.0001
m = []
r = []
r2 = []
zs = []
s = Saver(inf)
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()
inf.update(z)
inf.predict()
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
print(inf.y)
s.save()
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 ZeroOrderKF(R, Q, P=20):
""" Create zero order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=1, dim_z=1)
kf.x = np.array([0.])
kf.R *= R
kf.Q *= Q
kf.P *= P
kf.F = np.eye(1)
kf.H = np.eye(1)
return kf
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 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 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_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 single_measurement_test():
dt = 0.1
sigma = 2.
kf2 = KalmanFilter(dim_x=2, dim_z=1)
kf2.F = array ([[1., dt], [0., 1.]])
kf2.H = array ([[1., 0.]])
kf2.x = array ([[0.], [1.]])
kf2.Q = array ([[dt**3/3, dt**2/2],
[dt**2/2, dt]]) * 0.02
kf2.P *= 100
kf2.R[0,0] = sigma**2
random.seed(SEED)
xs = []
zs = []
nom = []
for i in range(1, 100):
m0 = i + randn()*sigma
z = array([[m0]])
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('std: {:.3f}'.format (std_dev))
global DO_PLOT
if DO_PLOT:
plt.subplot(211)
plt.plot(xs[:,0])
#plt.plot(zs[:,0])
plt.subplot(212)
plt.plot(res)
plt.show()
return std_dev
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1.,0.]])
P = np.eye(2)
R = np.eye(1)*std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range (2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append (x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def init_tracker():
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0 # time step
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
tracker.R = np.eye(2) * 1000
q = Q_discrete_white_noise(dim=2, dt=dt, var=1)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[2000, 0, 700, 0]]).T
tracker.P = np.eye(4) * 50.0
return tracker
def test_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2.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
zs = [None, 1.0, 2.0]
m, c, _, _ = f.batch_filter(zs, update_first=False)
m, c, _, _ = f.batch_filter(zs, update_first=True)
def createPersonKF(x, y):
kalman_filter = KalmanFilter(dim_x=6, dim_z=2)
# kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
dt2 = 0.005
# KF_F = np.array([[1., dt, 0 , 0],
# [0 , 1., 0 , 0],
# [0 , 0 , 1., dt],
# [0 , 0 , 0 , 1.]])
KF_Fca = np.array([[1.0, dt, dt2 / 2], [0, 1.0, dt], [0, 0, 1]])
KF_F = np.vstack(
(np.hstack((KF_Fca, np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), KF_Fca))) # np.zeros((3,3)))),
) # , np.zeros((3,3)))) )))#,
# np.hstack((np.zeros((3,3)),np.zeros((3,3)), KF_Fca))))
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_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(3, dt=0.1, var=KF_q), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), Q_discrete_white_noise(3, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_pa = 30.0
KF_P = np.diag([KF_pd, KF_pv, KF_pa, KF_pd, KF_pv, KF_pa])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
KF_ra = 2 # 0.5
KF_R = np.diag([KF_rd, KF_rd])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv, KF_ra, KF_ra])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0],[0,1.,0,0],[0,0,0,1.]])
KF_H = np.array([[1.0, 0, 0, 0, 0, 0], [0, 0, 0, 1.0, 0, 0]])
# kalman_filter.x = np.array([x,0,y,0])
kalman_filter.x = np.array([x, 0, 0, y, 0, 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
return [self.pos[0] + random.randn() * self.noise_scale,
self.pos[1] + random.randn() * self.noise_scale]
pos = [4,3]
s = PosSensor1(pos, (2,1), 1)
for i in range (50):
pos = s.read()
plt.scatter(pos[0], pos[1])
plt.show()
from filterpy.kalman import KalmanFilter
import numpy as np
f1 = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.# time step
f1.F = np.array ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
f1.u = 0 # no info about rotors turning
f1.H = np.array ([[1, 0, 0, 0], #measurement to state.
[0, 0, 1, 0]])
f1.R = np.array([[5,0], # initial variance in measurments
[0, 5]])
f1.Q = np.eye(4) * 0.001 # process noise
f1.x = np.array([[0,0,0,0]]).T # initial position
f1.P = np.eye(4) * 500 #covariance matrix
# initialize storage and other variables for the run
count = 60
xs, ys = [],[]
pxs, pys = [],[]
s = PosSensor1 ([0,0], (2,1), 3.)
a, b, c = parameters["a"], parameters["b"], parameters["c"]
abc_filter = KalmanFilter(dim_x=2, dim_z=2, dim_u=0)
# INIT FILTER
# ------- Predict Variables -------
# State Vector (X): storage and rainfall
# (2, 1) = column vector
abc_filter.x = np.array([[S0, r0]]).T
# State Covariance (P) initial estimate
# (2, 2) = square matrix
abc_filter.P[:] = np.diag([s_uncertainty, r_uncertainty])
# state transition (F) - the process model
# (2, 2) = square matrix
abc_filter.F = np.array([[1 - c, a], [0.0, 1.0]])
# Process noise (Q)
# (2, 2) = square matrix
abc_filter.Q = np.diag([s_noise, r_noise])
# ------- Update Variables -------
# Measurement function (H) (how do we go from state -> observed?)
# (1, 2) = row vector
abc_filter.H = np.array([[c, (1 - a - b)], [0, 1]])
# measurement uncertainty
# (2, 2) = square matrix OR is it just uncertainty on discharge (q)
abc_filter.R *= R
# Control inputs (defaults)
tracker.Q = block_diag(q, q)
tracker.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0]])
tracker.R = np.diag((0.1, 0.1, 0.1))
tracker.x = np.array([[0, 0, 0, 0, 0, 0]]).T
tracker.P = np.eye(6) * 25
return tracker
tracker = KalmanFilterpy(dim_x=6, dim_z=3)
dt = 0.2
tracker.F = np.array([[1.0, 0.0, 0.0, dt, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0, dt, 0.0],
[0.0, 0.0, 1.0, 0.0, 0.0, dt],
[0.0, 0.0, 0.0, 1, 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, 1]])
class KalmanFilter(object):
"""Kalman Filter class keeps track of the estimated state of
the system and the variance or uncertainty of the estimate.
Predict and Correct methods implement the functionality
Reference: https://en.wikipedia.org/wiki/Kalman_filter
Attributes: None
"""
def __init__(self):
"""Initialize variable used by Kalman Filter class
Return:
None
ESC4 = 20
power = 1050
powerrec = np.array([])
u_array = np.array([[0], [0], [0], [0]])
ref_array = np.array([[0], [0], [0], [0]])
integral_array = np.array([[0], [0], [0], [0]])
ucontrolrec = np.array([])
roll_ref = 0
pitch_ref = 0
gpsd = None
## Kalman Filter Initialization
kf = KalmanFilter(dim_x=4, dim_z=4)
kf.x = np.array([[0], [0], [0], [0]])
kf.F = np.array([[1, 0, 0, 0], [DT, 1, 0, 0], [0, 0, 1, 0], [0, 0, DT, 1]])
kf.B = np.array([[0.0001, -0.0001, -0.0001, 0.0001],
[0.0000007, -0.0000007, -0.0000007, 0.0000007],
[0.0001, 0.0001, -0.0001, -0.0001],
[0.0000007, 0.0000007, -0.0000007, -0.0000007]])
kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.P = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.R = np.array([[200, 0, 0, 0], [0, 1000, 0, 0], [0, 0, 200, 0],
[0, 0, 0, 1000]]) # default is 1000, 10000
klqr1 = 20 # default 20
klqr2 = 40 # default 80
klqr = np.array([[klqr1, klqr2, klqr1, klqr2], [-klqr1, -klqr2, klqr1, klqr2],
[-klqr1, -klqr2, -klqr1, -klqr2],
[klqr1, klqr2, -klqr1, -klqr2]])
# Created by Lee Yam Keng at 2018-09-12
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([
[2.], # position
[0.]
]) # velocity
f.x = np.array([2., 0.])
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P *= 1000.
f.P = np.array([[1000., 0.], [0., 1000.]])
f.R = 5
f.R = np.array([[5.]])
f.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
while True:
f.predict()
f.update(get_some_measurement())
# do something with the output
x = f.x
do_something_amazing(x)
sensor_readings = np.ndarray.tolist((process.get_column(index=11)))
timesteps = np.ndarray.tolist((process.get_column(index=0)))
#print(process.get_column(index=0))
# todo use numpy
alt_list = []
vel_list = []
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0.])
transition_matrix = np.array([[1., 1.], [0., 1.]])
f.F = transition_matrix
f.H = np.array([[1., 0.]])
f.P = np.array([[1000., 0.], [0., 1000.]])
f.R = np.array([[5.]])
from filterpy.common import Q_discrete_white_noise
f.Q = Q_discrete_white_noise(dim=2, dt=1., var=0.13)
for i in range(0, len(sensor_readings)):
sensor_reading = sensor_readings[i]
if i + 1 < len(sensor_readings):
## control transition matrix
import numpy as np
import cv2
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
####################################
x_kalman = KalmanFilter(dim_x=2, dim_z=1)
x_kalman.x = np.array([
[0.], # position
[0.]
]) # velocity
x_kalman.F = np.array([[1., 1.], [0., 1.]])
x_kalman.H = np.array([[1., 0.]])
x_kalman.P = x_kalman.P * 1000.0 #.array([[1000., 0.],
# [ 0., 1000.] ])
x_kalman.R = np.array([[5.]])
x_kalman.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
###################################
y_kalman = KalmanFilter(dim_x=2, dim_z=1)
y_kalman.x = np.array([
[0.], # position
# -*- coding: utf-8 -*- """ Created on Thu Jun 18 09:16:54 2015 @author: rlabbe """ from filterpy.kalman import KalmanFilter kf = KalmanFilter(1, 1) kf.x[0,0] = 1. kf.P = np.ones((1,1)) kf.H = np.array([[2.]]) kf.F = np.ones((1,1)) kf.R = 1 kf.Q = 0 kf.predict() kf.update(2)
from filterpy.common import Q_discrete_white_noise
def gen_track(start, end, stepsize=(0.3, 0.3)):
""" Generates track as seen in a tpc with wirespaceing 0.3mm from start to end position"""
wire_z = np.arange(start[0], end[0], step=stepsize[0])
wire_y = np.arange(start[1], end[1], step=stepsize[1])
if len(wire_z) == 0:
wire_z = np.array([start[0]] * len(wire_y))
if len(wire_y) == 0:
wire_y = np.array([start[1]] * len(wire_z))
return wire_z, wire_y
kf = KalmanFilter(dim_x=4, dim_z=2)
dz = 0.3
kf.x = np.array([1., 1.])
kf.R = 2
kf.F = np.array([[1., dz, 0., 0.], [0., 1., 0., 0.], [0., 0., 1., dz],
[0., 0., 0., 1.]])
kf.H = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.]])
kf.P *= [5., 2., 5., 2.]
q = Q_discrete_white_noise(2, dz, var=2.)
kf.Q = np.diag([q, q])
def kf_circle():
from filterpy.kalman import KalmanFilter
from math import radians
import math
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1] + x[2], x[2]])
def hx_inv(x, y):
angle = math.atan2(y, x)
radius = math.sqrt(x * x + y * y)
return np.array([radius, angle])
std_noise = .1
kf = KalmanFilter(dim_x=3, dim_z=2)
kf.x = np.array([50., 0., 0.])
F = np.array([[1., 0, 0.], [
0.,
1.,
1.,
], [
0.,
0.,
1.,
]])
kf.F = F
kf.P *= 100
kf.H = np.array([[1, 0, 0], [0, 1, 0]])
kf.R = np.eye(2) * (std_noise**2)
#kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
zs = []
kfxs = []
for t in range(2000):
a = t / 30 + 90
x = cos(radians(a)) * 50. + randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
z = hx_inv(x, y)
zs.append(z)
kf.predict()
kf.update(z)
# save data
kfxs.append(kf.x)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
if DO_PLOT:
plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
plt.plot(kfxs[:, 0], kfxs[:, 1], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
outputMeasSnr_Linear[batchIdx] = np.mean(np.divide(np.power(yGroundTruth[:, 1], 2), np.diag(R)[1]))
else:
outputMeasSnr_Linear[batchIdx] = np.mean(np.divide(np.power(yGroundTruth[:, 0], 2), np.diag(R)[0]))
processNoiseMeanPower_dbm = 10*np.log10(np.mean(np.power(xGroundTruth - xPredictGroundTruth, 2), axis=0)) + 30
autonomousStateMeanPower_dbm = 10*np.log10(np.mean(np.power(xPredictGroundTruth, 2), axis=0)) + 30
processMeanSnr_db = autonomousStateMeanPower_dbm - processNoiseMeanPower_dbm
processMeanSnr_Linear[batchIdx] = np.power(10, np.divide(processMeanSnr_db[-2], 10))
if enablePlot and batchIdx == 0:
print('input meas snr: %d [db]; output meas snr: %d [db]' %(inputMeasSnr_db, outputMeanSnr_db))
print('process mean snr [db]: ', np.array_str(processMeanSnr_db, precision=0, suppress_small=True))
if batchIdx == 0:
kf = KalmanFilter(dim_x=xDim, dim_z=measDim)
kf.F = F
kf.H = H
kf.R = R
kf.Q = Q
kf.x = np.zeros((xDim, 1))
kf.P = np.diag(Q)[0] * np.eye(xDim)
#kf.test_matrix_dimensions()
muF, covF, _, _ = kf.batch_filter(y)
muS, covS, _, _ = kf.rts_smoother(muF, covF)
errF = xGroundTruth - muF[:,:,0]
errS = xGroundTruth - muS[:,:,0]
init_vec = np.array([theta_init, thetadot_init])
kl = KLFilter(init_vec, F=F, P=P, R=R, Q=Q, B=B)
kl2_predict = np.zeros((N, 2))
kl2_estimate = np.zeros((N, 2))
kl_predict = np.zeros((N, 2))
kl_estimate = np.zeros((N, 2))
# Initialize:
kl2_predict[0] = init_vec
kl_predict[0] = init_vec
kl2 = KalmanFilter(dim_x=2, dim_z=2, dim_u=1)
kl2.x = init_vec
kl2.F = F
kl2.H = np.eye(2)
kl2.P = P
kl2.R = R
kl2.Q = Q
kl2.B = B.ravel()
for i in range(1, N):
input_thetadotdot = model_thetadotdot(a_ground[i - 1], theta[i - 1])
kl2.predict(input_thetadotdot) #(input_f[i - 1]/m_train)
kl2_predict[i] = kl2.x.ravel()
kl2.update(kalman_input_arr[i])
kl2_estimate[i] = kl2.x.ravel()
kl_predict[i] = kl.predict(input_thetadotdot) #(input_f[i-1]/m_train)
kl_estimate[i] = kl.update(kalman_input_arr[i])
def kinematic_kf(dim, order, dt=1., order_by_dim=True):
""" Returns a KalmanFilter using newtonian kinematics for an arbitrary
number of dimensions and order. So, for example, a constant velocity
filter in 3D space would be created with
kinematic_kf(3, 1)
which will set the state `x` to be interpreted as
[x, x', y, y', z, z'].T
If you set `order_by_dim` to False, then `x` is assumed to be
[x y z x' y' z'].T
As another example, a 2D constant jerk is created with
kinematic_kf(2, 3)
Assumes that the measurement z is position in each dimension. If this is not
true you will have to alter the H matrix by hand.
P, Q, R are all set to the Identity matrix.
H is assigned assuming the measurement is position, one per dimension `dim`.
Parameters
----------
dim : int
number of dimensions
order : int, >= 1
order of the filter. 2 would be a const acceleration model.
dim_z : int, default 1
size of z vector *per* dimension `dim`. Normally should be 1
dt : float, default 1.0
Time step. Used to create the state transition matrix
"""
dim_x = order + 1
kf = KalmanFilter(dim_x=dim * dim_x, dim_z=dim)
F = kinematic_state_transition(order, dt)
if order_by_dim:
diag = [F] * dim
kf.F = sp.linalg.block_diag(*diag)
else:
kf.F.fill(0.0)
for i, x in enumerate(F.ravel()):
f = np.eye(dim) * x
ix, iy = (i // dim_x) * dim, (i % dim_x) * dim
kf.F[ix:ix + dim, iy:iy + dim] = f
if order_by_dim:
for i in range(dim):
kf.H[i, i * dim_x] = 1.
else:
for i in range(dim):
kf.H[i, i] = 1.
return kf
