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)