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 = UKF(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()
s.to_array()
plt.plot(s.x[:, 0], s.x[:, 2])
def test_1d_array():
f1 = GHFilter (0, 0, 1, .8, .2)
f2 = GHFilter (array([0]), array([0]), 1, .8, .2)
str(f1)
str(f2)
# test both give same answers, and that we can
# use a scalar for the measurment
for i in range(1,10):
f1.update(i)
f2.update(i)
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
# test using an array for the measurement
s1 = Saver(f1)
s2 = Saver(f2)
for i in range(1,10):
f1.update(i)
f2.update(array([i]))
s1.save()
s2.save()
assert f1.x == f2.x[0]
assert f1.dx == f2.dx[0]
assert f1.VRF() == f2.VRF()
s1.to_array()
s2.to_array()
def run_filter(list_args: List[float],
data: pd.DataFrame) -> Tuple[KalmanFilter, Saver]:
if isinstance(data, tuple):
# data is the first *args
data = data[0]
# store the parameters in the list_args
s_noise_Q00, r_noise_Q11, q_meas_error_R00, r_meas_error_R11 = list_args
kf = init_filter(
r0=0.0,
S0=5.74,
s_variance_P00=1,
r_variance_P11=100,
s_noise_Q00=s_noise_Q00,
r_noise_Q11=r_noise_Q11,
q_meas_error_R00=q_meas_error_R00,
r_meas_error_R11=r_meas_error_R11,
)
s = Saver(kf)
lls = []
for z in np.vstack([data["q_obs"], data["r_obs"]]).T:
kf.predict()
kf.update(z)
log_likelihood = kf.log_likelihood_of(z)
lls.append(log_likelihood)
s.save()
s.to_array()
lls = np.array(lls)
return kf, s, lls
def test_saver_kf():
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
s = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 10
assert len(s.y) == 10
assert len(s.K) == 10
assert (len(s) == len(s.x))
# Force an exception to occur my malforming K
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
kf.K = 0.
s2 = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s2.save()
# this should raise an exception because K is malformed
try:
s2.to_array()
assert False, "Should not have been able to convert s.K into an array"
except:
pass
def test_save_properties():
global f, s
class Foo(object):
aa = 3
def __init__(self):
self.x = 7.
self.a = None
@property
def ll(self):
self.a = Foo.aa
Foo.aa += 1
return self.a
f = Foo()
assert f.a is None
s = Saver(f)
s.save() # try to trigger property writing to Foo.a
assert s.a[0] == f.a
assert s.ll[0] == f.a
assert f.a == 3
s.save()
assert s.a[1] == f.a
assert s.ll[1] == f.a
assert f.a == 4
def test_saver_UKF():
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.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i, i] for i in range(40)]
s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean'])
for z in zs:
kf.predict()
kf.update(z)
#print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
s.to_array()
def test_save_properties():
global f, s
class Foo(object):
aa = 3
def __init__(self):
self.x = 7.
self.a = None
@property
def ll(self):
self.a = Foo.aa
Foo.aa += 1
return self.a
f = Foo()
assert f.a is None
s = Saver(f)
s.save() # try to trigger property writing to Foo.a
assert s.a[0] == f.a
assert s.ll[0] == f.a
assert f.a == 3
s.save()
assert s.a[1] == f.a
assert s.ll[1] == f.a
assert f.a == 4
def test_saver_UKF():
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.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i, i] for i in range(40)]
s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean'])
for z in zs:
kf.predict()
kf.update(z)
#print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
s.to_array()
def test_saver_kf():
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
s = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 10
assert len(s.y) == 10
assert len(s.K) == 10
assert (len(s) == len(s.x))
# Force an exception to occur my malforming K
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
kf.K = 0.
s2 = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s2.save()
# this should raise an exception because K is malformed
try:
s2.to_array()
assert False, "Should not have been able to convert s.K into an array"
except:
pass
def test_1d():
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)
print(inf.SI)
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 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 test_MMAE2():
dt = 0.1
pos, zs = generate_data(120, noise_factor=0.6)
z_xs = zs[:, 0]
dt = 0.1
ca = make_ca_filter(dt, noise_factor=0.6)
cv = make_ca_filter(dt, noise_factor=0.6)
cv.F[:, 2] = 0 # remove acceleration term
cv.P[2, 2] = 0
cv.Q[2, 2] = 0
filters = [cv, ca]
bank = MMAEFilterBank(filters, (0.5, 0.5), dim_x=3, H=ca.H)
xs, probs = [], []
cvxs, caxs = [], []
s = Saver(bank)
for i, z in enumerate(z_xs):
bank.predict()
bank.update(z)
xs.append(bank.x[0])
cvxs.append(cv.x[0])
caxs.append(ca.x[0])
print(i, cv.likelihood, ca.likelihood, bank.p)
s.save()
probs.append(bank.p[0] / bank.p[1])
s.to_array()
if DO_PLOT:
plt.subplot(121)
plt.plot(xs)
plt.plot(pos[:, 0])
plt.subplot(122)
plt.plot(probs)
plt.title('probability ratio p(cv)/p(ca)')
plt.figure()
plt.plot(cvxs, label='CV')
plt.plot(caxs, label='CA')
plt.plot(pos[:, 0])
plt.legend()
plt.figure()
plt.plot(xs)
plt.plot(pos[:, 0])
return bank
def test_MMAE2():
dt = 0.1
pos, zs = generate_data(120, noise_factor=0.6)
z_xs = zs[:, 0]
dt = 0.1
ca = make_ca_filter(dt, noise_factor=0.6)
cv = make_ca_filter(dt, noise_factor=0.6)
cv.F[:, 2] = 0 # remove acceleration term
cv.P[2, 2] = 0
cv.Q[2, 2] = 0
filters = [cv, ca]
bank = MMAEFilterBank(filters, (0.5, 0.5), dim_x=3, H=ca.H)
xs, probs = [], []
cvxs, caxs = [], []
s = Saver(bank)
for i, z in enumerate(z_xs):
bank.predict()
bank.update(z)
xs.append(bank.x[0])
cvxs.append(cv.x[0])
caxs.append(ca.x[0])
print(i, cv.likelihood, ca.likelihood, bank.p)
s.save()
probs.append(bank.p[0] / bank.p[1])
s.to_array()
if DO_PLOT:
plt.subplot(121)
plt.plot(xs)
plt.plot(pos[:, 0])
plt.subplot(122)
plt.plot(probs)
plt.title('probability ratio p(cv)/p(ca)')
plt.figure()
plt.plot(cvxs, label='CV')
plt.plot(caxs, label='CA')
plt.plot(pos[:, 0])
plt.legend()
plt.figure()
plt.plot(xs)
plt.plot(pos[:, 0])
return bank
def test_1d_const_vel():
def hx(x):
return np.array([x[0]])
F = np.array([[1., 1.],[0., 1.]])
def fx(x, dt):
return np.dot(F, x)
x = np.array([0., 1.])
P = np.eye(2)* 100.
f = EnKF(x=x, P=P, dim_z=1, dt=1., N=8, hx=hx, fx=fx)
std_noise = 10.
f.R *= std_noise**2
f.Q = Q_discrete_white_noise(2, 1., .001)
measurements = []
results = []
ps = []
zs = []
s = Saver(f)
for t in range (0,100):
# create measurement = t plus white noise
z = t + randn()*std_noise
zs.append(z)
f.predict()
f.update(np.asarray([z]))
# save data
results.append (f.x[0])
measurements.append(z)
ps.append(3*(f.P[0,0]**.5))
s.save()
s.to_array()
results = np.asarray(results)
ps = np.asarray(ps)
if DO_PLOT:
plt.plot(results, label='EnKF')
plt.plot(measurements, c='r', label='z')
plt.plot (results-ps, c='k',linestyle='--', label='3$\sigma$')
plt.plot(results+ps, c='k', linestyle='--')
plt.legend(loc='best')
#print(ps)
return f
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_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_saver_ekf():
def HJ(x):
return np.array([[1, 1]])
def hx(x):
return np.array([x[0]])
kf = ExtendedKalmanFilter(2, 1)
s = Saver(kf)
for i in range(3):
kf.predict()
kf.update([[i]], HJ, hx)
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 3
assert len(s.y) == 3
assert len(s.K) == 3
def test_saver_ekf():
def HJ(x):
return np.array([[1, 1]])
def hx(x):
return np.array([x[0]])
kf = ExtendedKalmanFilter(2, 1)
s = Saver(kf)
for i in range(3):
kf.predict()
kf.update([[i]], HJ, hx)
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 3
assert len(s.y) == 3
assert len(s.K) == 3
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]])
#xx, pp, Sx = predict(f, x, P, Q)
#x, P = update(h, z, xx, pp, R)
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()
s.to_array()
def smooth(self, t, z):
dt = t[1:] - t[:-1]
if np.any(dt < 0):
raise ValueError('Times must be sorted in accedning order.')
t = t.copy()
z = z.copy()
if self.kf.x is None:
self.kf.x = self.init_x(z[0])
s = Saver(self.kf)
#s.save()
for i, ti in enumerate(t[1:]):
self.kf.predict(dt=dt[i])
self.kf.update(z[i + 1], epoch=ti)
s.save()
s.to_array()
xs, _, _ = self.kf.rts_smoother(s.x, s.P)
return xs[:, :6]
R=R,
)
s = Saver(kf)
# ------ RUN FILTER ------
if observe_every > 1:
data.loc[data.index % observe_every != 0, "q_obs"] = np.nan
# Iterate over the Kalman Filter
for time_ix, z in enumerate(np.vstack([data["q_obs"], data["r_obs"]]).T):
kf.predict()
# only make update steps every n timesteps
if time_ix % observe_every == 0:
kf.update(z)
s.save()
s.to_array()
# only observe every n values
# data["q_true_original"] = data["q_true"]
# update data with POSTERIOR estimates
# Calculate the DISCHARGE (measurement operator * \bar{x})
data["q_filtered"] = (s.H @ s.x)[:, 0]
data["q_variance"] = (s.H @ s.P)[:, 0, 0]
return kf, s, data
def calculate_r2_metrics(data):
def test_imm():
""" This test is drawn from Crassidis [1], example 4.6.
** References**
[1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press,
Second edition.
"""
r = 100.
dt = 1.
phi_sim = np.array(
[[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
gam = np.array([[dt**2/2, 0],
[dt, 0],
[0, dt**2/2],
[0, dt]])
x = np.array([[2000, 0, 10000, -15.]]).T
simxs = []
N = 600
for i in range(N):
x = np.dot(phi_sim, x)
if i >= 400:
x += np.dot(gam, np.array([[.075, .075]]).T)
simxs.append(x)
simxs = np.array(simxs)
zs = np.zeros((N, 2))
for i in range(len(zs)):
zs[i, 0] = simxs[i, 0] + randn()*r
zs[i, 1] = simxs[i, 2] + randn()*r
'''
try:
# data to test against crassidis' IMM matlab code
zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1]
zs = zs_tmp
except:
pass
'''
ca = KalmanFilter(6, 2)
cano = KalmanFilter(6, 2)
dt2 = (dt**2)/2
ca.F = np.array(
[[1, dt, dt2, 0, 0, 0],
[0, 1, dt, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, dt, dt2],
[0, 0, 0, 0, 1, dt],
[0, 0, 0, 0, 0, 1]])
cano.F = ca.F.copy()
ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T
cano.x = ca.x.copy()
ca.P *= 1.e-12
cano.P *= 1.e-12
ca.R *= r**2
cano.R *= r**2
cano.Q *= 0
q = np.array([[.05, .125, 1./6],
[.125, 1/3, .5],
[1./6, .5, 1.]])*1.e-3
ca.Q[0:3, 0:3] = q
ca.Q[3:6, 3:6] = q
ca.H = np.array([[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
cano.H = ca.H.copy()
filters = [ca, cano]
trans = np.array([[0.97, 0.03],
[0.03, 0.97]])
bank = IMMEstimator(filters, (0.5, 0.5), trans)
# ensure __repr__ doesn't have problems
str(bank)
s = Saver(bank)
ca_s = Saver(ca)
cano_s = Saver(cano)
for i, z in enumerate(zs):
z = np.array([z]).T
bank.update(z)
bank.predict()
s.save()
ca_s.save()
cano_s.save()
if DO_PLOT:
s.to_array()
ca_s.to_array()
cano_s.to_array()
plt.figure()
plt.subplot(121)
plt.plot(s.x[:, 0], s.x[:, 3], 'k')
#plt.plot(cvxs[:, 0], caxs[:, 3])
#plt.plot(simxs[:, 0], simxs[:, 2], 'g')
plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2)
plt.subplot(122)
plt.plot(s.mu[:, 0])
plt.plot(s.mu[:, 1])
plt.ylim(0, 1)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
def run_ukf(
data: pd.DataFrame,
S0_est: float,
s_variance: float,
r_variance: Optional[float],
Q00_s_noise: float,
Q11_r_noise: Optional[float],
R: float,
params: Dict[str, float],
alpha: float,
beta: float,
kappa: float,
dimension: int = 2,
observe_every: int = 1,
) -> Tuple[UKF.UnscentedKalmanFilter, pd.DataFrame, Saver]:
# --- INIT FILTER --- #
if dimension == 1:
ukf = init_filter(
S0=S0_est,
s_variance=s_variance,
Q00_s_noise=Q00_s_noise,
R=R,
h=hx,
f=abc,
params=params, # dict(a=a_est, b=b_est, c=c_est)
alpha=alpha,
beta=beta,
kappa=kappa,
)
elif dimension == 2:
ukf = init_2D_filter(
S0=S0_est,
r0=data["r_obs"][0],
s_variance=s_variance,
r_variance=r_variance,
Q00_s_noise=Q00_s_noise,
Q11_r_noise=Q11_r_noise,
R=R,
params=params,
alpha=alpha,
beta=beta,
kappa=kappa,
)
s = Saver(ukf)
# --- RUN FILTER --- #
for time_ix, (q, r) in enumerate(np.vstack([data["q_obs"], data["r_obs"]]).T):
# NOTE: z is the discharge (q)
if dimension == 1:
# TODO: how include measurement accuracy of rainfall ?
fx_args = hx_args = {"r": r} # rainfall inputs to the model
# predict
ukf.predict(**fx_args)
# update
if time_ix % observe_every == 0:
ukf.update(q, **hx_args)
elif dimension == 2:
z2d = np.array([q, r])
ukf.predict()
if time_ix % observe_every == 0:
ukf.update(z2d)
else:
assert False, "Have only implemented [1, 2] dimensions"
s.save()
# save the output data
s.to_array()
data_ukf = update_data_columns(data.copy(), s, dimension=dimension)
return ukf, data_ukf, s
def test_imm():
""" this test is drawn from Crassidis [1], example 4.6.
** References**
[1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press,
Second edition.
"""
r = 1.
dt = 1.
phi_sim = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
gam = np.array([[dt**2 / 2, 0], [dt, 0], [0, dt**2 / 2], [0, dt]])
x = np.array([[2000, 0, 10000, -15.]]).T
simxs = []
N = 600
for i in range(N):
x = np.dot(phi_sim, x)
if i >= 400:
x += np.dot(gam, np.array([[.075, .075]]).T)
simxs.append(x)
simxs = np.array(simxs)
#x = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/x.csv', delimiter=',')
zs = np.zeros((N, 2))
for i in range(len(zs)):
zs[i, 0] = simxs[i, 0] + randn() * r
zs[i, 1] = simxs[i, 2] + randn() * r
try:
#data to test against crassidis' IMM matlab code
zs_tmp = np.genfromtxt(
'c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv',
delimiter=',')[:-1]
zs = zs_tmp
except:
pass
ca = KalmanFilter(6, 2)
cano = KalmanFilter(6, 2)
dt2 = (dt**2) / 2
ca.F = np.array([[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0],
[0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2],
[0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]])
cano.F = ca.F.copy()
ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T
cano.x = ca.x.copy()
ca.P *= 1.e-12
cano.P *= 1.e-12
ca.R *= r**2
cano.R *= r**2
cano.Q *= 0
q = np.array([[.05, .125, 1 / 6], [.125, 1 / 3, .5], [1 / 6, .5, 1]
]) * 1.e-3
ca.Q[0:3, 0:3] = q
ca.Q[3:6, 3:6] = q
ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
cano.H = ca.H.copy()
filters = [ca, cano]
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
bank = IMMEstimator(filters, (0.5, 0.5), trans)
# ensure __repr__ doesn't have problems
str(bank)
xs, probs = [], []
cvxs, caxs = [], []
s = Saver(bank)
for i, z in enumerate(zs[0:10]):
z = np.array([z]).T
bank.update(z)
#print(ca.likelihood, cano.likelihood)
#print(ca.x.T)
xs.append(bank.x.copy())
cvxs.append(ca.x.copy())
caxs.append(cano.x.copy())
#print(i, ca.likelihood, cano.likelihood, bank.w)
#print('p', bank.p)
probs.append(bank.mu.copy())
s.save()
s.to_array()
if DO_PLOT:
xs = np.array(xs)
cvxs = np.array(cvxs)
caxs = np.array(caxs)
probs = np.array(probs)
plt.subplot(121)
plt.plot(xs[:, 0], xs[:, 3], 'k')
#plt.plot(cvxs[:, 0], caxs[:, 3])
#plt.plot(simxs[:, 0], simxs[:, 2], 'g')
plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2)
plt.subplot(122)
plt.plot(probs[:, 0])
plt.plot(probs[:, 1])
plt.ylim(-1.5, 1.5)
plt.title('probability ratio p(cv)/p(ca)')
'''plt.figure()
plt.plot(cvxs, label='CV')
plt.plot(caxs, label='CA')
plt.plot(xs[:, 0], label='GT')
plt.legend()
plt.figure()
plt.plot(xs)
plt.plot(xs[:, 0])'''
return bank
def test_linear_rts():
""" for a linear model the Kalman filter and UKF should produce nearly
identical results.
Test code mostly due to user gboehl as reported in GitHub issue #97, though
I converted it from an AR(1) process to constant velocity kinematic
model.
"""
dt = 1.0
F = np.array([[1., dt], [.0, 1]])
H = np.array([[1., .0]])
def t_func(x, dt):
F = np.array([[1., dt], [.0, 1]])
return np.dot(F, x)
def o_func(x):
return np.dot(H, x)
sig_t = .1 # peocess
sig_o = .00000001 # measurement
N = 50
X_true, X_obs = [], []
for i in range(N):
X_true.append([i + 1, 1.])
X_obs.append(i + 1 + np.random.normal(scale=sig_o))
X_true = np.array(X_true)
X_obs = np.array(X_obs)
oc = np.ones((1, 1)) * sig_o**2
tc = np.zeros((2, 2))
tc[1, 1] = sig_t**2
tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2)
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1)
ukf = UKF(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points)
ukf.x = np.array([0., 1.])
ukf.R = oc[:]
ukf.Q = tc[:]
s = Saver(ukf)
s.save()
s.to_array()
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([[0., 1]]).T
kf.R = oc[:]
kf.Q = tc[:]
kf.H = H[:]
kf.F = F[:]
mu_ukf, cov_ukf = ukf.batch_filter(X_obs)
x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf)
mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs)
x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf)
# check results of filtering are correct
kfx = mu_kf[:, 0, 0]
ukfx = mu_ukf[:, 0]
kfxx = mu_kf[:, 1, 0]
ukfxx = mu_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
# error in position should be smaller then error in velocity, hence
# atol is different for the two tests.
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
# now ensure the RTS smoothers gave nearly identical results
kfx = x_kf[:, 0, 0]
ukfx = x_ukf[:, 0]
kfxx = x_kf[:, 1, 0]
ukfxx = x_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
return ukf
#------------------------------ Use filterpy KF-----------------------------
f = KalmanFilter(dim_x=2, dim_z=1)
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.array([[1., 0.], [0., 1.]])
f.R = np.array([[sigma**2]]) # uwb dis std **2
saver_kf = Saver(f)
for i in range(len(All_path)):
if i == 0:
# dt = dut_t[0]
dt = 0.1
f.x = np.array([z_newton_ls[i], 0]) # position,velocity
f.F = np.array([[1, dt], [0, 1]])
f.predict()
saver_kf.save()
continue
# f.Q = Q_discrete_white_noise(2, dt=dt,
# var=2e-5, block_size=2)
dt = 0.1
f.Q = Q_discrete_white_noise(dim=2, dt=1, var=1e-4)
f.update(z=z_newton_ls[i])
f.predict(F=np.array([[1, dt], [0, 1]]))
saver_kf.save()
z_newton_ls_kf = np.array(saver_kf.x)[:, 0]
for m in z_newton_ls_kf:
pos_predictions_error = np.linalg.norm(m - tag[2])
PY_KF_z_error.append(round(pos_predictions_error, 2))
#------------------------------ Use filterpy UKF-----------------------------
import matplotlib.pyplot as plt
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise, Saver
r_std, q_std = 2., 0.003
cv = KalmanFilter(dim_x=2, dim_z=1)
cv.x = np.array([[0., 1.]]) # position, velocity
cv.F = np.array([[1, dt],[0, 1]])
cv.R = np.array([[r_std*r_std]])
f.H = np.array([[1., 0.]])
f.P = np.diag([0.1*0.1, 0.03*0.03])
f.Q = Q_discrete_white_noise(2, dt, q_std**2)
saver = Saver(cv)
for z in range(100):
cv.predict()
cv.update([z + randn() * r_std])
saver.save() # save the filter's state
saver.to_array()
plt.plot(saver.x[:, 0])
# plot all of the priors
plt.plot(saver.x_prior[:, 0])
# plot mahalanobis distance
plt.figure()
plt.plot(saver.mahalanobis)
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
fsq = SquareRootKalmanFilter (dim_x=2, dim_z=1)
fsq.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
fsq.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
fsq.H = np.array([[1.,0.]]) # Measurement function
fsq.P = np.eye(2) * 1000. # covariance matrix
fsq.R *= 5 # state uncertainty
fsq.Q *= 0.0001 # process uncertainty
# does __repr__ work?
str(fsq)
measurements = []
results = []
zs = []
s = Saver(fsq)
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.e-12
assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-12
# save data
results.append (f.x[0,0])
measurements.append(z)
s.save()
s.to_array()
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]]).T
f.P = np.eye(2)*100.
m,c,_,_ = f.batch_filter(zs,update_first=False)
# 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()
class KalmanFilterCloseModel(BaseModel):
def __init__(self):
# Create some assets:
assetsList = [Asset('WS30', 'traditional', 'historical'), # Index US
Asset('XAUUSD', 'traditional', 'historical'), # Commodity
Asset('GDAXIm', 'traditional', 'historical'), # Index EUR
Asset('EURUSD', 'traditional', 'historical'), # Major
Asset('GBPJPY', 'traditional', 'historical')] # Minor
# Initialize the ResearchStudy class:
super().__init__('KalmanFilterCloseModel', assetsList)
# Make a random seed to reproduce results:
np.random.seed(33)
# Print to see if working:
logger.warning(self.PORTFOLIO._portfolioDict['WS30'])
def _defineModelParameters(self):
'''1) It is very important to set the intial values correctly; the KF will rapidly move onto the real ones, but maybe we don't see them in the plots
because of that.
2) The transition covariance is also very important. Depending on that value, the transition between states will allow to have more variance or less.
We see that in the returns plot. If we don't increase that value, we see that we don't exactly approximate the quantities of returns. If we increase it, it will approximate more.'''
# Use the normal Kalman Filter
self.kalmanFilter = KalmanFilter(dim_x=1, dim_z=1)
# We will specify the parameters like the covariance matrices by hand.
self.kalmanFilter.x = np.array([0.0], dtype=float)
logger.warning('###########################################')
logger.warning(f'X: {self.kalmanFilter.x}')
logger.warning(f'X shape: {self.kalmanFilter.x.shape}')
logger.warning('###########################################')
self.kalmanFilter.P = np.array([[1.0]], dtype=float)
logger.warning(f'P: {self.kalmanFilter.P}')
logger.warning(f'P shape: {self.kalmanFilter.P.shape}')
logger.warning('###########################################')
self.kalmanFilter.R = np.array([[0.001]], dtype=float)
logger.warning(f'R: {self.kalmanFilter.R}')
logger.warning(f'R shape: {self.kalmanFilter.R.shape}')
logger.warning('###########################################')
self.kalmanFilter.F = np.array([[1.0]], dtype=float)
logger.warning(f'F: {self.kalmanFilter.F}')
logger.warning(f'F shape: {self.kalmanFilter.F.shape}')
logger.warning('###########################################')
self.kalmanFilter.H = np.array([[1.0]], dtype=float)
logger.warning(f'H: {self.kalmanFilter.H}')
logger.warning(f'H shape: {self.kalmanFilter.H.shape}')
logger.warning('###########################################')
self.kalmanFilter.Q = np.array([[1.e-05]], dtype=float)
logger.warning(f'Q: {self.kalmanFilter.Q}')
logger.warning(f'Q shape: {self.kalmanFilter.Q.shape}')
logger.warning('###########################################')
def _fitTheModel(self, saveDirectory):
'''Apply the Kalman Filter to estimate the hidden state at time t for t = [0...ntimesteps-1] given observations up to and including time t'''
# Initialize the parameters:
self._defineModelParameters()
# Create Saver object
self.saverObject = Saver(self.kalmanFilter)
# Loop the portfolio dict:
for eachAssetName, eachAssetDataFrame in self.PORTFOLIO._portfolioDict.items():
# Create a container for the looping:
self.means = []
# Re-initialize the parameters:
self._defineModelParameters()
# Loop each dataframe row:
for eachIndex, eachRow in eachAssetDataFrame.iterrows():
# Just perform the standard predict/update loop
self.kalmanFilter.predict()
self.kalmanFilter.update(eachRow['close'])
# Append to list:
self.means.append(self.kalmanFilter.x[0])
# Create the new column in the dataframe:
eachAssetDataFrame['KFValue'] = self.means
# NOTE: Create the slope of the KF:
eachAssetDataFrame['KFValue_Slope'] = eachAssetDataFrame['KFValue'].pct_change()
eachAssetDataFrame['KFValue_Binary'] = (eachAssetDataFrame['KFValue_Slope'] > 0).astype(int)
eachAssetDataFrame['KFValue_PlotCol'] = eachAssetDataFrame['KFValue']
#print(eachAssetDataFrame['KFValue_Slope'].head())
#print(eachAssetDataFrame['KFValue_Binary'].head())
# NOTE: Create a moving average to see the difference:
# SMA
eachAssetDataFrame['MA'] = eachAssetDataFrame['close'].rolling(window=25).mean()
# WMA
#eachAssetDataFrame['MA'] = tb.WMA(eachAssetDataFrame['close'].values, timeperiod=23)
# EMA
#eachAssetDataFrame['MA'] = tb.EMA(eachAssetDataFrame['close'].values, timeperiod=23)
eachAssetDataFrame.dropna(how='any', inplace=True)
# Save the filter values for further reference:
self._saveModel(eachAssetName, saveDirectory)
# Print:
logger.warning(self.PORTFOLIO._portfolioDict['WS30'])
def _saveModel(self, eachAssetName, saveDirectory):
# Save parameters in memory and to file:
self.saverObject.save()
with open(saveDirectory + f'/KFCloseParams_{eachAssetName}.pickle', 'wb') as file_path:
pickle.dump(dict(self.saverObject._DL), file_path)
def _plotModelOutput(self, saveDirectory='', showIt=False):
# Plot:
# Create the colormap:
colormap = cm.get_cmap('cool')
for eachAssetName, eachAssetDataFrame in self.PORTFOLIO._portfolioDict.items():
logger.warning(f'[{self._plotModelOutput.__name__}] - Looping for asset <{eachAssetName}>...')
# We will just get part of the dataframe for the plot:
eachAssetDataFrame_Little = eachAssetDataFrame[:100].copy()
eachAssetDataFrame_Little['date'] = range(1, len(eachAssetDataFrame_Little) + 1)
logger.warning(eachAssetDataFrame_Little.head())
# Create the figure:
f1, ax = plt.subplots(figsize = (10,5))
# Create the plots:
ax.plot(eachAssetDataFrame_Little.date, eachAssetDataFrame_Little.close, label='Close')
ax.plot(eachAssetDataFrame_Little.date, eachAssetDataFrame_Little.KFValue, label='KFValue')
ax.plot(eachAssetDataFrame_Little.date, eachAssetDataFrame_Little.MA, label='SMA')
ax.scatter(eachAssetDataFrame_Little.date, eachAssetDataFrame_Little.KFValue_PlotCol,
#c=eachAssetDataFrame_Little.KFValue_Slope,
c=eachAssetDataFrame_Little.KFValue_Binary,
cmap=colormap,
label='KF Slope',
s=80)
ax.grid(True)
plt.grid(linestyle='dotted')
plt.xlabel('Observations', horizontalalignment='center', verticalalignment='center', fontsize=14, labelpad=20)
plt.ylabel('Values', horizontalalignment='center', verticalalignment='center', fontsize=14, labelpad=20)
ax.legend(loc='best')
plt.title(f'Close and Kalman Filter value for asset <{eachAssetName}>')
plt.subplots_adjust(left=0.09, bottom=0.20, right=0.94, top=0.90, wspace=0.2, hspace=0)
f1.canvas.set_window_title('Kalman Filtering Plot')
# In PNG:
plt.savefig(saveDirectory + f'/KFClose_{eachAssetName}.png')
# Show it:
if showIt:
plt.show()
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)
