def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
gh = GHFilter(x=0, dx=0, dt=1, g=0.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x + random.randn() for x in range(0, 100)]
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2 * (2 * i + 1) / ((i + 2) * (i + 1))
h = 6 / ((i + 2) * (i + 1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.0e-13)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x - y
assert r < 1.0e-8
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x + random.randn() for x in range(0, 100)]
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2 * (2 * i + 1) / ((i + 2) * (i + 1))
h = 6 / ((i + 2) * (i + 1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-13)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x - y
assert r < 1.e-8
def test_listing_3_4():
""" listing 3.4 in Zarchan, p. 117"""
lsf = LeastSquaresFilter(0.1, order=2)
xs = [5*x*x - x + 2 + 30*random.randn() for x in np.arange(0, 10, 0.1)]
ys = []
for x in xs:
ys.append(lsf.update(x)[0])
plt.plot(xs)
plt.plot(ys)
def test_first_order():
''' data and example from Zarchan, page 105-6'''
lsf = LeastSquaresFilter(dt=1, order=1)
xs = [1.2, .2, 2.9, 2.1]
ys = []
for x in xs:
ys.append(lsf.update(x)[0])
plt.plot(xs, c='b')
plt.plot(ys, c='g')
plt.plot([0, len(xs)-1], [ys[0], ys[-1]])
def test_fig_3_8():
""" figure 3.8 in Zarchan, p. 108"""
lsf = LeastSquaresFilter(0.1, order=1)
lsf0 = LeastSquaresFilterOriginal(0.1, order=1)
xs = [x + 3 + random.randn() for x in np.arange(0, 10, 0.1)]
ys = []
for x in xs:
y0 = lsf0(x)
y = lsf.update(x)[0]
assert near_equal(y, y0)
ys.append(y)
plt.plot(xs)
plt.plot(ys)
def test_second_order():
''' data and example from Zarchan, page 114'''
lsf = LeastSquaresFilter(1, order=2)
lsf0 = LeastSquaresFilterOriginal(1, order=2)
xs = [1.2, .2, 2.9, 2.1]
ys = []
for x in xs:
y = lsf.update(x)[0]
y0 = lsf0(x)
assert near_equal(y, y0)
ys.append(y)
plt.scatter(range(len(xs)), xs, c='r', marker='+')
plt.plot(ys, c='g')
plt.plot([0, len(xs)-1], [ys[0], ys[-1]], c='b')
def test_lsq():
""" implements alternative version of first order Least Squares filter
using g-h filter formulation and uses it to check the output of the
LeastSquaresFilter class."""
global lsq, lsq2, xs, lsq_xs
gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02)
lsq = LeastSquaresFilterOriginal(dt=1, order=1)
lsq2 = LeastSquaresFilter(dt=1, order=1)
zs = [x+random.randn()*10 for x in range(0, 10000)]
# test __repr__ at least doesn't crash
try:
str(lsq2)
except:
assert False, "LeastSquaresFilter.__repr__ exception"
xs = []
lsq_xs = []
for i, z in enumerate(zs):
g = 2*(2*i + 1) / ((i+2)*(i+1))
h = 6 / ((i+2)*(i+1))
x, dx = gh.update(z, g, h)
lx = lsq(z)
lsq_xs.append(lx)
x2 = lsq2.update(z)
assert near_equal(x2[0], lx, 1.e-10), '{}, {}, {}'.format(
i, x2[0], lx)
xs.append(x)
plt.plot(xs)
plt.plot(lsq_xs)
for x, y in zip(xs, lsq_xs):
r = x-y
assert r < 1.e-8
def test_big_data():
N = 1000000
xs = np.array([i+random.randn() for i in range(N)])
for order in [1, 2]:
lsq = LeastSquaresFilter(dt=1, order=order)
ys = np.array([lsq.update(x)[0] for x in xs])
delta = xs - ys
assert delta.max() < 6, delta.max()
assert delta.min() > -6, delta.min()
# zero order is special case, it can't adapt quickly to changing data
xs = np.array([random.randn() for i in range(N)])
lsq = LeastSquaresFilter(dt=1, order=0)
ys = np.array([lsq.update(x)[0] for x in xs])
delta = xs - ys
assert delta.max() < 6, delta.max()
assert delta.min() > -6, delta.min()
fl = LSQ(2)
fl.H = np.array([[1.0, 1.0], [0.0, 1.0]])
fl.R = np.eye(2)
fl.P = np.array([[2.0, 0.5], [0.5, 2.0]])
for x in range(10):
fl.update(np.array([[x], [x]], dtype=float))
plt.scatter(x, fl.x[0, 0])
fl = LSQ(1)
fl.H = np.eye(1)
fl.R = np.eye(1)
fl.P = np.eye(1)
lsf = LeastSquaresFilter(0.1, order=2)
random.seed(234)
for x in range(40):
z = x + random.randn() * 5
plt.scatter(x, z, c="r", marker="+")
fl.update(np.array([[z]], dtype=float))
plt.scatter(x, fl.x[0, 0], c="b")
y = lsf.update(z)[0]
plt.scatter(x, y, c="g", alpha=0.5)
plt.plot([0, 40], [0, 40])
fl = LSQ(2)
fl.H = np.array([[1., 1.], [0., 1.]])
fl.R = np.eye(2)
fl.P = np.array([[2., .5], [.5, 2.]])
for x in range(10):
fl.update(np.array([[x], [x]], dtype=float))
plt.scatter(x, fl.x[0, 0])
fl = LSQ(1)
fl.H = np.eye(1)
fl.R = np.eye(1)
fl.P = np.eye(1)
lsf = LeastSquaresFilter(0.1, order=2)
random.seed(234)
for x in range(40):
z = x + random.randn() * 5
plt.scatter(x, z, c='r', marker='+')
fl.update(np.array([[z]], dtype=float))
plt.scatter(x, fl.x[0, 0], c='b')
y = lsf.update(z)[0]
plt.scatter(x, y, c='g', alpha=0.5)
plt.plot([0, 40], [0, 40])
if __name__ == "__main__":
