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_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_radar():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
return A.dot(x)
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0.)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20+dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
for i in range(len(t)):
r = radar.get_range()
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.subplot(313)
plt.plot(t, xs[:, 2])
def _test_log_likelihood():
from filterpy.common import Saver
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=points)
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1.1**2, block_size=2)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i + randn() * z_std, i + randn() * z_std] for i in range(40)]
s = Saver(kf)
for z in zs:
kf.predict()
kf.update(z)
print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
if DO_PLOT:
plt.plot(s.x[:, 0], s.x[:, 2])
def test_1d():
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1],
[1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i+randn()*0.1]])
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
s.save()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
def test_steadystate():
dim = 7
cv = kinematic_kf(dim=dim, order=5)
cv.x[1] = 1.0
for i in range(100):
cv.predict()
cv.update([i]*dim)
for i in range(100):
cv.predict_steadystate()
cv.update_steadystate([i]*dim)
# test mahalanobis
a = np.zeros(cv.y.shape)
maha = scipy_mahalanobis(a, cv.y, cv.SI)
assert cv.mahalanobis == approx(maha)
def test_steadystate():
dim = 7
cv = kinematic_kf(dim=dim, order=5)
cv.x[1] = 1.0
for i in range(100):
cv.predict()
cv.update([i] * dim)
for i in range(100):
cv.predict_steadystate()
cv.update_steadystate([i] * dim)
# test mahalanobis
a = np.zeros(cv.y.shape)
maha = scipy_mahalanobis(a, cv.y, cv.SI)
assert cv.mahalanobis == approx(maha)
def test_1d():
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1], [1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i + randn() * 0.1]])
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
s.save()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
def test_mahalanobis():
global a, b, S
# int test
a, b, S = 3, 1, 2
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
# int list
assert abs(mahalanobis([a], [b], [S]) -
scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
assert abs(mahalanobis([a], b, S) -
scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
# float
a, b, S = 3.123, 3.235235, .01234
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
assert abs(mahalanobis([a], [b], [S]) -
scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
assert abs(mahalanobis([a], b, S) -
scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
#float array
assert abs(
mahalanobis(np.array([a]), b, S) -
scipy_mahalanobis(a, b, 1 / S)) < 1.e-12
#1d array
a = np.array([1., 2.])
b = np.array([1.4, 1.2])
S = np.array([[1., 2.], [2., 4.001]])
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
#2d array
a = np.array([[1., 2.]])
b = np.array([[1.4, 1.2]])
S = np.array([[1., 2.], [2., 4.001]])
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a.T, b, S) -
scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a, b.T, S) -
scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a.T, b.T, S) -
scipy_mahalanobis(a, b, inv(S))) < 1.e-12
try:
# mismatched shapes
mahalanobis([1], b, S)
assert "didn't catch vectors of different lengths"
except ValueError:
pass
except:
assert "raised exception other than ValueError"
# okay, now check for numerical accuracy
for _ in range(ITERS):
N = np.random.randint(1, 20)
a = np.random.randn(N)
b = np.random.randn(N)
S = np.random.randn(N, N)
S = np.dot(S, S.T) #ensure positive semi-definite
assert abs(mahalanobis(a, b, S) -
scipy_mahalanobis(a, b, inv(S))) < 1.e-12
def test_mahalanobis():
global a, b, S
# int test
a, b, S = 3, 1, 2
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
# int list
assert abs(mahalanobis([a], [b], [S]) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
assert abs(mahalanobis([a], b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
# float
a, b, S = 3.123, 3.235235, .01234
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
assert abs(mahalanobis([a], [b], [S]) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
assert abs(mahalanobis([a], b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
#float array
assert abs(mahalanobis(np.array([a]), b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12
#1d array
a = np.array([1., 2.])
b = np.array([1.4, 1.2])
S = np.array([[1., 2.], [2., 4.001]])
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
#2d array
a = np.array([[1., 2.]])
b = np.array([[1.4, 1.2]])
S = np.array([[1., 2.], [2., 4.001]])
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a.T, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a, b.T, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
assert abs(mahalanobis(a.T, b.T, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
try:
# mismatched shapes
mahalanobis([1], b, S)
assert "didn't catch vectors of different lengths"
except ValueError:
pass
except:
assert "raised exception other than ValueError"
# okay, now check for numerical accuracy
for _ in range(ITERS):
N = np.random.randint(1, 20)
a = np.random.randn(N)
b = np.random.randn(N)
S = np.random.randn(N, N)
S = np.dot(S, S.T) #ensure positive semi-definite
assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12
def test_ekf():
def H_of(x):
""" compute Jacobian of H matrix for state x """
horiz_dist = x[0]
altitude = x[2]
denom = sqrt(horiz_dist**2 + altitude**2)
return array([[horiz_dist / denom, 0., altitude / denom]])
def hx(x):
""" takes a state variable and returns the measurement that would
correspond to that state.
"""
return sqrt(x[0]**2 + x[2]**2)
dt = 0.05
proccess_error = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
rk.F = eye(3) + array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) * dt
def fx(x, dt):
return np.dot(rk.F, x)
rk.x = array([-10., 90., 1100.])
rk.R *= 10
rk.Q = array([[0, 0, 0], [0, 1, 0], [0, 0, 1]]) * 0.001
rk.P *= 50
rs = []
xs = []
radar = RadarSim(dt)
ps = []
pos = []
s = Saver(rk)
for i in range(int(20 / dt)):
z = radar.get_range(proccess_error)
pos.append(radar.pos)
rk.update(asarray([z]), H_of, hx, R=hx(rk.x) * proccess_error)
ps.append(rk.P)
rk.predict()
xs.append(rk.x)
rs.append(z)
s.save()
# test mahalanobis
a = np.zeros(rk.y.shape)
maha = scipy_mahalanobis(a, rk.y, rk.SI)
assert rk.mahalanobis == approx(maha)
s.to_array()
xs = asarray(xs)
ps = asarray(ps)
rs = asarray(rs)
p_pos = ps[:, 0, 0]
p_vel = ps[:, 1, 1]
p_alt = ps[:, 2, 2]
pos = asarray(pos)
if DO_PLOT:
plt.subplot(311)
plt.plot(xs[:, 0])
plt.ylabel('position')
plt.subplot(312)
plt.plot(xs[:, 1])
plt.ylabel('velocity')
plt.subplot(313)
#plt.plot(xs[:,2])
#plt.ylabel('altitude')
plt.plot(p_pos)
plt.plot(-p_pos)
plt.plot(xs[:, 0] - pos)
def test_ekf():
def H_of(x):
""" compute Jacobian of H matrix for state x """
horiz_dist = x[0]
altitude = x[2]
denom = sqrt(horiz_dist**2 + altitude**2)
return array([[horiz_dist/denom, 0., altitude/denom]])
def hx(x):
""" takes a state variable and returns the measurement that would
correspond to that state.
"""
return sqrt(x[0]**2 + x[2]**2)
dt = 0.05
proccess_error = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
rk.F = eye(3) + array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])*dt
def fx(x, dt):
return np.dot(rk.F, x)
rk.x = array([-10., 90., 1100.])
rk.R *= 10
rk.Q = array([[0, 0, 0],
[0, 1, 0],
[0, 0, 1]]) * 0.001
rk.P *= 50
rs = []
xs = []
radar = RadarSim(dt)
ps = []
pos = []
s = Saver(rk)
for i in range(int(20/dt)):
z = radar.get_range(proccess_error)
pos.append(radar.pos)
rk.update(asarray([z]), H_of, hx, R=hx(rk.x)*proccess_error)
ps.append(rk.P)
rk.predict()
xs.append(rk.x)
rs.append(z)
s.save()
# test mahalanobis
a = np.zeros(rk.y.shape)
maha = scipy_mahalanobis(a, rk.y, rk.SI)
assert rk.mahalanobis == approx(maha)
s.to_array()
xs = asarray(xs)
ps = asarray(ps)
rs = asarray(rs)
p_pos = ps[:, 0, 0]
p_vel = ps[:, 1, 1]
p_alt = ps[:, 2, 2]
pos = asarray(pos)
if DO_PLOT:
plt.subplot(311)
plt.plot(xs[:, 0])
plt.ylabel('position')
plt.subplot(312)
plt.plot(xs[:, 1])
plt.ylabel('velocity')
plt.subplot(313)
#plt.plot(xs[:,2])
#plt.ylabel('altitude')
plt.plot(p_pos)
plt.plot(-p_pos)
plt.plot(xs[:, 0] - pos)
