Beispiel #1
0
def test_circle():
    def hx(x):
        return np.array([x[0], x[3]])

    F = np.array([[1., 1., .5, 0., 0., 0.],
                  [0., 1., 1., 0., 0., 0.],
                  [0., 0., 1., 0., 0., 0.],
                  [0., 0., 0., 1., 1., .5],
                  [0., 0., 0., 0., 1., 1.],
                  [0., 0., 0., 0., 0., 1.]])

    def fx(x, dt):
        return np.dot(F, x)

    x = np.array([50., 0., 0, 0, .0, 0.])
    P = np.eye(6)* 100.
    f = EnKF(x=x, P=P, dim_z=2, dt=1., N=30, hx=hx, fx=fx)

    std_noise = .1

    f.R *= std_noise**2
    f.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .001)
    f.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .001)

    measurements = []
    results = []

    zs = []
    for t in range (0,300):
        a = t / 300000
        x = cos(a) * 50.
        y = sin(a) * 50.
        # create measurement = t plus white noise
        z = np.array([x,y])
        zs.append(z)

        f.predict()
        f.update(z)

        # save data
        results.append (f.x)
        measurements.append(z)

    #test that __repr__ doesn't assert
    str(f)

    results = np.asarray(results)
    measurements = np.asarray(measurements)

    if DO_PLOT:
        plt.plot(results[:,0], results[:,2], label='EnKF')
        plt.plot(measurements[:,0], measurements[:,1], 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')
        plt.axis('equal')
Beispiel #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 = []
    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
#模型
def fx(x, dt):
    return state[time]


#观测噪声的标准差
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]
Beispiel #4
0
dim_z = 0
for element in H:
    if element != 0:
        dim_z += 1
##
##
##

# Define EnKF
#xTrue = np.array(h_iTrue)
#xTrue1 = np.array(h_iTrue1)
x = np.array(h_i)
u = np.array(u_i)
P = np.eye(nodesInZ * numberOfGrids) * stdState * stdState

f = EnKF(x=x, u=u, P=P, dim_z=dim_z, N=N)
f.R *= std_noise_R**2
f.Q *= std_noise_Q**2
ensemble = f.initialize(x=x, P=P)

# Generating lists for data collection and for plots
thetaList = []
ThetaListOne = []
ThetaListAll = []

ThetaU = []
ThetaUR = []
ThetaListMeanU = []
measurementListTPlot = []
measurementListTCal = []
measurementListF = []
Beispiel #5
0
#    x1 = x[0]
#    x2 = x[1]
#    if x1 < 0:  # Water levels in both tanks cannot be negative
#        x1 = 0
#    if x2 < 0:
#        x2 = 0
#    x1_p = ((-dt*cv1)/A)*math.sqrt(x1) + x1 + (dt/A)*u + randn()*processNoise
#    x2_p = ((dt*cv1)/A)*math.sqrt(x1) - ((dt*cv2)/A)*math.sqrt(x2) + x2 + randn()*processNoise
#    return [x1_p, x2_p]


# Define EnKF
#x = np.array(h_i)
#u = np.array(qi_i)
#P = np.eye(2) * stateCov
f = EnKF(x=x, u=u, P=P, dim_z=dim_z, dt=dt, N=N,
         hx=hx, fx=fx)

f.R *= std_noise ** 2
f.Q *= std_noise ** 2

# Generating lists for data collection and for plots
prediction1 = [x[0]]
prediction2 = [x[1]]

# Prediction using the model
for i in range(timeSpan):
    x_p = f.predict()
    prediction1.append(x_p[0])
    prediction2.append(x_p[1])

time = []
xx = (np.linspace(0, n - 1, n))
yy = (np.linspace(0, m - 1, m))
X, Y = np.meshgrid(xx, yy)
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]