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 = 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]])
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_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 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 creat_batch3(min_dn,max_dn,pos_noise,vel_noise,BN):
batch_size = int(100*BN)
my_traj, my_obser, Tran_m = trajectory_batch_generator(batch_size,data_len)
Traj_r = my_traj
Obser = my_obser
Traj_s = np.array([[[0 for i in range(4)] for j in range(data_len)] for k in range(batch_size)],'float64')
for i in range(batch_size):
my_SP = SP(dim_state,kappa=0.)
my_UKF = UKF(dim_x=dim_state, dim_z=dim_obser, dt=sT, hx=my_hx, fx=my_fx, points=my_SP)
my_UKF.Q *= var_m
my_UKF.R *= np.array([[azi_var,0],[0,dis_var]])
x0_noise = np.array([np.random.normal(0,pos_noise,2),np.random.normal(0,vel_noise,2)])
my_UKF.x = Traj_r[i,0,:] + np.reshape(x0_noise,[4,])
my_UKF.P *= 1.
#tracking results of UKF
xs = []
xs.append(my_UKF.x)
for j in range(data_len-1):
my_UKF.predict()
my_UKF.update(Obser[i,j+1,:])
xs.append(my_UKF.x.copy())
Traj_s[i] = np.asarray(xs)
return Traj_r, Obser, Traj_s, Traj_r-Traj_s
def main():
[t, dt, s, v, a, theta, omega, alpha] = build_real_values()
zs = build_measurement_values(t, [a, omega])
u = build_control_values(t, v)
[F, B, H, Q, R] = init_kalman(t, dt)
sigmas = MerweScaledSigmaPoints(n=9, alpha=.1, beta=2., kappa=-1)
kf = UKF(dim_x=9, dim_z=3, fx=f_bot, hx=h_bot, dt=0.2, points=sigmas)
kf.x = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0.])
kf.R = R
kf.F = F
kf.H = H
kf.Q = Q
xs, cov = [], []
for zk, uk in zip(zs, u):
kf.predict(fx_args=uk)
kf.update(z=zk)
xs.append(kf.x.copy())
cov.append(kf.P)
xs, cov = np.array(xs), np.array(cov)
xground = construct_xground(s, v, a, theta, omega, alpha, xs.shape)
nees = NEES(xground, xs, cov)
print(np.mean(nees))
plot_results(t, xs, xground, zs, nees)
class Filter:
def __init__(self, fx, hx):
self.sigmas = MerweScaledSigmaPoints(n=3, alpha=.3, beta=2., kappa=.1)
self.ukf = UnscentedKalmanFilter(dim_x=3,
dim_z=2,
dt=.02,
hx=hx,
fx=fx,
points=self.sigmas)
def initialize(self, p0, a0, p_std, a_std):
self.p_std = p_std
self.a_std = a_std
height = 44330 * (1 - pow((p0 / 101325), 0.1902))
self.ukf.x = np.array([height, 0, a0])
self.ukf.P *= np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.ukf.R = np.array([[p_std**2, 0], [0, a_std**2]])
self.ukf.Q = Q_discrete_white_noise(3, dt=.02, var=0.03)
def run(self, raw_p, raw_a):
heights, varios, accs = [], [], []
for i in range(len(raw_p) - 1):
meas = [raw_p[i + 1], raw_a[i + 1]]
self.ukf.predict()
self.ukf.update(meas)
heights.append(self.ukf.x[0])
varios.append(self.ukf.x[1])
accs.append(self.ukf.x[2])
return heights, varios, accs
def unscented_kf(self, number=NUMBER):
global Time
P0 = np.diag([
3e-2, 3e-2, 3e-2, 3e-6, 3e-6, 3e-6, 3e-2, 3e-2, 3e-2, 3e-6, 3e-6,
3e-6
])
error = np.random.multivariate_normal(mean=np.zeros(12), cov=P0)
X0 = np.hstack((HPOP_1[0], HPOP_2[0])) + error
points = MerweScaledSigmaPoints(n=12, alpha=0.001, beta=2.0, kappa=-9)
ukf = UKF(dim_x=12,
dim_z=4,
fx=self.state_equation,
hx=self.measure_equation,
dt=STEP,
points=points)
ukf.x = X0
ukf.P = P0
ukf.R = Rk
ukf.Q = Qk
XF, XP = [X0], [X0]
print(error, "\n", Qk[0][0], "\n", Rk[0][0])
for i in range(1, number + 1):
ukf.predict()
Z = nav.measure_stk(i)
ukf.update(Z)
X_Up = ukf.x.copy()
XF.append(X_Up)
Time = Time + STEP
XF = np.array(XF)
return XF
def _test_log_likelihood():
from filterpy.common import Saver
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)
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=1.1**2, block_size=2)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.
zs = [[i + randn() * z_std, i + randn() * z_std] for i in range(40)]
s = Saver(kf)
for z in zs:
kf.predict()
kf.update(z)
print(kf.x, kf.log_likelihood, kf.P.diagonal())
s.save()
s.to_array()
plt.plot(s.x[:, 0], s.x[:, 2])
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 = 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)
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 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 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
class MagPredictor():
stateNum = 7 # x, y, z, q0, q1, q2, q3
def __init__(self):
self.points = MerweScaledSigmaPoints(n=self.stateNum,
alpha=0.3,
beta=2.,
kappa=3 - self.stateNum)
self.dt = 0.03 # 时间间隔[s]
self.ukf = UKF(dim_x=self.stateNum,
dim_z=SLAVES * 3,
dt=self.dt,
points=self.points,
fx=self.f,
hx=h)
self.ukf.x = np.array([0.0, 0.0, 0.04, 1, 0, 0, 0]) # 初始值
self.ukf.R *= 3 # 先初始化为3，后面自适应赋值
self.ukf.P = np.eye(self.stateNum) * 0.016
self.ukf.Q = np.eye(
self.stateNum) * 0.01 * self.dt # 将速度作为过程噪声来源，Qi = [v*dt]
for i in range(3, 7):
self.ukf.Q[i, i] = 0.0001
def f(self, x, dt):
A = np.eye(self.stateNum)
return np.hstack(np.dot(A, x.reshape(self.stateNum, 1)))
def run(self, magData, Bg, state):
pos = (round(self.ukf.x[0], 3), round(self.ukf.x[1],
3), round(self.ukf.x[2], 3))
m = q2m(self.ukf.x[3], self.ukf.x[4], self.ukf.x[5], self.ukf.x[6])
# print(r'pos={}m, e_moment={}'.format(pos, m))
z = np.hstack(magData[:])
# 自适应 R
for i in range(SLAVES * 3):
# 1.sensor的方差随B的关系式为：Bvar = 2*E(-16)*B^4 - 2*E(-27)*B^3 + 2*E(-8)*B^2 + 1*E(-18)*B + 10
# Bm = magData[i] + Bg[i]
# self.ukf.R[i, i] = (2 * 10**(-16) * Bm ** 4 - 2 * 10**(-27) * Bm ** 3 + 2 * 10**(-8) * Bm * Bm + 10**(-18) * Bm + 10) * 0.005
# 2.sensor的方差随B的关系式为：Bvar = 1*E(-8)*B^2 - 2*E(-6)*B + 0.84
Bm = magData[i] + Bg[i]
self.ukf.R[i, i] = (math.exp(-8) * Bm**2 - 2 * math.exp(-6) * Bm +
0.84) * 1
# 3.sensor的方差随B的关系式为：Bvar = 1*E(-8)*B^2 + 6*E(-6)*B + 3.221
# Bm = magData[i] + Bg[i]
# self.ukf.R[i, i] = 10**(-8) * Bm ** 2 + 6 * 10**(-6) * Bm + 3.221
t0 = datetime.datetime.now()
self.ukf.predict()
self.ukf.update(z)
timeCost = (datetime.datetime.now() - t0).total_seconds()
state[:] = np.concatenate((mp.ukf.x, np.array([MOMENT, timeCost,
1]))) # 输出的结果
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 = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
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)
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
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 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.)
# sp = SimplexSigmaPoints(n=3)
kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
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 test_kalman_motion(testname, vel_s, vel_p, turn_rad, dangle_tot, variance_diag,
start_angle=0, manual_p=None, manual_x=None, t_total=5.0):
dt = 0.05 # 50 ms, robot control period
kf = TankKalman2(dt, 2.0, 0.0, 0.0)
kf.Q = np.zeros((6, 6))
omega = dangle_tot / t_total
kf.x = np.array([0, 0, vel_s, vel_p, start_angle, omega])
kf.P = np.diag(variance_diag)
for i in range(int(t_total / dt)):
UnscentedKalmanFilter.predict(kf)
if manual_x is not None:
x_expect = manual_x
elif dangle_tot < 0.0001:
x = vel_s * t_total
y = vel_p * t_total
if abs(start_angle) > 0.0001:
x2 = x * math.cos(start_angle) - y * math.sin(start_angle)
y2 = x * math.sin(start_angle) + y * math.cos(start_angle)
x = x2
y = y2
x_expect = np.array([x, y, vel_s, vel_p, start_angle, omega])
else:
raise Exception('not implemented')
# print('kf.x =', kf.x)
# print('kf.P =', kf.P)
cmp_arrays(kf.x, x_expect, 0.015, testname + ' x')
if manual_p is not None:
cmp_arrays(kf.P, manual_p, 1e-3, testname + ' manual_P')
else:
encoders = TankEncoderTracker(kf.wheel_base, 0, 0)
p_enc0 = np.diag([variance_diag[0], variance_diag[1], variance_diag[TankKalman2.STATE_THETA]])
if dangle_tot < 0.0001:
p_sp = encoders.propagate_line(vel_s * t_total, p_enc0)
else:
dl = TankEncoderTracker.dl_func(turn_rad, kf.x[TankKalman2.STATE_THETA], kf.wheel_base)
dr = TankEncoderTracker.dr_func(turn_rad, kf.x[TankKalman2.STATE_THETA], kf.wheel_base)
p_sp = encoders.propagate_arc(dl, dr, p_enc0)
t_rot = TankEncoderTracker.t_matrix(kf.x[TankKalman2.STATE_THETA])
p_xy = t_rot @ p_sp @ t_rot.T
t_expand = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 0], [0, 0, 0], [0, 0, 1], [0, 0, 0]])
p_expect = t_expand @ p_xy @ t_expand.T
cmp_arrays(kf.P, p_expect, 1e-3, testname + ' encoder_P')
return kf
def test_vhartman():
"""
Code provided by vhartman on github #172
https://github.com/rlabbe/filterpy/issues/172
"""
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
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]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
kf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([0.]) # initial state
kf.P = np.array([[1]]) # initial uncertainty
kf.R = np.diag([1]) # 1 standard
kf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
kf.predict()
ekf.predict()
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
kf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
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 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_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
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 *= 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 predict(self, dt, motors=None, nsteps=1):
# save the previous state for use during update()
self.prev_x = np.copy(self.x)
dt_step = dt / nsteps
for _ in range(nsteps):
# set Q and predict
self.Q = self.Q_func(dt_step, self.x[self.STATE_THETA])
accelerations = None
if motors and self.control_model:
accelerations = self.control_model.accelerations(motors, self.x[self.STATE_VS], self.x[self.STATE_OMEGA])
UnscentedKalmanFilter.predict(self, dt=dt_step, accelerations=accelerations)
return
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 = 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)
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)
class filterr:
def __init__(self, current_temp, dt):
"""Unscented kalman filter"""
self._interval = 0
self._last_update = time.time()
# sigmas = JulierSigmaPoints(n=2, kappa=0)
sigmas = MerweScaledSigmaPoints(n=2, alpha=0.001, beta=2, kappa=0)
self._kf_temp = UnscentedKalmanFilter(
dim_x=2, dim_z=1, dt=dt, hx=hx, fx=fx, points=sigmas
)
self._kf_temp.x = np.array([float(current_temp), 0.0])
self._kf_temp.P *= 0.2 # initial uncertainty
self.interval = dt
def kf_predict(self):
dt = time.time() - self._last_update
self._last_update = time.time()
self._kf_temp.predict(dt=dt)
def kf_update(self, current_temp):
self._kf_temp.update(float(current_temp))
@property
def get_temp(self):
return float(self._kf_temp.x[0])
@property
def get_vel(self):
return float(self._kf_temp.x[1])
def set_Q_R(self, dt=None):
""" process noise """
self._kf_temp.Q = Q_discrete_white_noise(
dim=2, dt=self._interval, var=(0.005 / (self._interval ** 1.2)) ** 2
)
"""measurement noise std**2 """
self._kf_temp.R = np.diag([(4 * (1800 / self._interval) ** 0.8) ** 2])
@property
def interval(self):
return self._interval
@interval.setter
def interval(self, dt):
if dt != self.interval:
self._interval = dt
self.set_Q_R()
class KF:
def __init__(self, initial_x, initial_y, initial_vx, initial_vy):
self.dt = 0.05
# create sigma points to use in the filter. This is standard for Gaussian processes
self.points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1)
self.kf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=self.dt,
fx=fx,
hx=hx,
points=self.points)
self.kf.x = np.array([initial_x, 0, initial_y, 0]) # initial state
self.kf.P *= 0.1 # initial uncertainty
self.z_std = 0.1
self.kf.R = np.diag([self.z_std**2, self.z_std**2]) # 1 standard
self.kf.Q = Q_discrete_white_noise(dim=2,
dt=self.dt,
var=0.01**2,
block_size=2)
def predict(self):
return self.kf.predict()
def update(self, meas_value):
self.kf.update([meas_value[0], meas_value[1]])
@property
def calulatedmean(self):
return self.kf.x
def run_ukf(zs, dt=1.0):
sigmas = MerweScaledSigmaPoints(4, alpha=0.1, beta=2.0, kappa=1.0)
ukf = UKF(dim_x=4, dim_z=2, fx=f, hx=h, dt=dt, points=sigmas)
ukf.x = np.array([0.0, 0.0, 0.0, 0.0])
ukf.R = np.diag([0.09, 0.09])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)
uxs = []
for z in zs:
ukf.predict()
ukf.update(z)
uxs.append(ukf.x.copy())
uxs = np.array(uxs)
return uxs
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 predict(self, dt):
# save the previous state for use during update()
self.prev_x = np.copy(self.x)
# set Q and predict
self.Q = self.Q_func(dt, self.x[self.STATE_THETA])
return UnscentedKalmanFilter.predict(self)
def test_ukf_ekf_comparison():
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
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]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
ukf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
ukf.x = np.array([0.]) # initial state
ukf.P = np.array([[1]]) # initial uncertainty
ukf.R = np.diag([1]) # 1 standard
ukf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
ukf.predict()
ekf.predict()
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after prediction'
ukf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after update'
def iniciar_ukf(list_z):
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([1., 1., 1., 1]) # initial state
kf.P *= 0.2 # 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)
zs = list_z
x_predichas = []
y_predichas = []
x_estimadas = []
y_estimadas = []
for z in zs:
# Predicción
kf.predict()
xp = kf.x[0]
yp = kf.x[1]
x_predichas.append(xp)
y_predichas.append(yp)
print("PREDICCION: x:", xp, "y:", yp)
# Actualización
kf.update(z)
xe = kf.x[0]
ye = kf.x[1]
x_estimadas.append(xe)
y_estimadas.append(ye)
print("ESTIMADO: x:", xe, "y:", ye)
print("--------------------------------------")
plt.plot(x_predichas, y_predichas, linestyle="-", color='orange')
plt.plot(x_estimadas, y_estimadas, linestyle="-", color='b')
plt.show()
def filter(measurements):
dt = 0.1
# x = [x, x', x'' y, y', y'']
x = np.array([measurements[0][0], 0., 0., measurements[0][1], 0., 0.])
G = np.array([[0.19*(dt**2)],
[dt],
[1.],
[0.19*(dt**2)],
[dt],
[1.]])
Q = G*G.T*0.1**2
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=6, alpha=1., beta=2., kappa=-3.)
bot_filter = UKF(dim_x=6, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., 0, measurements[0][1], 0., 0.])
#bot_filter.F = F
bot_filter.H = np.array([[1., 0., 0., 1., 0., 0.]])
#bot_filter.Q = Q
bot_filter.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.P *= 500
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements)-60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0,60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
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 linear_filter(measurements):
dt = 1.0
# x = [x, x', y, y']
x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
H = np.array([[1., 0., 1., 0.]])
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
bot_filter = UKF(dim_x=4,
dim_z=2,
fx=f_linear,
hx=h_linear,
dt=dt,
points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
#bot_filter.F = F
bot_filter.H = np.asarray(H)
#bot_filter.Q = Q
bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.P *= 10
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements) - 60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0, 60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
def filter(measurements):
dt = 0.1
# x = [x, x', x'' y, y', y'']
x = np.array([measurements[0][0], 0., 0., measurements[0][1], 0., 0.])
G = np.array([[0.19 * (dt**2)], [dt], [1.], [0.19 * (dt**2)], [dt], [1.]])
Q = G * G.T * 0.1**2
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=6, alpha=1., beta=2., kappa=-3.)
bot_filter = UKF(dim_x=6, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
bot_filter.x = np.array(
[measurements[0][0], 0., 0, measurements[0][1], 0., 0.])
#bot_filter.F = F
bot_filter.H = np.array([[1., 0., 0., 1., 0., 0.]])
#bot_filter.Q = Q
bot_filter.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=1, var=0.0002)
bot_filter.P *= 500
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements) - 60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0, 60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
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]])
#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
s.save()
s.to_array()
def plot_filtered(log_data):
def f(x, dt):
""" state transition function """
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0,
1]])
return np.dot(F, x)
def h(x):
""" measurement function (state to measurement transform) """
return np.array(x[0], x[2])
dt = 1.
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1)
ukf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
fx=f,
hx=h,
dt=dt,
points=points)
ukf.x = np.array([0., 0., 0., 0.])
ukf.R = np.diag([0.09, 0.09])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)
uxs = []
zs = np.stack((log_data['longitude'], log_data['latitude']), axis=-1)
for z in zs:
ukf.predict()
ukf.update(z)
uxs.append(ukf.x.copy())
uxs = np.array(uxs)
plt.subplot(211)
plt.plot(log_data['longitude'], log_data['latitude'])
plt.subplot(212)
plt.plot(uxs[:, 0], uxs[:, 2])
plt.show()
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]], dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0]])
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]])
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 UKF_tracking(x0, Obser):
#定义UKF
dim_state = len(x0)
[data_len, dim_obser] = Obser.shape
my_SP = SP(dim_state, kappa=0.)
my_UKF = UKF(dim_x=dim_state,
dim_z=dim_obser,
dt=sT,
hx=my_hx,
fx=my_fx,
points=my_SP)
my_UKF.Q *= var_m
my_UKF.R *= np.array([[azi_var, 0], [0, dis_var]])
my_UKF.x = x0
my_UKF.P *= 1.
#跟踪的航迹结果
xs = []
xs.append(my_UKF.x.copy())
for i in range(data_len - 1):
my_UKF.predict()
my_UKF.update(Obser[i + 1, :])
xs.append(my_UKF.x.copy())
return np.asarray(xs)
def linear_filter(measurements):
dt = 1.0
# x = [x, x', y, y']
x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
H = np.array([[1., 0., 1., 0.]])
# Info available http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/05_Multivariate_Kalman_Filters.ipynb
sigmas = MerweScaledSigmaPoints(n=4, alpha=0.3, beta=2., kappa=-3.)
bot_filter = UKF(dim_x=4, dim_z=2, fx=f_linear, hx=h_linear, dt=dt, points=sigmas)
bot_filter.x = np.array([measurements[0][0], 0., measurements[0][1], 0.])
#bot_filter.F = F
bot_filter.H = np.asarray(H)
#bot_filter.Q = Q
bot_filter.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.1)
bot_filter.P *= 10
bot_filter.R = np.diag([0.0001, 0.0001])
observable_meas = measurements[0:len(measurements)-60]
pos, cov = [], []
for z in observable_meas:
pos.append(bot_filter.x)
cov.append(bot_filter.P)
bot_filter.predict()
bot_filter.update(z)
for i in range(0,60):
bot_filter.predict()
pos.append(bot_filter.x)
return pos
class UKF_estimate(object):
""" UKF_estimate """
def __init__(self, env, dim_x, dim_y, X_0, P, Q, R, mn):
# self.sigmas = JulierSigmaPoints(dim_x, alpha=.1, beta=2., kappa=1.)
self.sigmas = JulierSigmaPoints(dim_x, kappa=0.1)
self.env = env
self.measure_nums = mn
self.ukf = UnscentedKalmanFilter(dim_x=dim_x,
dim_z=dim_y,
dt=env.tau,
hx=self.measurement,
fx=UKF_model,
points=self.sigmas)
self.ukf.x = X_0[:, 0]
# print(self.ukf.x.shape)
self.ukf.P = np.copy(P)
self.ukf.R = np.copy(R)
self.ukf.Q = np.copy(Q)
# estimate states from input of measurement
def state_estimate(self, force, y):
# update estimation from kalman filter
self.ukf.predict(env=self.env, u=force)
self.ukf.update(y)
# print(self.ukf.x)
# return updated estimated state
return self.ukf.x
def measurement(self, x):
if self.measure_nums is 2:
y = np.array([x[0], x[3]])
elif self.measure_nums is 1:
y = np.array([x[0]])
return y
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
txs = []
cxs = []
radius = 100
delta = 2*np.pi/360*5
for i in range(70):
# 真实位置
target_pos_x = math.cos(i*delta)*radius + random.randn()*0.0001
target_pos_y = math.sin(i*delta)*radius + random.randn()*0.0001
txs.append((target_pos_x, target_pos_y))
# 测量位置
zx = target_pos_x + random.randn()*5
zy = target_pos_y + random.randn()*5
zs.append([zx,zy])
ukf.predict()
ukf.update([zx,zy])
ckf.predict()
ckf.update([zx,zy])
cxs.append(ckf.x.copy())
print "第%d轮结果:" % (i),ckf.x
#pukf
pSigma = e(ukf.sigmas_f)
dHat = np.sum(pSigma)/len(pSigma)
pddPukf = 0
pxdPukf = np.zeros((1,4))
for i in range(len(pSigma)):
pddPukf += ukf.W[i]*(
(pSigma[i] - dHat)*(pSigma[i] - dHat))
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
kf.predict()
kf.update(np.array([radians(45), d]))
print(kf.x)
print(kf.P)
stats.plot_covariance_ellipse(
kf.x, cov=kf.P, axis_equal=True,
facecolor='g', edgecolor=None, alpha=0.6)
plt.scatter([100], [100], c='g', label='45 degrees')
p = (13, -11)
hx.p = kf.x - np.array([-50,50])
d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5
dt = 0.05
points_obj=MerweScaledSigmaPoints(n=3)
radarUKF = UKF(dim_x=3, dim_z=1, dt=dt,hx= hx,fx=fx, points=points_obj)
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.GetRadar(dt)
rs.append(r)
radarUKF.predict()
radarUKF.update(r)
xs.append(radarUKF.x)
xs = np.asarray(xs)
plt.plot(rs)
plt.show()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.title('distance')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.title('velocity')
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
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')
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))
f.predict()
f.update(array([z]))
xs.append(f.x)
xs = asarray(xs)
track = asarray(track)
time = np.arange(0,len(xs)*dt, dt)
plt.figure()
plt.subplot(311)
plt.plot(time, track[:,0])
plt.plot(time, xs[:,0])
plt.legend(loc=4)
plt.xlabel('time (sec)')
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')
T = np.array([16.878, -7.1368, 0]) #Translation vector joining two inertial frames time = np.arange(START_TIME,END_TIME,dt) print time.shape,time[0],time[-1] d = np.linspace(20,40,time.shape[0]) zs = np.zeros((time.shape[0],15)) Rs = np.zeros((time.shape[0],15,15)) means = np.zeros((time.shape[0],12)) covs = np.zeros((time.shape[0],12,12)) cam_locs = np.zeros((time.shape[0],3)) ekf_locs = np.zeros((time.shape[0],3)) for i in range(1,time.shape[0]): t = time[i] ukf.predict(1.0/30) # Get camera location and covariance points in ball inertial frame cam_loc = track.get_mean(t,d[i]) + T cam_sigs = track.get_cov(t,d[i],10) + T[:,None] cam_points = np.vstack((cam_sigs.T, cam_loc)).T cam_cov = np.cov(cam_points) # Get kalman filter state estimate ekf_loc = left_reader.get_ekf_loc_1d(t) ekf_vel = left_reader.get_ekf_vel(t) ekf_att = np.deg2rad(left_reader.get_ekf_att(t)) ekf_attdot = np.deg2rad(left_reader.get_gyr(t)) ekf_locs[i,:] = ekf_loc cam_locs[i,:] = cam_loc
seed(12)
cmds = np.array(cmds)
cindex = 0
u = cmds[0]
track = []
std = 16
while cindex < len(cmds):
u = cmds[cindex]
xp = move(xp, u, dt, wheelbase) # simulate robot
track.append(xp)
ukf.predict(fx_args=u)
if cindex % 20 == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=std,
facecolor='b', alpha=0.58)
#print(cindex, ukf.P.diagonal())
#print(ukf.P.diagonal())
z = []
for lmark in m:
d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r
bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0])
a = normalize_angle(bearing - xp[2] + randn()*sigma_h)
z.extend([d, a])
#if cindex % 20 == 0:
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')
filter.R = np.diag([100,100,0.25])
robot = vehicle.Vehicle(1,50) #create a robot
robot.setOdometry(True) #configure its odometer
robot.setOdometryVariance([0.2,1])
speed,angle = [],[]
for a in xrange(4): #create a retangular path
for i in xrange(400):
angle.append(0)
for i in xrange(9):
angle.append(10)
for i in xrange(len(angle)): #set the speed to a constant along the path
speed.append(1)
robot.sim_Path(speed,angle) #run in a rectangular path
speed , angle = robot.readOdometry() #reads the sensors
cam = upper_cam.UpperCam([10,10,0.5],robot)
cam.readPath()
camPoses = cam.readPoses()
Uposes = [[],[]]
for i in range(len(speed)):
x = filter.x
#print 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]*0.01745])
#robot.show("Real")
plt.plot(Uposes[0],Uposes[1],'y',label = "UKF")
plt.legend()
plt.show()
def hx(x):
""" measurement function - convert position to bearing"""
return math.atan2(platform_pos[1], x[0] - platform_pos[0])
xs_scaled = []
xs = []
for i in range(300):
angle = hx([i + randn() * 0.1, 0]) + randn()
sf.update(angle, hx, fx)
xs_scaled.append(sf.x)
f.predict(fx)
f.update(angle, hx)
xs.append(f.x)
xs_scaled = asarray(xs_scaled)
xs = asarray(xs)
plt.subplot(211)
plt.plot(xs_scaled[:, 0], label="scaled")
plt.plot(xs[:, 0], label="Julier")
plt.legend(loc=4)
plt.subplot(212)
plt.plot(xs_scaled[:, 1], label="scaled")
plt.plot(xs[:, 1], label="Julier")
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
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)
xs.append(radarUKF.x)
xs = np.asarray(xs)
plt.subplot(411)
plt.plot(t, xs[:, 0])
plt.title('distance')
plt.subplot(412)
plt.plot(t, xs[:, 1])
plt.title('velocity')
plt.subplot(413)
