def test_batch_equals_recursive(): """ ensures that the batch filter and the recursive version both produce the same results. """ N = 4 # size of lag fls = FixedLagSmoother(dim_x=2, dim_z=1, N=N) fls.x = np.array([0., .5]) fls.F = np.array([[1., 1.], [0., 1.]]) fls.H = np.array([[1., 0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 nom = np.array([t / 2. for t in range(0, 40)]) zs = np.array([t + random.randn() * 1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) for k, z in enumerate(zs): fls.smooth(z) xSmooth = np.asarray(fls.xSmooth) xfl = xs[:, 0].T[0] res = xSmooth.T[0, 0] - xfl assert np.sum(res) < 1.e-12
def one_run_test_fls(): fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([0., .5]) kf.F = np.array([[1.,1.], [0.,1.]]) kf.H = np.array([[1.,0.]]) kf.P *= 2000 kf.R *= 1. kf.Q *= 0.001 N = 4 # size of lag nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) M, P, _, _ = kf.batch_filter(zs) rts_x, _, _, _ = kf.rts_smoother(M, P) xfl = xs[:,0].T[0] xkf = M[:,0].T[0] fl_res = abs(xfl-nom) kf_res = abs(xkf-nom) if DO_PLOT: plt.cla() plt.plot(zs,'o', alpha=0.5, marker='o', label='zs') plt.plot(x[:,0], label='FLS') plt.plot(xfl, label='FLS S') plt.plot(xkf, label='KF') plt.plot(rts_x[:,0], label='RTS') plt.legend(loc=4) plt.show() print(fl_res) print(kf_res) print('std fixed lag:', np.mean(fl_res[N:])) print('std kalman:', np.mean(kf_res[N:])) return np.mean(fl_res) <= np.mean(kf_res)
def one_run_test_fls(): fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([0., .5]) kf.F = np.array([[1.,1.], [0.,1.]]) kf.H = np.array([[1.,0.]]) kf.P *= 2000 kf.R *= 1. kf.Q *= 0.001 N = 4 # size of lag nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) M, P, *_ = kf.batch_filter(zs) rts_x, *_ = kf.rts_smoother(M, P) xfl = xs[:,0].T[0] xkf = M[:,0].T[0] fl_res = abs(xfl-nom) kf_res = abs(xkf-nom) if DO_PLOT: plt.cla() plt.plot(zs,'o', alpha=0.5, marker='o', label='zs') plt.plot(x[:,0], label='FLS') plt.plot(xfl, label='FLS S') plt.plot(xkf, label='KF') plt.plot(rts_x[:,0], label='RTS') plt.legend(loc=4) plt.show() print(fl_res) print(kf_res) print('std fixed lag:', np.mean(fl_res[N:])) print('std kalman:', np.mean(kf_res[N:])) return np.mean(fl_res) <= np.mean(kf_res)
def one_run_test_fls(): fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([0., .5]) kf.F = np.array([[1.,1.], [0.,1.]]) kf.H = np.array([[1.,0.]]) kf.P *= 2000 kf.R *= 1. kf.Q *= 0.001 N = 4 # size of lag nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) M,P,_,_ = kf.batch_filter(zs) rts_x,_,_ = kf.rts_smoother(M, P) xfl = xs[:,0].T[0] xkf = M[:,0].T[0] fl_res = abs(xfl-nom) kf_res = abs(xkf-nom) return np.mean(fl_res) <= np.mean(kf_res)
def test_batch_equals_recursive(): """ ensures that the batch filter and the recursive version both produce the same results. """ N = 4 # size of lag fls = FixedLagSmoother(dim_x=2, dim_z=1, N=N) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) for k,z in enumerate(zs): fls.smooth(z) xSmooth = np.asarray(fls.xSmooth) xfl = xs[:,0].T[0] res = xSmooth.T[0,0] - xfl assert np.sum(res) < 1.e-12