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_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()
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
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__": pass # test_listing_3_4() # test_second_order() # fig_3_8() # test_second_order()
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__": pass #test_listing_3_4() #test_second_order() #fig_3_8() #test_second_order()