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) results = np.asarray(results) measurements = np.asarray(measurements)
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')
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 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)
def kalman(): filter = KalmanFilter(dim_x=2, dim_z=1) x = array([0., 1.]) P = eye(2) * 100. enf = EnsembleKalmanFilter(x=x, P=P, dim_z=1, dt=1., N=20, hx=hx, fx=fx) measurements = [] results = [] ps = [] kf_results = [] for i in range(len(data)): z = data.iloc[i].as_array() enf.predict() enf.update(asarray([z])) filter.predict() filter.update(asarray([[z]])) results.append (enf.x[0]) kf_results.append (kf.x[0,0]) measurements.append(z) ps.append(3*(enf.P[0,0]**.5)) results = asarray(results) ps = asarray(ps)
#观测噪声 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] plt.figure() plt.plot(np.arange(380, 383.5, 0.1), true_x, marker='s', label='True', color='r') plt.plot(np.arange(380, 383.5, 0.1), filter_result, marker='s', label='EnKF+Takens', color='b')
measurement = measurementListTCal[k] measurementArray = np.array(measurement) measurementArray = measurementArray + randn() * std_noise_R if dim_z == 1: # seems useless. dim_z ==1 only happens when working with 1D EnKF measurementSensor = measurementArray[depthOfSensor] elif dim_z == nodesInZ * numberOfGrids: measurementSensor = measurementArray else: l = 0 measurementSensor = np.ones(dim_z) for index, item in enumerate(H): if item != 0: measurementSensor[l] = measurementArray[index] l += 1 measurementSensorList.append(measurementSensor) ThetaU = f.update(ThetaListAll, measurementSensor, H) # ThetaUR = [] # for i in range(len(ThetaU)): # ThetaU1 = ThetaU[i]#[::-1] # ThetaUR.append(ThetaU1) ThetaMeanU = np.mean(ThetaU, axis=0) ThetaListMeanU.append(ThetaMeanU) # depthList depthList = [] for i in range(0, depth * nodesInZ): depthList.append(-1 * i * dz) xTrue1 = ones(nodesInZ) * -0.2 xTrue5 = ones(nodesInZ) * -0.8 xF = ones(nodesInZ) * -0.5
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() PP[i, :] = results #FIT SVM grid = { "C": [10**e for e in range(-1, 7)], "gamma": [10**e for e in range(-7, -2)], "epsilon": [10**e for e in range(-2, -1)] }