Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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 = []
    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))

    results = np.asarray(results)
    ps = np.asarray(ps)
Ejemplo n.º 3
0
std_noise = 1

#初值
x = np.array([state[0]])

#状态协方差矩阵初始化
P = np.eye(1)

#ensemble kalman filter
enkf = EnKF(x=x, P=P, dim_z=1, dt=0.1, N=5, hx=hx, fx=fx)

#观测噪声
enkf.R *= std_noise**2

#系统噪声
enkf.Q = np.eye(1) * 1
filter_result = []

#方便写fx
global time
time = 0

#运行
for i in range(0, state.shape[0]):
    m = measurements[i]
    enkf.predict()
    enkf.update(np.asarray([m]))
    filter_result.append(enkf.x)
    time = time + 1

filter_result = [x[0] for x in filter_result]
xxa = X.copy()
xa = np.array([xxa[:, i] for i in range(0, xxa.shape[1])]).flatten()
yya = Y.copy()
ya = np.array([yya[:, i] for i in range(0, yya.shape[1])]).flatten()

#  Kalman filter
for i in range(1, npd):
    F = np.array([[1., 0.], [0., 1.]])
    zz = fval[:][i]
    x = np.array([1., 0.])
    P = np.eye(2) * 100.
    enf = EnKF(x=x, P=P, dim_z=1, dt=1., N=5, hx=hx, fx=fx)

    std_noise = 1e-6
    enf.R *= std_noise**2
    enf.Q = Q_discrete_white_noise(2, std_noise, std_noise)

    results = np.zeros((1, tt)).T
    for t in range(0, tt):
        # create measurement = t plus white noise
        z = zz[t] + randn() * std_noise

        enf.predict()
        enf.update(np.asarray([z]))
        # save data
        if zz[t] > 1e-12:
            results[t] = enf.x[0]
        else:
            results[t] = zz[t]

    results = results.flatten()