def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2],
[1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = UKF.weights(2, kappa)
sp1 = UKF.weights(2, kappa*1000)
Xi0 = UKF.sigma_points (x, P, kappa)
Xi1 = UKF.sigma_points (x, P, kappa*1000)
assert max(Xi1[:,0]) > max(Xi0[:,0])
assert max(Xi1[:,1]) > max(Xi0[:,1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i,0]-x[0, 0])*sp0[i] + x[0, 0],
(Xi0[i,1]-x[0, 1])*sp0[i] + x[0, 1],
color='blue')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0]-x[0, 0]) * sp1[i] + x[0,0],
(Xi1[i, 1]-x[0, 1]) * sp1[i] + x[0,1],
color='green')
stats.plot_covariance_ellipse([1, 2], P)
def test_batch_missing_data():
""" batch filter should accept missing data with None in the measurements """
def fx(x, dt):
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P*=0.0001
zs = []
for i in range(20):
z = np.array([i+randn()*0.1, i+randn()*0.1])
zs.append(z)
zs[2] = None
Rs = [1]*len(zs)
Rs[2] = None
Ms, Ps = kf.batch_filter(zs)
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
points = ukf.weights(1, 0.)
assert len(points) == 3
mean = 5
cov = 9
Xi = ukf.sigma_points (mean, cov, kappa)
xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x,w in zip(Xi, ukf.W):
m += x*w
assert abs(m-mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0,0]-cov) < 1.e-12
assert Xi.shape == (3,1)
assert len(ukf.W) == 3
def test_linear_2d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P*=0.0001
#kf.R *=0
#kf.Q
zs = []
for i in range(20):
z = np.array([i+randn()*0.1, i+randn()*0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
def sensor_fusion_kf():
global hx, fx
# create unscented Kalman filter with large initial uncertainty
points = JulierSigmaPoints(2, kappa=2)
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points)
kf.x = np.array([100, 100.])
kf.P *= 40
return kf
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=1.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal, hx=h_kal,
dt=dt, points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
def __init__(self, trueTrajectory, dt, Q=np.eye(4), R=np.eye(4)):
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.5, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_accel, hx=h_kal_accel,
dt=dt, points=sigmas, x_mean_fn = state_mean, residual_x=res_x,
residual_z=res_x)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
def run_localization(
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
sigma_bearing, ellipse_step=1, step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2,
sigma_bearing**2]*len(landmarks))
ukf.Q = np.eye(3)*0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1],
marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt/step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='k', alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn()*sigma_bearing))
z.extend([d, a])
ukf.update(z, hx_args=(landmarks,))
if i % ellipse_step == 0:
plot_covariance_ellipse(
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
facecolor='g', alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def __init__(self, trueTrajectory, dt, route, Q=np.eye(2), R=np.eye(2)):
#from filterpy.kalman import KalmanFilter as KF
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
n_state = len(Q)
n_meas = len(R)
sigmas = SigmaPoints(n_state, alpha=.1, beta=2., kappa=0.)
ukf = UKF(dim_x=n_state, dim_z=n_meas, fx=f_kal_v, hx=h_kal,
dt=dt, points=sigmas)
ukf.Q = Q
ukf.R = R
self.ukf = ukf
self.isFirst = True
self.route = route
def show_sigma_transform():
fig = plt.figure()
ax=fig.gca()
x = np.array([0, 5])
P = np.array([[4, -2.2], [-2.2, 3]])
plot_covariance_ellipse(x, P, facecolor='b', variance=9, alpha=0.5)
S = UKF.sigma_points(x=x, P=P, kappa=0)
plt.scatter(S[:,0], S[:,1], c='k', s=80)
x = np.array([15, 5])
P = np.array([[3, 1.2],[1.2, 6]])
plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.5)
ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6))
ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6))
ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6))
ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6))
ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6))
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
#plt.axis('equal')
plt.show()
def test_saver_UKF():
def fx(x, dt):
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i, i] for i in range(40)]
s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean'])
for z in zs:
kf.predict()
kf.update(z)
#print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
s.to_array()
def __init__(self, trueTrajectory, dt=.1, noise = 0.):
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
self.dt = dt
sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.)
self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal,
dt=dt, points=sigmas)
self.KF.Q = np.diag([1., 0.5, 0.2])
self.KF.R = np.diag([2., 1.12])*noise + np.diag([.05, .05])
self.first = True
def test_linear_2d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, kappa=0)
kf.x = np.array([-1., 1., -1., 1])
kf.P*=0.0001
#kf.R *=0
#kf.Q
zs = []
for i in range(20):
z = np.array([i+randn()*0.1, i+randn()*0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
if DO_PLOT:
zs = np.asarray(zs)
#plt.plot(zs[:,0])
plt.plot(Ms[:,0])
plt.plot(smooth_x[:,0], smooth_x[:,2])
print(smooth_x)
def estimateUKF(camPoses):
points = MerweScaledSigmaPoints(3,.1,2.,0.)
filter = UKF(3,3,0,h, f, points, sqrt_fn=None, x_mean_fn=state_mean, z_mean_fn=state_mean, residual_x=residual, residual_z=residual)
filter.P = np.diag([0.04,0.04,0.003])
filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
filter.R = np.diag([100,100,0.25])
Uposes = [[],[]]
for i in range(len(speed)):
x = filter.x
Uposes[0].append(x[0])
Uposes[1].append(x[1])
filter.predict(fx_args=[speed[i],angle[i]*0.01745])
filter.update(z = [camPoses[0][i],camPoses[1][i],camPoses[2][i]])
return Uposes
def test_1d():
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([[x[0]]])
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1],
[1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
for i in range(50):
z = np.array([[i+randn()*0.1]])
#xx, pp, Sx = predict(f, x, P, Q)
#x, P = update(h, z, xx, pp, R)
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] -kf.x[0]) < 1e-10
assert abs(ckf.x[1] -kf.x[1]) < 1e-10
plt.show()
def test_1d():
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return x[0:1]
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1],
[1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
s = Saver(kf)
for i in range(50):
z = np.array([[i+randn()*0.1]])
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
s.save()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
def test_radar():
def fx(x, dt):
A = np.eye(3) + dt * np.array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
return A.dot(x)
def hx(x):
return np.sqrt (x[0]**2 + x[2]**2)
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0.)
kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0,20+dt, dt)
n = len(t)
xs = np.zeros((n,3))
random.seed(200)
rs = []
#xs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i,:] = kf.x
rs.append(r)
if DO_PLOT:
print(xs[:,0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:,0])
plt.subplot(312)
plt.plot(t, xs[:,1])
plt.subplot(313)
plt.plot(t, xs[:,2])
def __init__(self, deltaT, measurementNoiseStd):
self.updatedPredictions = []
self.mus = []
"""
First init constant linear model
"""
points1 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.constantLinearModel = UnscentedKalmanFilter(
dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_linearModel,
hx=h_unscented_linearModel,
points=points1)
self.constantLinearModel.x = np.array([0.01, 0.01, 0.01, 0.01, 0])
self.constantLinearModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.constantLinearModel.R = np.eye(2) * (measurementNoiseStd**2)
self.constantLinearModel.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0])
"""
Second init constant turn rate model
"""
points2 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.constantTurnRateModel = UnscentedKalmanFilter(
dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_turnRateModel,
hx=h_unscented_turnRateModel,
points=points2)
self.constantTurnRateModel.x = np.array(
[0.01, 0.01, 0.01, 0.001, 1e-5])
self.constantTurnRateModel.P = np.eye(5) * (measurementNoiseStd**
2) / 2.0
self.constantTurnRateModel.R = np.eye(2) * (measurementNoiseStd**2)
self.constantTurnRateModel.Q = np.diag(
[1e-24, 1e-24, 1e-3, 4e-3, 1e-10])
"""
Third init random motion model
"""
points3 = MerweScaledSigmaPoints(5, alpha=0.0025, beta=2., kappa=0)
self.randomModel = UnscentedKalmanFilter(dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_randomModel,
hx=h_unscented_randomModel,
points=points3)
self.randomModel.x = np.array([0.01, 0.01, 0.01, 0.001, 1e-5])
self.randomModel.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.randomModel.R = np.eye(2) * (measurementNoiseStd**2)
self.randomModel.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24])
#############################33
if (1):
filters = [self.constantLinearModel, self.constantTurnRateModel]
mu = [0.5, 0.5]
trans = np.array([[0.9, 0.1], [0.1, 0.9]])
self.imm = IMMEstimator(filters, mu, trans)
else:
filters = [
self.constantLinearModel, self.constantTurnRateModel,
self.randomModel
]
mu = [0.34, 0.33, 0.33]
trans = np.array([[0.9, 0.05, 0.05], [0.05, 0.9, 0.05],
[0.05, 0.05, 0.9]])
self.imm = IMMEstimator(filters, mu, trans)
[0., 0., 1.]])
return np.dot(F, x)
def hx(x):
""" returns slant range based on downrange distance and altitude"""
return (x[0]**2 + x[2]**2)**.5
if __name__ == "__main__":
dt = 0.05
radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.)
radarUKF.Q *= Q_discrete_white_noise(3, 1, .01)
radarUKF.R *= 10
radarUKF.x = np.array([0., 90., 1100.])
radarUKF.P *= 100.
t = np.arange(0, 20+dt, dt)
n = len(t)
xs = []
rs = []
for i in range(n):
r = GetRadar(dt)
rs.append(r)
radarUKF.update(r, hx, fx)
#m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]])
#m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]])
#m = array([[5., 10], [10, 5]])
#m = array([[5., 10], [10, 5]])
sigma_r = .3
sigma_h = .1#radians(.5)#np.radians(1)
#sigma_steer = radians(10)
dt = 0.1
wheelbase = 0.5
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x)
#points = JulierSigmaPoints(n=3, kappa=3)
ukf= UKF(dim_x=3, dim_z=2*len(m), fx=fx, hx=Hx, dt=dt, points=points,
x_mean_fn=state_mean, z_mean_fn=z_mean,
residual_x=residual_x, residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m))
ukf.Q =np.eye(3)*.00001
u = array([1.1, 0.])
xp = ukf.x.copy()
plt.cla()
plt.scatter(m[:, 0], m[:, 1])
def test_julier_weights():
for n in range(1,15):
for k in np.linspace(0,5,0.1):
Wm = UKF.weights(n, k)
assert abs(sum(Wm) - 1) < 1.e-12
def test_linear_1d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1., dt],
[0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([x[0]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt,
fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1],
[1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
z = np.array([2.])
kf.predict()
kf.update(z)
zs = []
for i in range(50):
z = np.array([i + randn()*0.1])
zs.append(z)
kf.predict()
kf.update(z)
print('K', kf.K.T)
print('x', kf.x)
def test_rts():
def fx(x, dt):
A = np.eye(3) + dt * np.array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return np.sqrt (x[0]**2 + x[2]**2)
dt = 0.05
kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=1.)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0,20+dt, dt)
n = len(t)
xs = np.zeros((n,3))
random.seed(200)
rs = []
#xs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i,:] = kf.x
rs.append(r)
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3)*100
M, P = kf.batch_filter(rs)
assert np.array_equal(M, xs), "Batch filter generated different output"
Qs = [kf.Q]*len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
if DO_PLOT:
print(xs[:,0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:,0])
plt.plot(t, M2[:,0], c='g')
plt.subplot(312)
plt.plot(t, xs[:,1])
plt.plot(t, M2[:,1], c='g')
plt.subplot(313)
plt.plot(t, xs[:,2])
plt.plot(t, M2[:,2], c='g')
def test_fixed_lag():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 1.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
M = []
P = []
N = 10
flxs = []
for i in range(len(t)):
r = radar.get_range()
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
flxs.append(kf.x)
rs.append(r)
M.append(kf.x)
P.append(kf.P)
print(i)
if i == 20 and len(M) >= N:
try:
M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:],
Ps=np.asarray(P)[-N:])
flxs[-N:] = M2
except:
print('except', i)
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3) * 100
M, P = kf.batch_filter(rs)
Qs = [kf.Q]*len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
flxs = np.asarray(flxs)
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.plot(t, flxs[:, 0], c='r')
plt.plot(t, M2[:, 0], c='g')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.plot(t, flxs[:, 1], c='r')
plt.plot(t, M2[:, 1], c='g')
plt.subplot(313)
plt.plot(t, xs[:, 2])
plt.plot(t, flxs[:, 2], c='r')
plt.plot(t, M2[:, 2], c='g')
sigma_bearing=0.1
# x = [1.3905235974633778, 7.832049402391674, 9.020314462351989, 3.687485010010496, 4.933087914504979, 5.842278721831094, 7.374384722598193, 11.548435059429593, 10.08330407331331, 4.048332216167037, 11.753400231528877, 5.24814930818585, 0.3780861370621249, 3.819226471192638, 3.219009627591963, -0.660991670650813, -6.810902143188039, -2.648632039619782, -0.8651469995661083, -5.882826870804094, -6.771881598941817, -6.883212384180803, -10.970288387596014, -9.38416048883138, -2.0041408634734, -4.1999609273146294, -4.427392485133488, -4.094107923812431, -4.539507495972897, -6.958816977357863, -0.5837156626237164, 3.211904185873981, 2.6999090897786973, 4.946462347786874, 6.250544373196611, 1.4360007457367132, 6.376447944684558, 5.395872942944427, 3.6592834591198713, 3.6799054753035985, 6.372070482657179, 10.83971503990248, 6.471684767984395, -4.13122094642976, 1.3657905443071188, -2.1545837703277186, -3.6218498314375362, -5.990316163817381, -2.825131401522447, -8.47962049553197, -3.0710249464300077, -5.242374500395425, -11.356227320748491, -3.580215175474069, -8.949524398862625, -12.148475712671257, -3.541357682569754, -2.3218551317498113, -4.10750098248105, -1.154362522019348, 1.719708807469195, -2.9317706605909493, 5.581580285882415, 7.322885401113478, 8.035547065332059, 7.115626723235949, 6.8834109751060195, 9.605681429819468, 5.378480946301948, 4.55936497171603, 0.9696632799355314, 3.0281397205843197, -2.4315041509242974, -0.4213417059299991, 0.13371487118430309, -2.5616840637149783, -3.330922819747191, -3.238792470549532, -12.848697164298123, -11.650132469341331, -2.9250345334239203, -8.844969766009035, -7.54940178448948, -10.204223020208033, -7.4851192397126525, -9.156624882388012, -6.366425519643718, -8.553734125327592, -4.609302566545153, -4.264881667574036, 1.4533519725675805]
# y = [5.1592964225372135, 8.216934337621904, 14.221677454416298, 9.606966741201592, 15.393598119630198, 15.463991661062682, 18.798461424361072, 17.83425651827887, 16.480645145332982, 22.97048479933387, 19.510051232214657, 24.642699320629887, 25.45829257417096, 25.497397786842278, 22.450305133766594, 21.947846746481055, 18.037770599424036, 23.858011123610375, 19.308656588152285, 16.69873918296076, 20.488408553230123, 16.367880867164754, 23.654044378766407, 20.73648776720881, 15.90450795825495, 16.135338583542634, 8.493714748632618, 8.913938224865586, 8.734285273935438, 8.050993570472942, 10.158164230122795, 8.08449276240243, 14.886302280598855, 14.237310205824675, 17.306486687897937, 13.8601024213346, 17.90588766410904, 15.869047349619871, 20.686353443244506, 18.42398071352659, 23.22554467544738, 21.198830063607687, 24.727001975565397, 23.188652263477202, 25.512192853204706, 25.07747201310473, 24.054712728944807, 22.68899006890544, 22.641944066718526, 18.33228864387304, 17.94999176153331, 22.10590333682616, 19.005801408713417, 13.582191596376182, 10.639031293990215, 15.577633926934181, 9.911438955202737, 8.666040822401431, 11.027340108357107, 12.56573800372876, 11.771468252890216, 12.46635569231553, 8.986038622183006, 12.215863938535502, 10.291879983793017, 16.505270167068254, 19.6009636347262, 19.10972604819267, 15.892910888479538, 26.63984263799601, 17.951774058138326, 21.107945349676598, 23.441244468726836, 21.853959925952115, 21.162499359087953, 21.808181391384416, 20.52011983367284, 19.153098462721978, 26.471740153772178, 25.208554508505728, 17.67356964440606, 18.51548339755294, 16.646498463883713, 15.599794378503121, 17.747778170685166, 13.998749493955843, 13.588001144109704, 16.94352162086974, 9.476077823803571, 5.4278110057897955, 12.342893333479008]
# x_actual = [0.0, 1.4672214011007085, 2.83753958756461, 4.0510650791270315, 5.054760988665319, 5.80476098866532, 6.268286480227742, 6.4250791751292216, 6.268286480227741, 5.804760988665318, 5.054760988665317, 4.051065079127029, 2.837539587564608, 1.4672214011007072, -8.881784197001252e-16, -1.5000000000000009, -2.9672214011007103, -4.337539587564613, -5.551065079127038, -6.554760988665329, -7.304760988665335, -7.768286480227762, -7.925079175129248, -7.768286480227774, -7.3047609886653575, -6.55476098866536, -5.551065079127074, -4.3375395875646525, -2.9672214011007503, -1.500000000000041, -4.107825191113079e-14, 1.4672214011006668, 2.837539587564567, 4.051065079126986, 5.054760988665272, 5.8047609886652705, 6.268286480227689, 6.4250791751291665, 6.268286480227683, 5.804760988665258, 5.0547609886652545, 4.051065079126965, 2.8375395875645424, 1.4672214011006404, -6.816769371198461e-14, -1.5000000000000682, -2.967221401100777, -4.337539587564679, -5.551065079127102, -6.554760988665391, -7.304760988665395, -7.76828648022782, -7.925079175129303, -7.768286480227826, -7.304760988665407, -6.55476098866541, -5.551065079127124, -4.337539587564702, -2.9672214011008, -1.5000000000000908, -9.08162434143378e-14, 1.467221401100617, 2.837539587564517, 4.0510650791269365, 5.0547609886652225, 5.804760988665221, 6.26828648022764, 6.425079175129117, 6.2682864802276335, 5.804760988665208, 5.054760988665205, 4.051065079126915, 2.8375395875644926, 1.4672214011005906, -1.1790568521519162e-13, -1.500000000000118, -2.9672214011008267, -4.337539587564729, -5.551065079127151, -6.554760988665441, -7.3047609886654445, -7.76828648022787, -7.925079175129353, -7.768286480227876, -7.304760988665457, -6.55476098866546, -5.551065079127174, -4.337539587564752, -2.9672214011008498, -1.5000000000001406, -1.4055423491754482e-13]
# y_actual = [10.0, 10.31186753622664, 10.92197250084034, 11.803650379279048, 12.91836761749514, 14.217405723171797, 15.643990497614528, 17.135773340666937, 18.627556183719346, 20.054140958162076, 21.353179063838734, 22.467896302054825, 23.349574180493534, 23.959679145107234, 24.271546681333874, 24.271546681333877, 23.95967914510724, 23.349574180493544, 22.46789630205484, 21.353179063838752, 20.054140958162098, 18.62755618371937, 17.13577334066696, 15.643990497614551, 14.217405723171819, 12.918367617495159, 11.803650379279066, 10.921972500840356, 10.311867536226657, 10.000000000000021, 10.000000000000025, 10.311867536226666, 10.921972500840369, 11.80365037927908, 12.918367617495173, 14.217405723171833, 15.643990497614563, 17.135773340666972, 18.62755618371938, 20.05414095816211, 21.353179063838766, 22.467896302054854, 23.349574180493562, 23.95967914510726, 24.2715466813339, 24.2715466813339, 23.95967914510726, 23.349574180493562, 22.467896302054854, 21.353179063838766, 20.05414095816211, 18.62755618371938, 17.135773340666972, 15.643990497614562, 14.217405723171831, 12.918367617495171, 11.803650379279079, 10.921972500840369, 10.31186753622667, 10.000000000000034, 10.000000000000037, 10.311867536226679, 10.921972500840381, 11.803650379279093, 12.918367617495186, 14.217405723171845, 15.643990497614576, 17.135773340666987, 18.627556183719395, 20.054140958162126, 21.35317906383878, 22.467896302054868, 23.349574180493576, 23.959679145107273, 24.271546681333913, 24.271546681333913, 23.959679145107273, 23.349574180493576, 22.467896302054868, 21.35317906383878, 20.054140958162126, 18.627556183719395, 17.135773340666987, 15.643990497614576, 14.217405723171845, 12.918367617495186, 11.803650379279093, 10.921972500840383, 10.311867536226684, 10.000000000000048, 10.000000000000052]
x = [-3.422319443527482, -0.3670766780148629, 0.42810463566372503, 7.971751432584803, 5.561623301852658, 5.122631477269733, 7.9380784789008345, 5.724443001813307, 3.9453729476132224, 7.805691945653483, 7.103981420312309, 3.6438350482459896, 2.307982454031695, 3.700120851211732, 2.659684755972816, -3.5930177497743228, -3.5940690041937104, -7.197420705291179, -3.320401730928432, -9.86609819752164, -7.080369683242467, -6.127129784273851, -8.85849826178098, -6.263547220912284, -8.512470940768917, -4.743424343342259, -5.395873837722023, -7.8941877456860885, -2.6621170295142553, -6.09164203736454, -1.320245983651153]
y = [14.16729015960461, 18.105403721893914, 11.857680736011378, 11.965905696440576, 12.378397170235502, 17.813946679056244, 20.347459881502157, 24.746811440113035, 25.764577855024726, 29.784943985662252, 22.03292253903822, 26.837906066246074, 27.727039870771407, 27.711933191437307, 34.07656757469763, 28.72982212627928, 28.284308305957804, 29.184032547849366, 26.636778632724987, 27.76168258806925, 27.716226098275158, 25.345843624215934, 14.664004496070973, 20.461258499585742, 18.967501506449352, 14.098137295901621, 14.072033819348718, 15.520102110851612, 16.343599524337055, 15.096377045760601, 15.52746451234623]
x_actual = [0.0, 1.4672214011007085, 2.83753958756461, 4.0510650791270315, 5.054760988665319, 5.80476098866532, 6.268286480227742, 6.4250791751292216, 6.268286480227741, 5.804760988665318, 5.054760988665317, 4.051065079127029, 2.837539587564608, 1.4672214011007072, -8.881784197001252e-16, -1.5000000000000009, -2.9672214011007103, -4.337539587564613, -5.551065079127038, -6.554760988665329, -7.304760988665335, -7.768286480227762, -7.925079175129248, -7.768286480227774, -7.3047609886653575, -6.55476098866536, -5.551065079127074, -4.3375395875646525, -2.9672214011007503, -1.500000000000041, -4.107825191113079e-14]
y_actual = [15.0, 15.31186753622664, 15.92197250084034, 16.80365037927905, 17.91836761749514, 19.217405723171797, 20.643990497614528, 22.135773340666937, 23.627556183719346, 25.054140958162076, 26.353179063838734, 27.467896302054825, 28.349574180493534, 28.959679145107234, 29.271546681333874, 29.271546681333877, 28.95967914510724, 28.349574180493544, 27.46789630205484, 26.353179063838752, 25.054140958162098, 23.62755618371937, 22.13577334066696, 20.643990497614553, 19.217405723171822, 17.918367617495164, 16.803650379279073, 15.921972500840363, 15.311867536226664, 15.000000000000028, 15.000000000000032]
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, subtract=residual_x)
ukf = UKF(dim_x = 3, dim_z = 2, fx=fx, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([1.3905235974633778, 7.832049402391674, 0.])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag( [sigma_range**2, sigma_bearing**2] )
ukf.Q = np.eye(3) * 0.0001
state = ukf.x.copy()
for i in range(1, len(x)):
state = x_actual[i], y_actual[i]
#state = move(state, dt, turning)
ukf.predict(dt = 1., fx_args = (1.5, 2*pi / 30))
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]], dtype=float)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0], x[2]])
dt = 1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([0., 0., 0., 0]) # initial state
kf.P *= 0.1 # initial uncertainty
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
errs=0
count=0
P=lx.Project(mode='2D',solver='LSE')
filepath="lowlow"
vel = {}
position = {}
def test_linear_rts():
""" for a linear model the Kalman filter and UKF should produce nearly
identical results.
Test code mostly due to user gboehl as reported in GitHub issue #97, though
I converted it from an AR(1) process to constant velocity kinematic
model.
"""
dt = 1.0
F = np.array([[1., dt], [.0, 1]])
H = np.array([[1., .0]])
def t_func(x, dt):
F = np.array([[1., dt], [.0, 1]])
return np.dot(F, x)
def o_func(x):
return np.dot(H, x)
sig_t = .1 # peocess
sig_o = .0001 # measurement
N = 50
X_true, X_obs = [], []
for i in range(N):
X_true.append([i + 1, 1.])
X_obs.append(i + 1 + np.random.normal(scale=sig_o))
X_true = np.array(X_true)
X_obs = np.array(X_obs)
oc = np.ones((1, 1)) * sig_o**2
tc = np.zeros((2, 2))
tc[1, 1] = sig_t**2
tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2)
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1)
ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points)
ukf.x = np.array([0., 1.])
ukf.R = np.copy(oc)
ukf.Q = np.copy(tc)
s = Saver(ukf)
s.save()
s.to_array()
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([[0., 1]]).T
kf.R = np.copy(oc)
kf.Q = np.copy(tc)
kf.H = np.copy(H)
kf.F = np.copy(F)
mu_ukf, cov_ukf = ukf.batch_filter(X_obs)
x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf)
mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs)
x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf)
# check results of filtering are correct
kfx = mu_kf[:, 0, 0]
ukfx = mu_ukf[:, 0]
kfxx = mu_kf[:, 1, 0]
ukfxx = mu_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
# error in position should be smaller then error in velocity, hence
# atol is different for the two tests.
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
# now ensure the RTS smoothers gave nearly identical results
kfx = x_kf[:, 0, 0]
ukfx = x_ukf[:, 0]
kfxx = x_kf[:, 1, 0]
ukfxx = x_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
return ukf
x[1] = x[1] + x[3]*dt
x[2] = x[2]
x[3] = x[3] + ((0.0034 * g * np.exp(-x[1] / 22000) * x[3] ** 2) / (2 * b) - g)*dt
return x
def h_cv(x):
"""Measurement function -
measuring only position"""
return np.array([x[0], x[1]])
starting_conditions = [100., 1.00e4, 0., 0., 200.]
points = MerweScaledSigmaPoints(n=4, alpha=.0001, beta=2., kappa=-1)
ukf = UKF(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=points)
ukf.x = np.array([1000., 1.00e3, 0., 0.])
ukf.R = np.diag([[std_x[0]**2, std_x[0]**2]])
#ukf.H = np.array([[1]])
ukf.Q = np.diag([100., 100., 1000., 1000.])
uxs = []
trux = []
truv = []
measx = []
time = []
covarx = []
covarv = []
zs = np.arange(0, 1435 + dt, dt)
f = falling_object(starting_conditions[0:2], starting_conditions[2:4], starting_conditions[4], std_x, std_model)
t = 0
def run_localization(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3,
alpha=.00001,
beta=2,
kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3,
dim_z=2 * len(landmarks),
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks))
ukf.Q = np.eye(3) * 0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, dt / step, u, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(u=u, wheelbase=wheelbase)
if i % ellipse_step == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn() * sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn() * sigma_bearing))
z.extend([d, a])
ukf.update(z, landmarks=landmarks)
if i % ellipse_step == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
F = np.array([[1., dt, 0.], [0., 1., 0.], [0., 0., 1.]])
return np.dot(F, x)
def hx(x):
""" returns slant range based on downrange distance and altitude"""
return (x[0]**2 + x[2]**2)**.5
if __name__ == "__main__":
dt = 0.05
radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.)
radarUKF.Q *= Q_discrete_white_noise(3, 1, .01)
radarUKF.R *= 10
radarUKF.x = np.array([0., 90., 1100.])
radarUKF.P *= 100.
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = []
rs = []
for i in range(n):
r = GetRadar(dt)
rs.append(r)
radarUKF.update(r, hx, fx)
# define display window name
windowName = "Kalman Object Tracking" # window name
windowName2 = "Hue histogram back projection" # window name
windowNameSelection = "initial selected region"
# init kalman filter object
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1)
dt = 1.0
ukf = UKF(dim_x=4,
dim_z=2,
fx=state_transistion_function,
hx=measurement_Matrix,
dt=dt,
points=points)
ukf.x = np.array([0., 0., 0., 0.])
ukf.Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32) * 0.03
kalman = cv2.KalmanFilter(4, 2)
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]], np.float32) * 0.03
[0, 0, 0, 1]])
return np.dot(F, x)
def h_cv(x):
return np.array([x[0], x[2]])
def e(x):
res = []
for n in range(x.shape[0]):
res.append(np.sqrt(x[n][0]**2+x[n][2]**2))
return res
dt = 1.0
random.seed(1234)
ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0)
ukf.x = np.array([100., 0., 0., 0.])
ukf.R = np.diag([25, 25])
ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)
ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt)
ckf.x = np.array([100., 0., 0., 0.])
ckf.R = np.diag([25, 25])
ckf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ckf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.04)
uxs = []
pxs = []
zs = []
txs = []
class Model:
"""
SIRD model of Covid-19.
"""
__CONFIRMED_URL = 'https://bit.ly/35yJO0d'
__RECOVERED_URL = 'https://bit.ly/2L6jLE9'
__DEATHS_URL = 'https://bit.ly/2L0hzxQ'
__POPULATION_URL = 'https://bit.ly/2WYjZCD'
__JHU_DATA_SHIFT = 4
__N_FILTERED = 7 # Number of state variables to filter (I, R, D, β, γ, μ and n, the population).
__N_MEASURED = 3 # Number of measured variables (I, R and D).
__NB_OF_STEPS = 100
__DELTA_T = 1 / __NB_OF_STEPS
__FIG_SIZE = (11, 13)
__S_COLOR = '#0072bd'
__I_COLOR = '#d95319'
__R_COLOR = '#edb120'
__D_COLOR = '#7e2f8e'
__BETA_COLOR = '#77ac30'
__GAMMA_COLOR = '#4dbeee'
__MU_COLOR = '#a2142f'
__DATA_ALPHA = 0.3
__DATA = None
__POPULATION = None
class Use(Enum):
WIKIPEDIA = auto()
DATA = auto()
def __init__(self, use=Use.DATA, country='New Zealand', max_data=0):
"""
Initialise our Model object.
"""
# Retrieve the data (if requested and needed).
if use == Model.Use.DATA and Model.__DATA is None:
confirmed_data, confirmed_data_start = self.__jhu_data(Model.__CONFIRMED_URL, country)
recovered_data, recovered_data_start = self.__jhu_data(Model.__RECOVERED_URL, country)
deaths_data, deaths_data_start = self.__jhu_data(Model.__DEATHS_URL, country)
data_start = min(confirmed_data_start, recovered_data_start, deaths_data_start) - Model.__JHU_DATA_SHIFT
for i in range(data_start, confirmed_data.shape[1]):
c = confirmed_data.iloc[0][i]
r = recovered_data.iloc[0][i]
d = deaths_data.iloc[0][i]
data = [c - r - d, r, d]
if Model.__DATA is None:
Model.__DATA = np.array(data)
else:
Model.__DATA = np.vstack((Model.__DATA, data))
if use == Model.Use.DATA:
self.__data = Model.__DATA
else:
self.__data = None
if self.__data is not None:
if not isinstance(max_data, int):
sys.exit('Error: \'max_data\' must be an integer value.')
if max_data > 0:
self.__data = self.__data[:max_data]
# Retrieve the population (if needed).
if use == Model.Use.DATA and Model.__POPULATION is None:
Model.__POPULATION = {}
response = requests.get(Model.__POPULATION_URL)
soup = BeautifulSoup(response.text, 'html.parser')
data = soup.select('div div div div div tbody tr')
for i in range(len(data)):
country_soup = BeautifulSoup(data[i].prettify(), 'html.parser')
country_value = country_soup.select('tr td a')[0].get_text().strip()
population_value = country_soup.select('tr td')[2].get_text().strip().replace(',', '')
Model.__POPULATION[country_value] = int(population_value)
if use == Model.Use.DATA:
if country in Model.__POPULATION:
self.__population = Model.__POPULATION[country]
else:
sys.exit('Error: no population data is available for {}.'.format(country))
# Keep track of whether to use the data.
self.__use_data = use == Model.Use.DATA
# Declare some internal variables (that will then be initialised through our call to reset()).
self.__beta = None
self.__gamma = None
self.__mu = None
self.__ukf = None
self.__x = None
self.__n = None
self.__data_s_values = None
self.__data_i_values = None
self.__data_r_values = None
self.__data_d_values = None
self.__s_values = None
self.__i_values = None
self.__r_values = None
self.__d_values = None
self.__beta_values = None
self.__gamma_values = None
self.__mu_values = None
# Initialise (i.e. reset) our SIRD model.
self.reset()
@staticmethod
def __jhu_data(url, country):
data = pd.read_csv(url)
data = data[(data['Country/Region'] == country) & data['Province/State'].isnull()]
if data.shape[0] == 0:
sys.exit('Error: no Covid-19 data is available for {}.'.format(country))
data = data.drop(data.columns[list(range(Model.__JHU_DATA_SHIFT))], axis=1) # Skip non-data columns.
start = None
for i in range(data.shape[1]):
if data.iloc[0][i] != 0:
start = Model.__JHU_DATA_SHIFT + i
break
return data, start
def __data_x(self, day, index):
"""
Return the I/R/D value for the given day.
"""
return self.__data[day][index] if self.__use_data else math.nan
def __data_s(self, day):
"""
Return the S value for the given day.
"""
if self.__use_data:
return self.__population - self.__data_i(day) - self.__data_r(day) - self.__data_d(day)
else:
return math.nan
def __data_i(self, day):
"""
Return the I value for the given day.
"""
return self.__data_x(day, 0)
def __data_r(self, day):
"""
Return the R value for the given day.
"""
return self.__data_x(day, 1)
def __data_d(self, day):
"""
Return the D value for the given day.
"""
return self.__data_x(day, 2)
def __data_available(self, day):
"""
Return whether some data is available for the given day.
"""
return day <= self.__data.shape[0] - 1 if self.__use_data else False
def __s_value(self):
"""
Return the S value based on the values of I, R, D and N.
"""
return self.__n - self.__x.sum()
def __i_value(self):
"""
Return the I value.
"""
return self.__x[0]
def __r_value(self):
"""
Return the R value.
"""
return self.__x[1]
def __d_value(self):
"""
Return the D value.
"""
return self.__x[2]
@staticmethod
def __f(x, dt, **kwargs):
"""
State function.
The ODE system to solve is:
dI/dt = βIS/N - γI - μI
dR/dt = γI
dD/dt = μI
"""
model_self = kwargs.get('model_self')
with_ukf = kwargs.get('with_ukf', True)
if with_ukf:
s = x[6] - x[:3].sum()
beta = x[3]
gamma = x[4]
mu = x[5]
n = x[6]
else:
s = model_self.__n - x.sum()
beta = model_self.__beta
gamma = model_self.__gamma
mu = model_self.__mu
n = model_self.__n
a = np.array([[1 + dt * (beta * s / n - gamma - mu), 0, 0, 0, 0, 0, 0],
[dt * gamma, 1, 0, 0, 0, 0, 0],
[dt * mu, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
if with_ukf:
return a @ x
else:
return a[:3, :3] @ x
@staticmethod
def __h(x):
"""
Measurement function.
"""
return x[:Model.__N_MEASURED]
def reset(self):
"""
Reset our SIRD model.
"""
# Reset β, γ and μ to the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
self.__beta = 0.4
self.__gamma = 0.035
self.__mu = 0.005
# Reset I, R and D to the data at day 0 or the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
if self.__use_data:
self.__x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0)])
self.__n = self.__population
else:
self.__x = np.array([3, 0, 0])
self.__n = 1000
# Reset our Unscented Kalman filter (if required). Note tat we use a dt value of 1 (day) and not the value of
# Model.__DELTA_T.
if self.__use_data:
points = MerweScaledSigmaPoints(Model.__N_FILTERED,
1e-3, # Alpha value (usually a small positive value like 1e-3).
2, # Beta value (a value of 2 is optimal for a Gaussian distribution).
0, # Kappa value (usually, either 0 or 3-n).
)
self.__ukf = UnscentedKalmanFilter(Model.__N_FILTERED, Model.__N_MEASURED, 1, self.__h, Model.__f, points)
self.__ukf.x = np.array([self.__data_i(0), self.__data_r(0), self.__data_d(0),
self.__beta, self.__gamma, self.__mu, self.__n])
# Reset our data (if requested).
if self.__use_data:
self.__data_s_values = np.array([self.__data_s(0)])
self.__data_i_values = np.array([self.__data_i(0)])
self.__data_r_values = np.array([self.__data_r(0)])
self.__data_d_values = np.array([self.__data_d(0)])
# Reset our predicted/estimated values.
self.__s_values = np.array([self.__s_value()])
self.__i_values = np.array([self.__i_value()])
self.__r_values = np.array([self.__r_value()])
self.__d_values = np.array([self.__d_value()])
# Reset our estimated SIRD model parameters.
self.__beta_values = np.array([self.__beta])
self.__gamma_values = np.array([self.__gamma])
self.__mu_values = np.array([self.__mu])
def run(self, batch_filter=True, nb_of_days=100):
"""
Run our SIRD model for the given number of days, taking advantage of the data (if requested) to estimate the
values of β, γ and μ.
"""
# Make sure that we were given a valid number of days.
if not isinstance(nb_of_days, int) or nb_of_days <= 0:
sys.exit('Error: \'nb_of_days\' must be an integer value greater than zero.')
# Run our SIRD simulation, which involves computing our predicted/estimated state by computing our SIRD model /
# Unscented Kalman filter in batch filter mode, if required.
if self.__use_data and batch_filter:
mu, cov = self.__ukf.batch_filter(self.__data)
batch_filter_x, _, _ = self.__ukf.rts_smoother(mu, cov)
# Override the first value of S, I, R and D.
x = batch_filter_x[0][:3]
self.__s_values = np.array([self.__n - x.sum()])
self.__i_values = np.array([x[0]])
self.__r_values = np.array([x[1]])
self.__d_values = np.array([x[2]])
for i in range(1, nb_of_days + 1):
# Compute our predicted/estimated state by computing our SIRD model / Unscented Kalman filter for one day.
if self.__use_data and self.__data_available(i):
if batch_filter:
self.__x = batch_filter_x[i][:3]
self.__beta = batch_filter_x[i][3]
self.__gamma = batch_filter_x[i][4]
self.__mu = batch_filter_x[i][5]
else:
self.__ukf.predict(model_self=self)
self.__ukf.update(np.array([self.__data_i(i), self.__data_r(i), self.__data_d(i)]))
self.__x = self.__ukf.x[:3]
self.__beta = self.__ukf.x[3]
self.__gamma = self.__ukf.x[4]
self.__mu = self.__ukf.x[5]
else:
for j in range(1, Model.__NB_OF_STEPS + 1):
self.__x = Model.__f(self.__x, Model.__DELTA_T, model_self=self, with_ukf=False)
# Keep track of our data (if requested).
if self.__use_data:
if self.__data_available(i):
self.__data_s_values = np.append(self.__data_s_values, self.__data_s(i))
self.__data_i_values = np.append(self.__data_i_values, self.__data_i(i))
self.__data_r_values = np.append(self.__data_r_values, self.__data_r(i))
self.__data_d_values = np.append(self.__data_d_values, self.__data_d(i))
else:
self.__data_s_values = np.append(self.__data_s_values, math.nan)
self.__data_i_values = np.append(self.__data_i_values, math.nan)
self.__data_r_values = np.append(self.__data_r_values, math.nan)
self.__data_d_values = np.append(self.__data_d_values, math.nan)
# Keep track of our predicted/estimated values.
self.__s_values = np.append(self.__s_values, self.__s_value())
self.__i_values = np.append(self.__i_values, self.__i_value())
self.__r_values = np.append(self.__r_values, self.__r_value())
self.__d_values = np.append(self.__d_values, self.__d_value())
# Keep track of our estimated SIRD model parameters.
self.__beta_values = np.append(self.__beta_values, self.__beta)
self.__gamma_values = np.append(self.__gamma_values, self.__gamma)
self.__mu_values = np.append(self.__mu_values, self.__mu)
def plot(self, figure=None, two_axes=False):
"""
Plot the results using five subplots for 1) S, 2) I and R, 3) D, 4) β, and 5) γ and μ. In each subplot, we plot
the data (if requested) as bars and the computed value as a line.
"""
days = range(self.__s_values.size)
nrows = 5 if self.__use_data else 3
ncols = 1
if figure is None:
show_figure = True
figure, axes = plt.subplots(nrows, ncols, figsize=Model.__FIG_SIZE)
else:
figure.clf()
show_figure = False
axes = figure.subplots(nrows, ncols)
figure.canvas.set_window_title('SIRD model fitted to data' if self.__use_data else 'Wikipedia SIRD model')
# First subplot: S.
axis1 = axes[0]
axis1.plot(days, self.__s_values, Model.__S_COLOR, label='S')
axis1.legend(loc='best')
if self.__use_data:
axis2 = axis1.twinx() if two_axes else axis1
axis2.bar(days, self.__data_s_values, color=Model.__S_COLOR, alpha=Model.__DATA_ALPHA)
data_s_range = self.__population - min(self.__data_s_values)
data_block = 10 ** (math.floor(math.log10(data_s_range)) - 1)
s_values_shift = data_block * math.ceil(data_s_range / data_block)
axis2.set_ylim(min(min(self.__s_values), self.__population - s_values_shift), self.__population)
# Second subplot: I and R.
axis1 = axes[1]
axis1.plot(days, self.__i_values, Model.__I_COLOR, label='I')
axis1.plot(days, self.__r_values, Model.__R_COLOR, label='R')
axis1.legend(loc='best')
if self.__use_data:
axis2 = axis1.twinx() if two_axes else axis1
axis2.bar(days, self.__data_i_values, color=Model.__I_COLOR, alpha=Model.__DATA_ALPHA)
axis2.bar(days, self.__data_r_values, color=Model.__R_COLOR, alpha=Model.__DATA_ALPHA)
# Third subplot: D.
axis1 = axes[2]
axis1.plot(days, self.__d_values, Model.__D_COLOR, label='D')
axis1.legend(loc='best')
if self.__use_data:
axis2 = axis1.twinx() if two_axes else axis1
axis2.bar(days, self.__data_d_values, color=Model.__D_COLOR, alpha=Model.__DATA_ALPHA)
# Fourth subplot: β.
if self.__use_data:
axis1 = axes[3]
axis1.plot(days, self.__beta_values, Model.__BETA_COLOR, label='β')
axis1.legend(loc='best')
# Fourth subplot: γ and μ.
if self.__use_data:
axis1 = axes[4]
axis1.plot(days, self.__gamma_values, Model.__GAMMA_COLOR, label='γ')
axis1.plot(days, self.__mu_values, Model.__MU_COLOR, label='μ')
axis1.legend(loc='best')
plt.xlabel('time (day)')
if show_figure:
plt.show()
def movie(self, filename, batch_filter=True, nb_of_days=100):
"""
Generate, if using the data, a movie showing the evolution of our SIRD model throughout time.
"""
if self.__use_data:
data_size = Model.__DATA.shape[0]
figure = plt.figure(figsize=Model.__FIG_SIZE)
backend = matplotlib.get_backend()
writer = manimation.writers['ffmpeg']()
matplotlib.use("Agg")
with writer.saving(figure, filename, 96):
for i in range(1, data_size + 1):
print('Processing frame #', i, '/', data_size, '...', sep='')
self.__data = Model.__DATA[:i]
self.reset()
self.run(batch_filter=batch_filter, nb_of_days=nb_of_days)
self.plot(figure=figure)
writer.grab_frame()
print('All done!')
matplotlib.use(backend)
def s(self, day=-1):
"""
Return all the S values (if day=-1) or its value for a given day.
"""
if day == -1:
return self.__s_values
else:
return self.__s_values[day]
def i(self, day=-1):
"""
Return all the I values (if day=-1) or its value for a given day.
"""
if day == -1:
return self.__i_values
else:
return self.__i_values[day]
def r(self, day=-1):
"""
Return all the R values (if day=-1) or its value for a given day.
"""
if day == -1:
return self.__r_values
else:
return self.__r_values[day]
def d(self, day=-1):
"""
Return all the D values (if day=-1) or its value for a given day.
"""
if day == -1:
return self.__d_values
else:
return self.__d_values[day]
class Follower(BehaviorBase):
def __init__(self, g, zeta,
leader, trajectory_delay=2.0,
update_delta_t=0.01,
orig_leader=None, orig_leader_delay=None,
noise_sigma=0.0, log_file=None,
visibility_fov=config.FOV_ANGLE, visibility_radius=None,
id=None,
dt=0.02):
"""
Construct a follower Behavior.
leader is the Bot to be followed
g > 0 is a tuning parameter
zeta in (0, 1) is a damping coefficient
trajectory_delay is the time interval between leader and follower
"""
super(Follower, self).__init__()
assert(isinstance(leader, Bot))
assert(isinstance(orig_leader, Bot))
self.radius = config.BOT_RADIUS
self.leader = leader
self.orig_leader = orig_leader
self.trajectory_delay = trajectory_delay
assert g > 0, "Follower: g parameter must be positive"
self.g = g
assert 0 < zeta < 1, "Follower: zeta parameter must be in (0, 1)"
self.zeta = zeta
# leader_positions stores config.POSITIONS_BUFFER_SIZE tuples;
# tuple's first field is time, second is the
# corresponding position of the leader
self.leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)
self.noisy_leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)
self.leader_is_visible = False
# precise_orig_leader_states stores the first leader's precise state;
# used to calculate "real" errors (as opposed to calculations
# w.r.t. the approximation curve)
self.precise_orig_leader_states = []
# precise_leader_states is used for the same purpose, but with the bot's own leader
self.precise_leader_states = []
self.update_delta_t = update_delta_t
needed_buffer_size = config.SAMPLE_COUNT // 2 + self.trajectory_delay / self.update_delta_t
if needed_buffer_size > config.POSITIONS_BUFFER_SIZE:
sys.stderr.write("leader_positions buffer is too small for update_delta_t = {}".format(self.update_delta_t) +
" (current = {}, required = {})".format(config.POSITIONS_BUFFER_SIZE, needed_buffer_size))
raise RuntimeError, "leader_positions buffer is too small"
self.last_update_time = 0.0
self.noise_sigma = noise_sigma
self.log_file = log_file
self.visibility_fov = visibility_fov
if visibility_radius is None:
visibility_radius = 2.0 * trajectory_delay * config.BOT_VEL_CAP
self.visibility_radius = visibility_radius
self.id = id;
if orig_leader_delay is None:
orig_leader_delay = trajectory_delay
self.orig_leader_delay = orig_leader_delay
if self.log_file is not None:
log_dict = {"id": self.id,
"g": self.g,
"zeta": self.zeta,
"noise_sigma": self.noise_sigma,
"reference_points_cnt": config.SAMPLE_COUNT,
"trajectory_delay": trajectory_delay}
print >> self.log_file, log_dict
q = 0.01 # std of process
r = 0.05 # std of measurement
self.delta_time = dt
if not config.DISABLE_UKF:
points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=-3)
self.ukf = UKF(dim_x=6, dim_z=2, fx=f, hx=h, dt=dt, points=points)
self.ukf_x_initialized = 0
#self.ukf.x = np.array([0, 0, 1, pi/4, 0, 0])
self.ukf.R = np.diag([r, r]);
self.ukf.Q = np.diag([0, 0, 0, 0, q, q])
def point_in_fov(self, p):
if length(p - self.pos) > self.visibility_radius:
return False
d = p - self.pos
ang = atan2(cross(self.real_dir, d), dot(self.real_dir, d))
return -0.5 * self.visibility_fov < ang < 0.5 * self.visibility_fov
def point_is_visible(self, p, obstacles):
if not self.point_in_fov(p):
return False
try:
ray = Ray(self.pos, p - self.pos)
except NormalizationError:
ray = Ray(self.pos, Vector(1.0, 0.0))
i = first_intersection(ray, obstacles)
return (i is None) or (length(i - self.pos) > length(p - self.pos))
def store_leaders_state(self, engine, obstacles):
if self.point_is_visible(self.leader.real.pos, obstacles):
self.leader_is_visible = True
noisy_pos = self.leader.real.pos
noisy_pos += Vector(gauss(0.0, self.noise_sigma),
gauss(0.0, self.noise_sigma))
if config.DISABLE_UKF:
filtered_pos = noisy_pos
else:
if (self.ukf_x_initialized == 0):
self.ukf_x1 = noisy_pos
self.ukf_x_initialized += 1
filtered_pos = noisy_pos
self.ukf.x = np.array([noisy_pos.x, noisy_pos.y,
0, pi/2, 0, 0])
#elif (self.ukf_x_initialized == 1):
# self.ukf_x2 = noisy_pos
# self.ukf_x_initialized += 1
# self.ukf.x = np.array([noisy_pos.x, noisy_pos.y,
# length((noisy_pos - self.ukf_x1) / self.delta_time),
# atan2(noisy_pos.y - self.ukf_x1.y,
# noisy_pos.x - self.ukf_x1.x),
# 0, 0])
# filtered_pos = noisy_pos
else: # UKF is initialized
self.ukf.predict()
self.ukf.update(np.array(noisy_pos))
filtered_pos = Point(self.ukf.x[0], self.ukf.x[1])
self.leader_noisy_pos = noisy_pos
self.noisy_leader_positions.append(TimedPosition(engine.time, noisy_pos))
self.leader_positions.append(TimedPosition(engine.time, filtered_pos))
self.last_update_time = engine.time
else:
self.leader_is_visible = False
if not config.DISABLE_UKF:
self.ukf.predict()
#TODO: do magic with UKF?
orig_leader_theta = atan2(self.orig_leader.real.dir.y,
self.orig_leader.real.dir.x)
self.precise_orig_leader_states.append(TimedState(time=engine.time,
pos=self.orig_leader.real.pos,
theta=orig_leader_theta))
leader_theta = atan2(self.leader.real.dir.y,
self.leader.real.dir.x)
self.precise_leader_states.append(TimedState(time=engine.time,
pos=self.leader.real.pos,
theta=leader_theta))
def polyfit_trajectory(self, pos_data, t):
x_pos = np.array([el.pos.x for el in pos_data])
y_pos = np.array([el.pos.y for el in pos_data])
times = np.array([el.time for el in pos_data])
# needed for trajectory rendering
self.t_st = times[0]
self.t_fn = max(times[-1], t)
# calculate quadratic approximation of the reference trajectory
x_poly = np.polyfit(times, x_pos, deg=2)
y_poly = np.polyfit(times, y_pos, deg=2)
known = Trajectory.from_poly(x_poly, y_poly)
return known, x_pos[-1], y_pos[-1], times[-1]
def extend_trajectory(self, known, last_x, last_y, last_t):
# now adding a circle to the end of known trajectory
# k is signed curvature of the trajectry at t_fn
try:
k = known.curvature(last_t)
except (ValueError, ZeroDivisionError):
k = config.MIN_CIRCLE_CURVATURE
if abs(k) < config.MIN_CIRCLE_CURVATURE:
k = copysign(config.MIN_CIRCLE_CURVATURE, k)
if abs(k) > config.MAX_CURVATURE:
k = copysign(config.MAX_CURVATURE, k)
radius = abs(1.0/k)
# trajectory direction at time t_fn
try:
d = normalize(Vector(known.dx(last_t), known.dy(last_t)))
except NormalizationError:
d = self.real_dir
r = Vector(-d.y, d.x) / k
center = Point(last_x, last_y) + r
phase = atan2(-r.y, -r.x)
#freq = known.dx(last_t) / r.y
freq = k
self.x_approx = lambda time: known.x(time) if time < last_t else \
center.x + radius * cos(freq * (time - last_t) + phase)
self.y_approx = lambda time: known.y(time) if time < last_t else \
center.y + radius * sin(freq * (time - last_t) + phase)
dx = lambda time: known.dx(time) if time < last_t else \
-radius * freq * sin(freq * (time - last_t) + phase)
dy = lambda time: known.dy(time) if time < last_t else \
radius * freq * cos(freq * (time - last_t) + phase)
ddx = lambda time: known.ddx(time) if time < last_t else \
-radius * freq * freq * cos(freq * (time - last_t) + phase)
ddy = lambda time: known.ddy(time) if time < last_t else \
-radius * freq * freq * sin(freq * (time - last_t) + phase)
# FIXME: don't use division by y if y == 0
try:
if isnan(self.x_approx(last_t + 1)):
return known
except Exception:
return known
return Trajectory(x=self.x_approx, y=self.y_approx,
dx=dx, dy=dy, ddx=ddx, ddy=ddy)
def generate_trajectory(self, leader_positions, t):
arr = get_interval(self.leader_positions, t, config.SAMPLE_COUNT)
self.traj_interval = arr
if len(arr) == 0:
return None
known, last_x, last_y, last_t = self.polyfit_trajectory(arr, t)
if config.DISABLE_CIRCLES:
return known
else:
return self.extend_trajectory(known, last_x, last_y, last_t)
def calc_desired_velocity(self, bots, obstacles, targets, engine):
# update trajectory
if engine.time - self.last_update_time > self.update_delta_t:
self.store_leaders_state(engine, obstacles)
# reduce random movements at the start
# TODO: this causes oscillations that smash bots into each other.
# Change to something smoother. PID control?
#if self.leader_is_visible and length(self.pos - self.leader_noisy_pos) < MIN_DISTANCE_TO_LEADER:
# return Instr(0.0, 0.0)
t = engine.time - self.trajectory_delay
self.trajectory = self.generate_trajectory(self.leader_positions, t)
if self.trajectory is None:
return Instr(0.0, 0.0)
dx = self.trajectory.dx
dy = self.trajectory.dy
ddx = self.trajectory.ddx
ddy = self.trajectory.ddy
# calculate the feed-forward velocities
v_fun = lambda time: sqrt(dx(time)**2 + dy(time)**2)
#omega_fun = lambda time: (dx(time) * 2 * y_poly[0] - dy(time) * 2 * x_poly[0]) / (dx(time)**2 + dy(time)**2)
omega_fun = lambda time: (dx(time) * ddy(time) - dy(time) * ddx(time)) / (dx(time)**2 + dy(time)**2)
v_ff = v_fun(t)
omega_ff = omega_fun(t)
# x_r, y_r and theta_r denote the reference robot's state
r = State(x=self.trajectory.x(t),
y=self.trajectory.y(t),
theta=atan2(self.trajectory.dy(t),
self.trajectory.dx(t)))
self.target_point = Point(r.x, r.y)
if isnan(v_ff):
v_ff = 0.0
if isnan(omega_ff):
omega_ff = 0.0
# cur is the current state
cur = State(x=self.pos.x,
y=self.pos.y,
theta=atan2(self.real_dir.y, self.real_dir.x))
# error in the global (fixed) reference frame
delta = State(x=r.x - cur.x,
y=r.y - cur.y,
theta=normalize_angle(r.theta - cur.theta))
# translate error into the follower's reference frame
e = State(x= cos(cur.theta) * delta.x + sin(cur.theta) * delta.y,
y=-sin(cur.theta) * delta.x + cos(cur.theta) * delta.y,
theta=delta.theta)
# calculate gains k_x, k_y, k_theta
# these are just parameters, not a state in any sense!
omega_n = sqrt(omega_ff**2 + self.g * v_ff**2)
k_x = 2 * self.zeta * omega_n
k_y = self.g
k_theta = 2 * self.zeta * omega_n
# calculate control velocities
v = v_ff * cos(e.theta) + k_x * e.x
se = sin(e.theta) / e.theta if e.theta != 0 else 1.0
omega = omega_ff + k_y * v_ff * se * e.y + k_theta * e.theta
if config.DISABLE_FEEDBACK:
v = v_ff
omega = omega_ff
real_e = State(0.0, 0.0, 0.0)
try:
orig_t = engine.time - self.orig_leader_delay
orig_leader_state = lerp_precise_states(orig_t, self.precise_orig_leader_states)
real_e = State(x=orig_leader_state.x - cur.x,
y=orig_leader_state.y - cur.y,
theta=normalize_angle(orig_leader_state.theta - cur.theta))
except LerpError:
# not enough data is yet available to calculate error
pass
try:
leader_t = engine.time - self.trajectory_delay
leader_state = lerp_precise_states(leader_t, self.precise_leader_states)
approx_e = State(x=leader_state.x - cur.x,
y=leader_state.y - cur.y,
theta=normalize_angle(leader_state.theta - cur.theta))
except LerpError:
# same as above
pass
if self.log_file is not None:
log_dict = {"id": self.id,
"time": engine.time,
"delta_time": engine.time_since_last_bot_update,
"x": cur.x,
"y": cur.y,
"theta": cur.theta,
"v": v,
"omega": omega,
"v_ff": v_ff,
"omega_ff": omega_ff,
"e_x": e.x,
"e_y": e.y,
"e_theta": e.theta,
"real_e_x": real_e.x,
"real_e_y": real_e.y,
"real_e_theta": real_e.theta,
"approx_e_x": approx_e.x,
"approx_e_y": approx_e.y,
"approx_e_theta": approx_e.theta,
"leader_is_visible": self.leader_is_visible
}
print >> self.log_file, log_dict
return Instr(v, omega)
def draw(self, screen, field):
if config.DISPLAYED_POINTS_COUNT > 0:
k = config.POSITIONS_BUFFER_SIZE / config.DISPLAYED_POINTS_COUNT
if k == 0:
k = 1
for index, (time, point) in enumerate(self.leader_positions):
if index % k == 0:
draw_circle(screen, field, config.TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
for index, (time, point) in enumerate(self.noisy_leader_positions):
if index % k == 0:
draw_circle(screen, field, config.NOISY_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
if config.DISPLAYED_USED_POINTS_COUNT > 0:
k = len(self.traj_interval) / config.DISPLAYED_USED_POINTS_COUNT
if k == 0:
k = 1
for index, (time, point) in enumerate(self.traj_interval):
if index % k == 0:
draw_circle(screen, field, config.USED_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
if config.DRAW_DELAYED_LEADER_POS:
try:
orig_leader_dir = Vector(cos(self.orig_leader_theta),
sin(self.orig_leader_theta))
draw_directed_circle(screen, field, (0, 255, 0),
self.orig_leader_pos, 0.2,
orig_leader_dir, 1)
except AttributeError:
pass
if config.DRAW_SENSOR_RANGE:
ang = atan2(self.real_dir.y, self.real_dir.x)
draw_arc(screen, field, config.SENSOR_COLOR, self.pos, self.visibility_radius,
ang - 0.5 * self.visibility_fov,
ang + 0.5 * self.visibility_fov,
1)
draw_line(screen, field, config.SENSOR_COLOR,
self.pos,
self.pos + rotate(self.real_dir * self.visibility_radius,
0.5 * self.visibility_fov),
1)
draw_line(screen, field, config.SENSOR_COLOR,
self.pos,
self.pos + rotate(self.real_dir * self.visibility_radius,
-0.5 * self.visibility_fov),
1)
try:
if config.DRAW_VISIBILITY and self.leader_is_visible:
draw_circle(screen, field, config.config.VISIBILITY_COLOR, self.leader.real.pos,
0.5 * config.BOT_RADIUS)
except AttributeError:
pass
try:
if config.DRAW_REFERENCE_POS:
draw_circle(screen, field, config.TARGET_POINT_COLOR, self.target_point, 0.2)
if config.DRAW_APPROX_TRAJECTORY and self.trajectory is not None:
#if len(self.traj_interval) > 1:
# p2 = Point(self.traj_interval[0].pos.x,
# self.traj_interval[0].pos.y)
# for t, p in self.traj_interval:
# draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
# p2 = p
step = (self.t_fn - self.t_st) / config.TRAJECTORY_SEGMENT_COUNT
for t in (self.t_st + k * step for k in xrange(config.TRAJECTORY_SEGMENT_COUNT)):
p = Point(self.trajectory.x(t),
self.trajectory.y(t))
p2 = Point(self.trajectory.x(t + step),
self.trajectory.y(t + step))
draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
p_st = Point(self.trajectory.x(self.t_st), self.trajectory.y(self.t_st))
p_fn = Point(self.trajectory.x(self.t_fn), self.trajectory.y(self.t_fn))
step = 0.5 / config.TRAJECTORY_SEGMENT_COUNT
p = p_fn
p2 = p
t = self.t_fn
it = 0
while it < config.TRAJECTORY_SEGMENT_COUNT and min(dist(p, p_fn), dist(p, p_st)) < 1.0:
it += 1
t += step
p2 = p
p = Point(self.trajectory.x(t), self.trajectory.y(t))
draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
except AttributeError as e: # approximation hasn't been calculated yet
pass
def test_rts():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=1.)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
for i in range(len(t)):
r = radar.get_range()
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3) * 100
M, P = kf.batch_filter(rs)
assert np.array_equal(M, xs), "Batch filter generated different output"
Qs = [kf.Q] * len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.plot(t, M2[:, 0], c='g')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.plot(t, M2[:, 1], c='g')
plt.subplot(313)
plt.plot(t, xs[:, 2])
plt.plot(t, M2[:, 2], c='g')
Qd = calc_Qd(rhat, xhat)
Q_r = Qd[0:PARTICLE_DIM, 0:PARTICLE_DIM]
Q_x = Qd[PARTICLE_DIM:STATE_DIM, PARTICLE_DIM:STATE_DIM]
S = Qd[PARTICLE_DIM:STATE_DIM, 0:PARTICLE_DIM]
T_k0 = S.dot(np.linalg.inv(Q_r))
Q_x -= T_k0.dot(Q_r.dot(T_k0.T)) # noise decorrelation
return Q_x, Q_r, T_k0
x_points = MerweScaledSigmaPoints(MARGIN_DIM, alpha=.001, beta=2., kappa=0)
for i in range(N):
ukf = UnscentedKalmanFilter(MARGIN_DIM, PARTICLE_DIM, dt, x_measurement,
x_x_noiseless, x_points)
# x estimate
ukf.x = xhat0.reshape(MARGIN_DIM)
ukf.x_prior = ukf.x
# covariance of estimate
ukf.P_prior = P0_x
# get around needing to call predict() before update() (not applicable here)
ukf.sigmas_f = ukf.points_fn.sigma_points(ukf.x, ukf.P_prior)
kfs.append(ukf)
Q_m, R_m, _ = calc_marginalized_Q_R_T(particles[:, i:i + 1], xhat0)
ukf.Q = Q_m
ukf.R = R_m
def test_fixed_lag():
def fx(x, dt):
A = np.eye(3) + dt * np.array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return np.sqrt (x[0]**2 + x[2]**2)
dt = 0.05
kf = UKF(3, 1, dt, fx=fx, hx=hx, kappa=0.)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 1.
radar = RadarSim(dt)
t = np.arange(0,20+dt, dt)
n = len(t)
xs = np.zeros((n,3))
random.seed(200)
rs = []
#xs = []
M = []
P = []
N =10
flxs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i,:] = kf.x
flxs.append(kf.x)
rs.append(r)
M.append(kf.x)
P.append(kf.P)
print(i)
#print(i, np.asarray(flxs)[:,0])
if i == 20 and len(M) >= N:
try:
M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:])
flxs[-N:] = M2
#flxs[-N:] = [20]*N
except:
print('except', i)
#P[-N:] = P2
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3)*100
M, P = kf.batch_filter(rs)
Qs = [kf.Q]*len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
flxs = np.asarray(flxs)
print(xs[:,0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:,0])
plt.plot(t, flxs[:,0], c='r')
plt.plot(t, M2[:,0], c='g')
plt.subplot(312)
plt.plot(t, xs[:,1])
plt.plot(t, flxs[:,1], c='r')
plt.plot(t, M2[:,1], c='g')
plt.subplot(313)
plt.plot(t, xs[:,2])
plt.plot(t, flxs[:,2], c='r')
plt.plot(t, M2[:,2], c='g')
def two_radar():
# code is not complete - I was using to test RTS smoother. very similar
# to two_radary.py in book.
import numpy as np
import matplotlib.pyplot as plt
from numpy import array
from numpy.linalg import norm
from numpy.random import randn
from math import atan2
from filterpy.common import Q_discrete_white_noise
class RadarStation(object):
def __init__(self, pos, range_std, bearing_std):
self.pos = asarray(pos)
self.range_std = range_std
self.bearing_std = bearing_std
def reading_of(self, ac_pos):
""" Returns range and bearing to aircraft as tuple. bearing is in
radians.
"""
diff = np.subtract(self.pos, ac_pos)
rng = norm(diff)
brg = atan2(diff[1], diff[0])
return rng, brg
def noisy_reading(self, ac_pos):
rng, brg = self.reading_of(ac_pos)
rng += randn() * self.range_std
brg += randn() * self.bearing_std
return rng, brg
class ACSim(object):
def __init__(self, pos, vel, vel_std):
self.pos = asarray(pos, dtype=float)
self.vel = asarray(vel, dtype=float)
self.vel_std = vel_std
def update(self):
vel = self.vel + (randn() * self.vel_std)
self.pos += vel
return self.pos
dt = 1.
def hx(x):
r1, b1 = hx.R1.reading_of((x[0], x[2]))
r2, b2 = hx.R2.reading_of((x[0], x[2]))
return array([r1, b1, r2, b2])
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1]*dt
x_est[2] += x[3]*dt
return x_est
vx, vy = 0.1, 0.1
f = UnscentedKalmanFilter(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
aircraft = ACSim((100, 100), (vx*dt, vy*dt), 0.00000002)
range_std = 0.001 # 1 meter
bearing_std = 1./1000 # 1mrad
R1 = RadarStation((0, 0), range_std, bearing_std)
R2 = RadarStation((200, 0), range_std, bearing_std)
hx.R1 = R1
hx.R2 = R2
f.x = array([100, vx, 100, vy])
f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2])
q = Q_discrete_white_noise(2, var=0.0002, dt=dt)
f.Q[0:2, 0:2] = q
f.Q[2:4, 2:4] = q
f.P = np.diag([.1, 0.01, .1, 0.01])
track = []
zs = []
for i in range(int(300/dt)):
pos = aircraft.update()
r1, b1 = R1.noisy_reading(pos)
r2, b2 = R2.noisy_reading(pos)
z = np.array([r1, b1, r2, b2])
zs.append(z)
track.append(pos.copy())
zs = asarray(zs)
xs, Ps, Pxz, pM, pP = f.batch_filter(zs)
ms, _, _ = f.rts_smoother(xs, Ps)
track = asarray(track)
time = np.arange(0, len(xs) * dt, dt)
plt.figure()
plt.subplot(411)
plt.plot(time, track[:, 0])
plt.plot(time, xs[:, 0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('x position (m)')
plt.tight_layout()
plt.subplot(412)
plt.plot(time, track[:, 1])
plt.plot(time, xs[:, 2])
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.ylabel('y position (m)')
plt.tight_layout()
plt.subplot(413)
plt.plot(time, xs[:, 1])
plt.plot(time, ms[:, 1])
plt.legend(loc=4)
plt.ylim([0, 0.2])
plt.xlabel('time (sec)')
plt.ylabel('x velocity (m/s)')
plt.tight_layout()
plt.subplot(414)
plt.plot(time, xs[:, 3])
plt.plot(time, ms[:, 3])
plt.ylabel('y velocity (m/s)')
plt.legend(loc=4)
plt.xlabel('time (sec)')
plt.tight_layout()
plt.show()
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1]+x[2], x[2]])
std_noise = .1
f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, kappa=0)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2)*(std_noise**2)
f.Q = np.eye(3)*.001
f.Q[0,0]=0
f.Q[2,2]=0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
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.]])
kf.F = F
kf.P*= 100
kf.H = np.array([[1,0,0,0,0,0],
[0,0,0,1,0,0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
measurements = []
results = []
zs = []
kfxs = []
for t in range (0,12000):
a = t / 30 + 90
x = cos(radians(a)) * 50.+ randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x,y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append (hx(f.x))
kfxs.append(kf.x)
#print(f.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:,0], zs[:,1], c='r', label='z')
plt.plot(results[:,0], results[:,1], c='k', label='UKF')
plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
def main():
# Create a random points to interpolate into a path
x = np.arange(0, 30, 1) # x coord
x = np.append(x, x[::-1])
y = np.ones(len(x))
# designing course
# TODO: function to create courses
y = [x * 2 for x in x[:15]]
y.extend([x * 3 for x in x[15:30]])
y.extend([x * 1.2 for x in x[30:55]])
y.extend([x % 5 for x in x[55:60]])
y[-5:] = [-1, -1, -1, -1, -2]
# prepare map for func
map_ = np.vstack((x, y))
# draw map,landmarks & get trajectory
path = make_map(map_, landmarks=5, random_seed=42)
lmark_pos = np.array(make_map.landmarks)
# round path for controller
x_path = [round(x, 4) for x in path[0][:]]
y_path = [round(y, 4) for y in path[1][:]]
# state and PID object
state = State(x=x_path[0], y=y_path[0], yaw=np.radians(90.0), v=0.0)
pd = PID(Kp=0.5, Ki=0.1, Kd=-0.15)
target_speed = 30.0 / 3.6 # km/h - > [m/s]
# Lists to store
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
lat_error = [0.0]
# Setup Initial values
target_index, _ = calc_target_index(state, x_path, y_path)
last_index = len(x_path) - 1
max_sim_time = 50.0
dt = 0.1
time = 0.0
show_animation = True
# setup UKF
points = MerweScaledSigmaPoints(n=3,
alpha=1e-4,
kappa=0.0,
beta=2,
subtract=residual_x)
sigma_range = 0.3
sigma_bearing = 0.1
ukf = UKF(dim_x=3,
dim_z=2 * len(lmark_pos),
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * 5) # 5 landmarks
ukf.Q = np.eye(3) * 0.0001
while time <= max_sim_time and last_index > target_index:
#TODO: make stanley model work with ukf
ukf.predict(u=u, wheelbase=wheelbase)
if time % 10 == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
# set up controls
ai = pd.pid_control(target_speed, state.v, lat_error[-1], time)
di, target_index = stanley(state, x_path, y_path, target_index)
state.update(ai, di)
time += dt
# store data for plotting
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
lat_error.append(stanley.lat_error)
# speed up time if oscilaltions
if stanley.lat_error > abs(1.5):
time += 1
if show_animation:
simple_animation(path, [x, y], lmark_pos, time, max_sim_time)
assert last_index >= target_index, "Cannot reach goal"
if show_animation:
#plt.plot(path[0][:],path[1][:], ".r", label = 'course')
#plt.plot(x,y,'-b',label='trajectory')
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.grid(True)
_, (ax1, ax2) = plt.subplots(2, sharex='row')
ax1.plot(t, [iv * 3.6 for iv in v], "-r")
ax1.set_ylabel("Speed[km/h]")
ax1.grid(True)
ax2.plot(t, lat_error, label='lateral error to next [m]')
ax2.set_ylabel("Lateral Error")
ax2.set_xlabel("Time[s]")
plt.grid(True)
plt.show()
def h_cv(x):
return np.array([x[0], x[2]])
def e(x):
res = []
for n in range(x.shape[0]):
res.append(np.sqrt(x[n][0]**2 + x[n][2]**2))
return res
dt = 1.0
random.seed(1234)
ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, kappa=0)
ukf.x = np.array([100., 0., 0., 0.])
ukf.R = np.diag([25, 25])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt, var=0.04)
ckf = CubatureKalmanFilter(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt)
ckf.x = np.array([100., 0., 0., 0.])
ckf.R = np.diag([25, 25])
ckf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt, var=0.04)
ckf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt, var=0.04)
uxs = []
pxs = []
zs = []
txs = []
def hx(x):
return (x[0]**2 + x[2]**2)**.5
pass
def fx(x, dt):
result = x.copy()
result[0] += x[1]*dt
return result
f = UKF(3, 1, dt= dt, hx=hx, fx=fx, kappa=1)
radar = RadarSim(dt, pos=-1000., vel=100., alt=1000.)
f.x = array([0, 90, 1005])
f.R = 0.1
f.Q *= 0.002
xs = []
track = []
for i in range(int(20/dt)):
z = radar.get_range()
track.append((radar.pos, radar.vel, radar.alt))
class StateEstimation(object):
'''
Class that estimates the state of the drone using an Unscented Kalman Filter
(UKF) applied to raw sensor data.
'''
# TODO: Make a reference to the UKF math document that is being written up,
# once it is in a complete enough state and can be placed in a shared
# location.
def __init__(self, ir_throttled=False, imu_throttled=False, optical_flow_throttled=False):
# self.ready_to_filter is False until we get initial measurements in
# order to be able to initialize the filter's state vector x and
# covariance matrix P.
self.ready_to_filter = False
self.printed_filter_start_notice = False
self.got_ir = False
self.got_optical_flow = False
self.ir_topic_str = '/pidrone/infrared'
self.imu_topic_str = '/pidrone/imu'
self.optical_flow_topic_str = '/pidrone/picamera/twist'
throttle_suffix = '_throttle'
if ir_throttled:
self.ir_topic_str += throttle_suffix
if imu_throttled:
self.imu_topic_str += throttle_suffix
if optical_flow_throttled:
self.optical_flow_topic_str += throttle_suffix
self.in_callback = False
self.initialize_ukf()
# The last time that we received an input and formed a prediction with
# the state transition function
self.last_state_transition_time = None
# Time in seconds between consecutive state transitions, dictated by
# when the inputs come in
self.dt = None
# Initialize the last control input as 0 m/s^2 along each axis
self.last_control_input = np.array([0.0, 0.0, 0.0])
self.initialize_ros()
def initialize_ros(self):
'''
Initialize ROS-related objects, e.g., the node, subscribers, etc.
'''
self.node_name = 'state_estimator_ukf_test_2'
print 'Initializing {} node...'.format(self.node_name)
rospy.init_node(self.node_name)
# Subscribe to topics to which the drone publishes in order to get raw
# data from sensors, which we can then filter
rospy.Subscriber(self.imu_topic_str, Imu, self.imu_data_callback)
rospy.Subscriber(self.ir_topic_str, Range, self.ir_data_callback)
rospy.Subscriber(self.optical_flow_topic_str, TwistStamped,
self.optical_flow_data_callback)
# Create the publisher to publish state estimates
self.state_pub = rospy.Publisher('/pidrone/state', State, queue_size=1,
tcp_nodelay=False)
def initialize_ukf(self):
'''
Initialize the parameters of the Unscented Kalman Filter (UKF) that is
used to estimate the state of the drone.
'''
# Number of state variables being tracked
self.state_vector_dim = 6
# The state vector consists of the following column vector.
# Note that FilterPy initializes the state vector with zeros.
# [[x],
# [y],
# [z],
# [x_vel],
# [y_vel],
# [z_vel]]
# Number of measurement variables that the drone receives
self.measurement_vector_dim = 3
# The measurement variables consist of the following vector:
# [[slant_range],
# [x_vel],
# [y_vel]]
# Function to generate sigma points for the UKF
# TODO: Modify these sigma point parameters appropriately. Currently
# just guesses
sigma_points = MerweScaledSigmaPoints(n=self.state_vector_dim,
alpha=0.1,
beta=2.0,
kappa=(3.0-self.state_vector_dim))
# Create the UKF object
# Note that dt will get updated dynamically as sensor data comes in,
# as will the measurement function, since measurements come in at
# distinct rates. Setting compute_log_likelihood=False saves some
# computation.
self.ukf = UnscentedKalmanFilter(dim_x=self.state_vector_dim,
dim_z=self.measurement_vector_dim,
dt=1.0,
hx=self.measurement_function,
fx=self.state_transition_function,
points=sigma_points,
compute_log_likelihood=False)
self.initialize_ukf_matrices()
def initialize_ukf_matrices(self):
'''
Initialize the covariance matrices of the UKF
'''
# Initialize state covariance matrix P:
# TODO: Tune these initial values appropriately. Currently these are
# just guesses
self.ukf.P = np.diag([0.1, 0.1, 0.1, 0.2, 0.2, 0.2])
# Initialize the process noise covariance matrix Q:
# TODO: Tune appropriately. Currently just a guess
self.ukf.Q = np.diag([0.01, 0.01, 0.01, 1.0, 1.0, 1.0])*0.005
# Initialize the measurement covariance matrix R
# IR slant range variance (m^2), determined experimentally in a static
# setup with mean range around 0.335 m:
self.measurement_cov_ir = np.array([2.2221e-05])
self.measurement_cov_optical_flow = np.diag([0.01, 0.01])
def update_input_time(self, msg):
'''
Update the time at which we have received the most recent input, based
on the timestamp in the header of a ROS message
msg : a ROS message that includes a header with a timestamp that
indicates the time at which the respective input was originally
recorded
'''
self.last_time_secs = msg.header.stamp.secs
self.last_time_nsecs = msg.header.stamp.nsecs
new_time = self.last_time_secs + self.last_time_nsecs*1e-9
# Compute the time interval since the last state transition / input
self.dt = new_time - self.last_state_transition_time
# Set the current time at which we just received an input
# to be the last input time
self.last_state_transition_time = new_time
def initialize_input_time(self, msg):
'''
Initialize the input time (self.last_state_transition_time) based on the
timestamp in the header of a ROS message. This is called before we start
filtering in order to attain an initial time value, which enables us to
then compute a time interval self.dt by calling self.update_input_time()
msg : a ROS message that includes a header with a timestamp
'''
self.last_time_secs = msg.header.stamp.secs
self.last_time_nsecs = msg.header.stamp.nsecs
self.last_state_transition_time = (self.last_time_secs +
self.last_time_nsecs*1e-9)
def ukf_predict(self):
'''
Compute the prior for the UKF based on the current state, a control
input, and a time step.
'''
self.ukf.predict(dt=self.dt, u=self.last_control_input)
def print_notice_if_first(self):
if not self.printed_filter_start_notice:
print 'Starting filter'
self.printed_filter_start_notice = True
def imu_data_callback(self, data):
'''
Handle the receipt of an Imu message. Only take the linear acceleration
along the z-axis.
This method PREDICTS with a control input.
'''
if self.in_callback:
return
self.in_callback = True
self.last_control_input = np.array([data.linear_acceleration.x,
data.linear_acceleration.y,
data.linear_acceleration.z])
# self.last_control_input = np.array([0.0,
# 0.0,
# data.linear_acceleration.z])
if self.ready_to_filter:
# Wait to predict until we get an initial IR measurement to
# initialize our state vector
self.print_notice_if_first()
self.update_input_time(data)
self.ukf_predict()
self.publish_current_state()
self.in_callback = False
def ir_data_callback(self, data):
'''
Handle the receipt of a Range message from the IR sensor.
This method PREDICTS with the most recent control input and UPDATES.
'''
if self.in_callback:
return
self.in_callback = True
if self.ready_to_filter:
self.print_notice_if_first()
self.update_input_time(data)
self.ukf_predict()
# Now that a prediction has been formed to bring the current prior
# state estimate to the same point in time as the measurement,
# perform a measurement update with the slant range reading
measurement_z = np.array([data.range])
self.ukf.update(measurement_z,
hx=self.measurement_function_ir,
R=self.measurement_cov_ir)
self.publish_current_state()
else:
self.initialize_input_time(data)
# Got a raw slant range reading, so update the initial state
# vector of the UKF
self.ukf.x[2] = data.range
self.ukf.x[5] = 0.0 # initialize velocity as 0 m/s
# Update the state covariance matrix to reflect estimated
# measurement error. Variance of the measurement -> variance of
# the corresponding state variable
self.ukf.P[2, 2] = self.measurement_cov_ir[0]
self.got_ir = True
self.check_if_ready_to_filter()
self.in_callback = False
def optical_flow_data_callback(self, data):
'''
Handle the receipt of a TwistStamped message from optical flow.
The message parts that we will be using:
- x velocity (m/s)
- y velocity (m/s)
This method PREDICTS with the most recent control input and UPDATES.
'''
if self.in_callback:
return
self.in_callback = True
if self.ready_to_filter:
self.print_notice_if_first()
self.update_input_time(data)
self.ukf_predict()
# Now that a prediction has been formed to bring the current prior
# state estimate to the same point in time as the measurement,
# perform a measurement update with x velocity, y velocity, and yaw
# velocity data in the TwistStamped message
# TODO: Verify the units of these velocities that are being
# published
measurement_z = np.array([data.twist.linear.x, # x velocity
data.twist.linear.y]) # y velocity
# Ensure that we are using subtraction to compute the residual
#self.ukf.residual_z = np.subtract
self.ukf.update(measurement_z,
hx=self.measurement_function_optical_flow,
R=self.measurement_cov_optical_flow)
self.publish_current_state()
else:
self.initialize_input_time(data)
# Update the initial state vector of the UKF
self.ukf.x[3] = data.twist.linear.x # x velocity
self.ukf.x[4] = data.twist.linear.y # y velocity
# Update the state covariance matrix to reflect estimated
# measurement error. Variance of the measurement -> variance of
# the corresponding state variable
self.ukf.P[3, 3] = self.measurement_cov_optical_flow[0, 0]
self.ukf.P[4, 4] = self.measurement_cov_optical_flow[1, 1]
self.got_optical_flow = True
self.check_if_ready_to_filter()
self.in_callback = False
def check_if_ready_to_filter(self):
self.ready_to_filter = (self.got_ir and self.got_optical_flow)
def publish_current_state(self):
'''
Publish the current state estimate and covariance from the UKF. This is
a State message containing:
- Header
- PoseWithCovariance
- TwistWithCovariance
Note that a lot of these ROS message fields will be left empty, as the
1D UKF does not track information on the entire state space of the
drone.
'''
state_msg = State()
state_msg.header.stamp.secs = self.last_time_secs
state_msg.header.stamp.nsecs = self.last_time_nsecs
state_msg.header.frame_id = 'global'
# Get the current state estimate from self.ukf.x, and fill the rest of
# the message with NaN
state_msg.pose_with_covariance.pose.position.x = self.ukf.x[0]
state_msg.pose_with_covariance.pose.position.y = self.ukf.x[1]
state_msg.pose_with_covariance.pose.position.z = self.ukf.x[2]
# TODO: Leave commented if JS interface Top View animation is desired
# state_msg.pose_with_covariance.pose.orientation.x = np.nan
# state_msg.pose_with_covariance.pose.orientation.y = np.nan
# state_msg.pose_with_covariance.pose.orientation.z = np.nan
# state_msg.pose_with_covariance.pose.orientation.w = np.nan
# TODO: Leave following line if JS interface Top View animation is desired
state_msg.pose_with_covariance.pose.orientation.w = 1
state_msg.twist_with_covariance.twist.linear.x = self.ukf.x[3]
state_msg.twist_with_covariance.twist.linear.y = self.ukf.x[4]
state_msg.twist_with_covariance.twist.linear.z = self.ukf.x[5]
state_msg.twist_with_covariance.twist.angular.x = np.nan
state_msg.twist_with_covariance.twist.angular.y = np.nan
state_msg.twist_with_covariance.twist.angular.z = np.nan
# Prepare covariance matrices
# TODO: Finish populating these matrices, if deemed necessary
# 36-element array, in a row-major order, according to ROS msg docs
pose_cov_mat = np.full((36,), np.nan)
twist_cov_mat = np.full((36,), np.nan)
pose_cov_mat[14] = self.ukf.P[2, 2] # z variance
twist_cov_mat[14] = self.ukf.P[5, 5] # z velocity variance
# Add covariances to message
state_msg.pose_with_covariance.covariance = pose_cov_mat
state_msg.twist_with_covariance.covariance = twist_cov_mat
self.state_pub.publish(state_msg)
def state_transition_function(self, x, dt, u):
'''
The state transition function to compute the prior in the prediction
step, propagating the state to the next time step.
x : current state. A NumPy array
dt : time step. A float
u : control input. A NumPy array
'''
dt2 = dt**2.0
return x + np.array([x[3]*dt + 0.5*u[0]*dt2,
x[4]*dt + 0.5*u[1]*dt2,
x[5]*dt + 0.5*u[2]*dt2,
u[0]*dt,
u[1]*dt,
u[2]*dt])
def measurement_function(self, x):
'''
Transform the state x into measurement space. In this simple model, we
assume that the range measurement corresponds exactly to position along
the z-axis, as we are assuming there is no pitch and roll.
x : current state. A NumPy array
'''
pass
def measurement_function_ir(self, x):
return np.array([x[2]])
def measurement_function_optical_flow(self, x):
return np.array([x[3], x[4]])
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1] + x[2], x[2]])
std_noise = .1
sp = JulierSigmaPoints(n=3, kappa=0.)
f = UnscentedKalmanFilter(dim_x=3, dim_z=2, dt=.01,
hx=hx, fx=fx, points=sp)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2)*(std_noise**2)
f.Q = np.eye(3)*.001
f.Q[0, 0] = 0
f.Q[2, 2] = 0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
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.]])
kf.F = F
kf.P *= 100
kf.H = np.array([[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
results = []
zs = []
kfxs = []
for t in range(12000):
a = t / 30 + 90
x = cos(radians(a)) * 50. + randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x, y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append(hx(f.x))
kfxs.append(kf.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
plt.plot(results[:, 0], results[:, 1], c='k', label='UKF')
plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
def __init__(self, dim, limit, dim_z=2, fx=None, W=None, obs_noise_func=None,
collision_func=None, sampling_period=0.5, kappa=1):
"""
dim : dimension of state
***Assuming dim==3: (x,y,theta), dim==4: (x,y,xdot,ydot), dim==5: (x,y,theta,v,w)
limit : An array of two vectors. limit[0] = minimum values for the state,
limit[1] = maximum value for the state
dim_z : dimension of observation,
fx : x_tp1 = fx(x_t, dt), state dynamic function
W : state noise matrix
obs_noise_func : observation noise matrix function of z
collision_func : collision checking function
n : the number of sigma points
"""
self.dim = dim
self.limit = limit
self.W = W if W is not None else np.zeros((self.dim, self.dim))
self.obs_noise_func = obs_noise_func
self.collision_func = collision_func
def hx(y, agent_state, measure_func=util.relative_measure):
r_pred, alpha_pred, _ = measure_func(y, agent_state)
return np.array([r_pred, alpha_pred])
def x_mean_fn_(sigmas, Wm):
if dim == 3:
x = np.zeros(dim)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
x[1] += s[1] * Wm[i]
sum_sin += np.sin(s[2])*Wm[i]
sum_cos += np.cos(s[2])*Wm[i]
x[2] = np.arctan2(sum_sin, sum_cos)
return x
elif dim == 5:
x = np.zeros(dim)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
x[1] += s[1] * Wm[i]
x[3] += s[3] * Wm[i]
x[4] += s[4] * Wm[i]
sum_sin += np.sin(s[2])*Wm[i]
sum_cos += np.cos(s[2])*Wm[i]
x[2] = np.arctan2(sum_sin, sum_cos)
return x
else:
return None
def z_mean_fn_(sigmas, Wm):
x = np.zeros(dim_z)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
sum_sin += np.sin(s[1])*Wm[i]
sum_cos += np.cos(s[1])*Wm[i]
x[1] = np.arctan2(sum_sin, sum_cos)
return x
def residual_x_(x, xp):
"""
x : state, [x, y, theta]
xp : predicted state
"""
if dim == 3 or dim == 5:
r_x = x - xp
r_x[2] = util.wrap_around(r_x[2])
return r_x
else:
return None
def residual_z_(z, zp):
"""
z : observation, [r, alpha]
zp : predicted observation
"""
r_z = z - zp
r_z[1] = util.wrap_around(r_z[1])
return r_z
sigmas = JulierSigmaPoints(n=dim, kappa=kappa)
self.ukf = UnscentedKalmanFilter(dim, dim_z, sampling_period, fx=fx, hx=hx,
points=sigmas, x_mean_fn=x_mean_fn_, z_mean_fn=z_mean_fn_,
residual_x=residual_x_, residual_z=residual_z_)
def hx(x):
""" returns slant range based on downrange distance and altitude"""
return (x[0]**2 + x[2]**2)**.5
from filterpy.kalman.sigma_points import JulierSigmaPoints as sigmapoints
if __name__ == "__main__":
dt = 0.05
points = sigmapoints(n=3, kappa = 0);
radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, hx = hx, fx = fx, points = points);
radarUKF.Q *= Q_discrete_white_noise(3, 1, .01)
radarUKF.R *= 10
radarUKF.x = np.array([0., 90., 1100.])
radarUKF.P *= 100.
t = np.arange(0, 20+dt, dt)
n = len(t)
xs = []
rs = []
for i in range(n):
r = GetRadar(dt)
rs.append(r)
#rs = r;
radarUKF.predict();
radarUKF.update(r)
from filterpy.common import stats
import matplotlib.pyplot as plt
p = (-10, -10)
def hx(x):
dx = x[0] - hx.p[0]
dy = x[1] - hx.p[1]
return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5])
def fx(x,dt):
return x
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.)
kf.x = np.array([100, 100.])
kf.P *= 40
hx.p = kf.x - np.array([50,50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='y', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='y', label='Initial')
kf.R[0,0] = radians (1)**2
kf.R[1,1] = 2.**2
class RearEndKalman:
def __init__(self, trueTrajectory, dt=.1, noise = 0.):
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
self.dt = dt
sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.)
self.KF = UKF(dim_x=3, dim_z=2, fx=f_kal_a, hx=h_kal,
dt=dt, points=sigmas)
self.KF.Q = np.diag([1., 0.5, 0.2])
self.KF.R = np.diag([2., 1.12])*noise + np.diag([.05, .05])
self.first = True
def predict(self, vData, predictTimes):
nobs = vData.shape[0]
vcurrentState = vData.iloc[nobs-1].copy()
# first check if vehicle has already stopped, don't need KF then
if np.mean(vData['speed'].iloc[nobs-2:nobs]) <= 0:
returnedStates = vData[vData['time']<0] # empty
for time in predictTimes:
vNextState = vcurrentState.copy()
vNextState.time = time
vNextState.y = -8.25
returnedStates = returnedStates.append(vNextState)
return returnedStates
# # train KF
# self.KF.x[0] = vData.iloc[0].x
# self.KF.x[1] = vData.iloc[0].speed
# self.KF.predict()
#
# for time in np.arange(1,nobs-1):
# vState = vData.iloc[time]
# self.KF.update(np.array([vState.x,vState.speed]))
# self.KF.predict()
# self.KF.update(np.array([vData.iloc[nobs-1].x,vData.iloc[nobs-1].speed]))
vState = vData.iloc[nobs-1]
if self.first:
self.KF.x[0] = vState.x
self.KF.x[1] = vState.speed
#self.KF.predict()
self.first = False
else:
if vState.speed < 0:
self.KF.update(np.array([vState.x, 0.]))
else:
self.KF.update(np.array([vState.x,vState.speed]))
# now you can predict
# return a dataframe of answers
vcurrentState = vData.iloc[vData.shape[0]-1].copy()
vcurrentState.x = self.KF.x[0]
vcurrentState.speed = self.KF.x[1]
returnedStates = vData[vData['time']<0] # empty
for time in predictTimes:
vNextState = movePhysics(vcurrentState, self.KF.x[2]/self.dt, time)
returnedStates = returnedStates.append(vNextState)
self.KF.predict()
return returnedStates
class InMagPredictor:
stateNum = 10 # x, y, z, q0, q1, q2, q3, wx, wy, wz
measureNum = SLAVES * 3 + 2 # sensor*3 + IMU
points = MerweScaledSigmaPoints(n=stateNum, alpha=0.3, beta=2., kappa=3 - stateNum)
dt = 0.03 # 时间间隔[s]
t0 = datetime.datetime.now() # 初始化时间戳
def __init__(self, sensor_std, state0, state):
"""
初始化UKF滤波器
:param sensor_std:【float】sensor噪声标准差 [mG]
:param state0:【np.array】初始值 (stateNum,)
:param state: 【np.array】真实值 (stateNum,)
"""
# self.mp = MahonyPredictor()
self.ukf = UKF(dim_x=self.stateNum, dim_z=self.measureNum, dt=self.dt, points=self.points, fx=self.f, hx=h)
self.ukf.x = state0.copy() # 初始值
q0i, q1i, q2i, q3i = state0[3: 7]
q0, q1, q2, q3 = state[3: 7]
for i in range(6):
self.ukf.R[i, i] = sensor_std
for i in range(7, self.measureNum):
self.ukf.R[i, i] = 5
self.ukf.P = np.eye(self.stateNum) * 0.01
for i in range(3):
self.ukf.P[i, i] = 1.5 * (state0[i] - state[i]) ** 2 # 位置初始值的误差
# self.ukf.P[3: 7, 3: 7] = 1.5 * np.array([ # 姿态四元数初始值的误差
# [(q0i - q0) ** 2, (q0i - q0) * (q1i - q1), (q0i - q0) * (q2i - q2), (q0i - q0) * (q3i - q3)],
# [(q0i - q0) * (q1i - q1), (q1i - q1) ** 2, 0, 0],
# [(q0i - q0) * (q2i - q2), 0, (q2i - q2) ** 2, 0],
# [(q0i - q0) * (q3i - q3), 0, 0, (q3i - q3) ** 2]
# ])
self.ukf.P += np.eye(self.stateNum) * 0.001
self.ukf.Q = np.eye(self.stateNum) * 0.05 * self.dt # 将速度作为过程噪声来源，Qi = [v*dt]
# for i in range(3, self.stateNum):
# self.ukf.Q[i, i] = 0.0001
Qqii, Qqij = 0.01, 0.001
self.ukf.Q[3: 7, 3: 7] = np.array([ # 精细化定义姿态(四元数)的过程误差
[Qqii, Qqij, Qqij, Qqij],
[Qqij, Qqii, 0, 0],
[Qqij, 0, Qqii, 0],
[Qqij, 0, 0, Qqij]
])
for i in range(7, self.stateNum):
self.ukf.Q[i, i] = 0.001 # 角速度的过程误差
def f(self, x, dt):
wx, wy, wz = self.ukf.x[-3:]
A = np.eye(self.stateNum)
A[3:7, 3:7] = np.eye(4) + 0.5 * dt * np.array([[0, -wx, -wy, -wz],
[wx, 0, wz, -wy],
[wy, -wz, 0, wx],
[wz, wy, -wx, 0]])
return np.hstack(np.dot(A, x.reshape(self.stateNum, 1)))
def h0(self, state):
"""
使用互补滤波预测的结果(q)来估算预估量的四元数
:param state: 胶囊的位姿状态
:return: 预估量的四元数
"""
H = np.zeros((7, 4))
for i in range(3):
H[i + 4, i] = 0
return np.dot(state, H)
def run(self, z, printBool):
pos = np.round(self.ukf.x[:3], 3)
em = q2R(self.ukf.x[3: 7])[:, -1]
timeCost = (datetime.datetime.now() - self.t0).total_seconds()
if printBool:
print(r'pos={}m, em={}, w={}, timeCost={:.3f}s'.format(pos, np.round(em, 3),
np.round(self.ukf.x[-3:], 3), timeCost))
self.t0 = datetime.datetime.now()
# 使用IMU的数据更新滤波器
# for i in range(20):
# self.mp.IMUupdate(z[6: 9], z[-3:])
# emq = q2R(self.mp.q)[-1]
# print(r'IMU update: pos={}m, em={}, w={}'.format(pos, np.round(emq, 3), np.round(z[6: 9], 3)))
# self.ukf.x[3: 7] = self.mp.q
# self.ukf.x[7:] = z[6: 9]
# 使用磁传感器的数据更新滤波器
self.ukf.predict()
self.ukf.update(z)
def generate_data(self, state, sensor_std, printBool):
"""
生成模拟数据
:param state: 【np.array】模拟的胶囊状态 (m,)
:param sensor_std：【float】磁传感器的噪声标准差 [mG]
:param printBool: 【bool】是否打印输出
:return: 【np.array】模拟的B值 + 加速度计的数值, (num_data, )
"""
Bmid = h(state)[:-2] # 模拟B值数据的中间值
Bsim = np.zeros(SLAVES * 3)
for j in range(SLAVES * 3):
Bsim[j] = np.random.normal(Bmid[j], sensor_std, 1)
R = q2R(state[3: 7])
accSim = R[:, -1]
if printBool:
print('Bmid={}'.format(np.round(Bmid, 0)))
print('Bsim={}'.format(np.round(Bsim, 0)))
print('truth: pos={}m, e_moment={}\n'.format(state[:3], np.round(accSim, 3)))
return np.concatenate((Bsim, accSim))
def sim(self, states, sensor_std, plotType, plotBool, printBool, maxIter=20):
"""
使用模拟的观测值验证算法的准确性
:param states: 【list】模拟的真实状态，可以有多个不同的状态
:param sensor_std: 【float】sensor的噪声标准差[mG]
:param plotType: 【tuple】描绘位置的分量 'xy' or 'yz'
:param plotBool: 【bool】是否绘图
:param printBool: 【bool】是否打印输出
:param maxIter: 【int】最大迭代次数
:return: 【tuple】 位置[x, y, z]和姿态ez的误差百分比
"""
# self.ukf.x = state0.copy() # 初始值
state = states[0] # 真实值
# simData = self.generate_data(state, sensor_std, printBool)
simData = generate_data(self.measureNum, state, sensor_std, printBool)
err_pos, err_em = (0, 0)
for i in range(maxIter):
if plotBool:
print('=========={}=========='.format(i))
plt.ion()
plotP(self, state, i, plotType)
if i == maxIter - 1:
plt.ioff()
plt.show()
self.run(simData, printBool)
posTruth, emTruth = state[:3], q2R(state[3: 7])[:, -1]
pos, em = self.ukf.x[:3], q2R(self.ukf.x[3: 7])[:, -1]
err_pos = np.linalg.norm(pos - posTruth) / np.linalg.norm(posTruth)
err_em = np.linalg.norm(em - emTruth) # 方向矢量本身是归一化的
print('err_pos={:.0%}, err_em={:.0%}'.format(err_pos, err_em))
return (err_pos, err_em)
g = -9.81
x[0] = x[0] + x[1] * dt
x[1] = x[1] + g * dt
return x
def h_cv(x):
"""Measurement function -
measuring only position"""
return np.array([x[0]])
starting_conditions = [1e5, 0., 2000]
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=0)
ukf = UKF(dim_x=2, dim_z=1, fx=f_cv, hx=h_cv, dt=dt, points=points)
ukf.x = np.array([1.001e5, 0.])
ukf.R = np.array([[1e3]])
#ukf.H = np.array([[1]])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.5)
uxs = []
trux = []
truv = []
measx = []
zs = np.arange(0, 465 + dt, dt)
time = []
t = 0
f = falling_object(starting_conditions[0], starting_conditions[1],
starting_conditions[2], std_x)
def __init__(self, g, zeta,
leader, trajectory_delay=2.0,
update_delta_t=0.01,
orig_leader=None, orig_leader_delay=None,
noise_sigma=0.0, log_file=None,
visibility_fov=config.FOV_ANGLE, visibility_radius=None,
id=None,
dt=0.02):
"""
Construct a follower Behavior.
leader is the Bot to be followed
g > 0 is a tuning parameter
zeta in (0, 1) is a damping coefficient
trajectory_delay is the time interval between leader and follower
"""
super(Follower, self).__init__()
assert(isinstance(leader, Bot))
assert(isinstance(orig_leader, Bot))
self.radius = config.BOT_RADIUS
self.leader = leader
self.orig_leader = orig_leader
self.trajectory_delay = trajectory_delay
assert g > 0, "Follower: g parameter must be positive"
self.g = g
assert 0 < zeta < 1, "Follower: zeta parameter must be in (0, 1)"
self.zeta = zeta
# leader_positions stores config.POSITIONS_BUFFER_SIZE tuples;
# tuple's first field is time, second is the
# corresponding position of the leader
self.leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)
self.noisy_leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)
self.leader_is_visible = False
# precise_orig_leader_states stores the first leader's precise state;
# used to calculate "real" errors (as opposed to calculations
# w.r.t. the approximation curve)
self.precise_orig_leader_states = []
# precise_leader_states is used for the same purpose, but with the bot's own leader
self.precise_leader_states = []
self.update_delta_t = update_delta_t
needed_buffer_size = config.SAMPLE_COUNT // 2 + self.trajectory_delay / self.update_delta_t
if needed_buffer_size > config.POSITIONS_BUFFER_SIZE:
sys.stderr.write("leader_positions buffer is too small for update_delta_t = {}".format(self.update_delta_t) +
" (current = {}, required = {})".format(config.POSITIONS_BUFFER_SIZE, needed_buffer_size))
raise RuntimeError, "leader_positions buffer is too small"
self.last_update_time = 0.0
self.noise_sigma = noise_sigma
self.log_file = log_file
self.visibility_fov = visibility_fov
if visibility_radius is None:
visibility_radius = 2.0 * trajectory_delay * config.BOT_VEL_CAP
self.visibility_radius = visibility_radius
self.id = id;
if orig_leader_delay is None:
orig_leader_delay = trajectory_delay
self.orig_leader_delay = orig_leader_delay
if self.log_file is not None:
log_dict = {"id": self.id,
"g": self.g,
"zeta": self.zeta,
"noise_sigma": self.noise_sigma,
"reference_points_cnt": config.SAMPLE_COUNT,
"trajectory_delay": trajectory_delay}
print >> self.log_file, log_dict
q = 0.01 # std of process
r = 0.05 # std of measurement
self.delta_time = dt
if not config.DISABLE_UKF:
points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=-3)
self.ukf = UKF(dim_x=6, dim_z=2, fx=f, hx=h, dt=dt, points=points)
self.ukf_x_initialized = 0
#self.ukf.x = np.array([0, 0, 1, pi/4, 0, 0])
self.ukf.R = np.diag([r, r]);
self.ukf.Q = np.diag([0, 0, 0, 0, q, q])
pf = ParticleFilter(STATE_DIM, N, pdf_x0, noisyUpdate, measurement_pdf,
q0_sampler)
particles_thetas = []
particles_omegas = []
particles_times = []
particles_s = []
points = MerweScaledSigmaPoints(STATE_DIM, alpha=.1, beta=2., kappa=-1)
def measurement(xhat):
return [xhat[THETA]]
ukf = UnscentedKalmanFilter(STATE_DIM, 1, dt, measurement, noiselessUpdate,
points)
ukf.x = xhat0
ukf.P = P0
ukf.R = R
startTimer("main")
NUM = 1
def drawEllipse(cov, ax, avg, K=3):
angles = np.linspace(0, math.pi * 2, 100)
w, v = np.linalg.eig(np.linalg.inv(cov))
lambda1 = w[0]
lambda2 = w[1]
