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',
#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 = []
for i in range(timeSpan+1):
time.append(i)
# Write to csv file
with open(csvFileName, 'w') as csvFile:
table = csv.writer(csvFile)
table.writerow(time)
table.writerow(prediction1)
table.writerow(prediction2)
#Plot the graph
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()
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)]
