def build_ukf(x0=None, P0=None,
Q = None, R = None
):
# build ukf
if x0 is None:
x0 = np.zeros(6)
if P0 is None:
P0 = np.diag([1e-6,1e-6,1e-6, 1e-1, 1e-1, 1e-1])
if Q is None:
Q = np.diag([1e-4, 1e-4, 1e-2, 1e-1, 1e-1, 1e-1]) #xyhvw
if R is None:
R = np.diag([1e-1, 1e-1, 1e-1]) # xyh
#spts = MerweScaledSigmaPoints(6, 1e-3, 2, 3-6, subtract=ukf_residual)
spts = JulierSigmaPoints(6, 6-2, sqrt_method=np.linalg.cholesky, subtract=ukf_residual)
ukf = UKF(6, 3, (1.0 / 30.), # dt guess
ukf_hx, ukf_fx, spts,
x_mean_fn=ukf_mean,
z_mean_fn=ukf_mean,
residual_x=ukf_residual,
residual_z=ukf_residual)
ukf.x = x0.copy()
ukf.P = P0.copy()
ukf.Q = Q
ukf.R = R
return ukf
def create_ukf(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
points = MerweScaledSigmaPoints(n=3,
alpha=0.03,
beta=2.,
kappa=0,
subtract=residual_x,
sqrt_method=sqrt_func)
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([203.0, 1549.2, 1.34])
ukf.P = np.diag([100., 100., .5])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks))
ukf.Q = np.diag([10.**2, 10.**2, 0.3**2])
return ukf
def build_ukf(x, P, std_r, std_b, dt=1.0):
'''
Build UKF.
x: initial state.
P: initial covariance matrix.
std_r: standard var. of laser measurement.
std_b: standard var. of IMU measurement.
dt: time interval.
Plus some defined functions as parameters.
returns ukf.
'''
# Calculate sigma points.
points = MerweScaledSigmaPoints(n=6, alpha=0.001, beta=2, kappa=-3, subtract=residual_x)
ukf = UKF(dim_x=6, dim_z=4, 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_z)
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([std_r ** 2, std_r ** 2, std_b ** 2, std_b ** 2])
q1 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0)
q2 = Q_discrete_white_noise(dim=2, dt=dt, var=1.0)
q3 = Q_discrete_white_noise(dim=2, dt=dt, var=3.05 * pow(10, -4))
ukf.Q = block_diag(q1, q2, q3)
# ukf.Q = np.eye(3) * 0.0001
return ukf
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 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_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 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 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)
def ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0):
""" construct Unscented Kalman Filter with the initial state x
and the initial covaiance matrix P
sigma_range: the std of laser range sensors
sigma_bearing: the std of IMU
"""
# construct the sigma points
points = MerweScaledSigmaPoints(n=3,
alpha=0.001,
beta=2,
kappa=0,
subtract=residual)
# build the UKF based on previous functions
ukf = UKF(dim_x=3,
dim_z=3,
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual,
residual_z=residual)
# assign the parameters of ukf
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([sigma_range**2, sigma_range**2, sigma_bearing**2])
ukf.Q = np.eye(3) * 0.0001
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
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 myUKF(fx, hx, P, Q, R):
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1.)
kf = UKF(4, 2, dt, fx=fx, hx=hx,
points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints)
kf.P = P
kf.Q = Q
kf.R = R
kf.x = np.array([0., 90., 1100., 0.]) # initial gauss
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 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 get_nonlinear_tracker1():
dt = IMSHOW_SLEEP_TIME / 1000 # time step
sigmas = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=1.)
ukf = UKF(dim_x=6, dim_z=2, fx=f_cv1, hx=h_cv1, dt=dt, points=sigmas)
ukf.x = np.array([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)
return ukf
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 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_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 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
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, 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 create(self, pose, *args, **kwargs):
print 'CREATE!'
ukf = UKF(**self.ukf_args)
ukf._Q = self.Q.copy()
ukf.Q = self.Q.copy()
ukf.R = self.R.copy()
ukf.x = pose #np.zeros(5, dtype=np.float32)
#ukf.x[:3] = pose[:3]
ukf.P = self.P.copy()
# TODO : fill in more info, such as color(red/green/unknown), type(target/obs/unknown)
self.est[self.p_idx] = UKFEstimate(pose, *args, ukf=ukf, **kwargs)
self.p_idx += 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
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 UKFinit():
global ukf
ukf_fuse = []
std_y, std_z = 0.01, 0.01
vstd_y = 0.1
vstd_z = 0.1
dt = 0.005
sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0)
ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([0., 0., 0., 0.])
ukf.R = np.diag([std_y, vstd_y, std_z, vstd_z])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.2)
ukf.P = np.diag([3**2, 0.5, 3**2, 0.5])
def UKFinit():
global ukf
ukf_fuse = []
p_std_x, p_std_y = 0.2, 0.2
v_std_x, v_std_y = 0.01, 0.01
a_std_x, a_std_y = 0.01, 0.01
dt = 0.0125 #80HZ
sigmas = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=-1.0)
ukf = UKF(dim_x=6, dim_z=6, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([0., 0., 0., 0., 0., 0.])
ukf.R = np.diag([p_std_x, v_std_x, a_std_x, p_std_y, v_std_y, a_std_y])
ukf.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=dt, var=0.2)
ukf.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=dt, var=0.2)
ukf.P = np.diag([8, 2, 2, 5, 2, 2])
def UKFinit():
global ukf
p_std_x, p_std_y = 0.1, 0.1
v_std_x, v_std_y = 0.1, 0.1
dt = 0.0125 #80HZ
sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0)
ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([0., 0., 0., 0.])
ukf.R = np.diag([p_std_x, v_std_x, p_std_y, v_std_y])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.2)
ukf.P = np.diag([8, 1.5 ,5, 1.5])
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)
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 UKFinit():
global ukf
ukf_fuse = []
p_std_yaw = 0.004
v_std_yaw = 0.008
dt = 0.0125 #80HZ
sigmas = MerweScaledSigmaPoints(2, alpha=.1, beta=2., kappa=-1.0)
ukf = UKF(dim_x=2, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([
0.,
0.,
])
ukf.R = np.diag([p_std_yaw, v_std_yaw])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.2)
ukf.P = np.diag([6.3, 1])
def Unscentedfilter(zs): # Filter function
points = MerweScaledSigmaPoints(2, alpha=.1, beta=2., kappa=1)
ukf = UnscentedKalmanFilter(dim_x=2,
dim_z=1,
fx=fx,
hx=hx,
points=points,
dt=dt)
ukf.Q = array(([50, 0], [0, 50]))
ukf.R = 100
ukf.P = eye(2) * 2
mu, cov = ukf.batch_filter(zs)
x, _, _ = ukf.rts_smoother(mu, cov)
return x[:, 0]
def UKFinit():
global ukf
ukf_fuse = []
p_std_x = rospy.get_param('~p_std_x',0.005)
v_std_x = rospy.get_param('~v_std_x',0.05)
p_std_y = rospy.get_param('~p_std_y',0.005)
v_std_y = rospy.get_param('~v_std_y',0.05)
dt = rospy.get_param('~dt',0.01) #100HZ
sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1.0)
ukf = UKF(dim_x=4, dim_z=4, fx=f_cv, hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([0., 0., 0., 0.,])
ukf.R = np.diag([p_std_x, v_std_x, p_std_y, v_std_y])
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=4.0)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=4.0)
ukf.P = np.diag([3, 1, 3, 1])
def ukf_process(x, P, sigma_range, sigma_bearing, dt=1.0):
points = MerweScaledSigmaPoints(n=3, alpha=0.001, beta=2, kappa=0,
subtract=residual)
# build the UKF based on previous functions
ukf = UKF(dim_x=3, dim_z=3, fx=move, hx=Hx,
dt=dt, points=points, x_mean_fn=state_mean,
z_mean_fn=z_mean, residual_x=residual,
residual_z=residual)
# assign the parameters of ukf
ukf.x = np.array(x)
ukf.P = P
ukf.R = np.diag([sigma_range**2, sigma_range**2, sigma_bearing**2])
ukf.Q = np.eye(3)*0.0001
return ukf
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 __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 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 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 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 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 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 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')
y = a - b
y[2] = normalize_angle(y[2])
return y
def f(x,dt,u):
"""Estimate the non-linear state of the system"""
#print ((u[0]/self.L)*math.tan(u[1]))
return np.array([x[0]+u[0]*math.cos(x[2]),
x[1]+u[0]*math.sin(x[2]),
x[2]+((u[0]/1)*math.tan(u[1]))])
def h(z):
return z
points = MerweScaledSigmaPoints(3,.001,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,1])
filter.Q = np.diag([(0.2)**2,(0.2)**2,(1*3.14/180)**2])
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
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))
f.predict()
f.update(array([z]))
correspond to that state.
"""
px = landmark[0]
py = landmark[1]
dist = np.sqrt((px - x[0])**2 + (py - x[1])**2)
Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]])
return Hx
points = MerweScaledSigmaPoints(n=3, alpha=1.e-3, beta=2, kappa=0)
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 = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .2])
ukf.R = np.diag([sigma_r**2, sigma_h**2])
ukf.Q = np.zeros((3,3))
u = array([1.1, .01])
xp = ukf.x.copy()
plt.figure()
plt.scatter(m[:, 0], m[:, 1])
for i in range(200):
xp = move(xp, u, dt/10., wheelbase) # simulate robot
plt.plot(xp[0], xp[1], ',', color='g')
if i % 10 == 0:
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 = []
cxs = []
radius = 100
def two_radar_constvel():
dt = 5
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])
pass
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1] * dt
x_est[2] += x[3] * dt
return x_est
f = UKF(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
aircraft = ACSim((100, 100), (0.1 * dt, 0.02 * dt), 0.002)
range_std = 0.2
bearing_std = radians(0.5)
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, 0.1, 100, 0.02])
f.R = np.diag([range_std ** 2, bearing_std ** 2, range_std ** 2, bearing_std ** 2])
q = Q_discrete_white_noise(2, var=0.002, dt=dt)
# q = np.array([[0,0],[0,0.0002]])
f.Q[0:2, 0:2] = q
f.Q[2:4, 2:4] = q
f.P = np.diag([0.1, 0.01, 0.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 = f.batch_filter(zs)
ms, _, _ = f.rts_smoother2(xs, Ps, Pxz)
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.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.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.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.show()
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
return np.dot(F, x)
def h_cv(x):
r = np.sqrt(x[0]**2 + x[2]**2)
angle = np.arctan2(x[2],x[0])
return np.array([r, angle])
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([400., 0., 300., 0.])
ukf.R = np.diag([25, 1])
ukf.Q[0:2,0:2] = Q_discrete_white_noise(2, dt, var=0.02)
ukf.Q[2:4,2:4] = Q_discrete_white_noise(2, dt, var=0.02)
uxs = []
zs = []
txs = []
radius = 100
delta = 2*np.pi/360*10
for i in range(5):
# 真实位置
target_pos_x = 300+math.cos(i*delta)*radius + random.randn()*0.0001
target_pos_y = 300+math.sin(i*delta)*radius + random.randn()*0.0001
txs.append((target_pos_x, target_pos_y))
# 测量位置
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 two_radar_constalt():
dt = 0.05
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])
pass
def fx(x, dt):
x_est = x.copy()
x_est[0] += x[1] * dt
return x_est
vx = 100 / 1000 # meters/sec
vz = 0.0
f = UKF(dim_x=3, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0)
aircraft = ACSim((0, 1), (vx * dt, vz * dt), 0.00)
range_std = 1 / 1000.0
bearing_std = 1 / 1000000.0
R1 = RadarStation((0, 0), range_std, bearing_std)
R2 = RadarStation((60, 0), range_std, bearing_std)
hx.R1 = R1
hx.R2 = R2
f.x = array([aircraft.pos[0], vx, aircraft.pos[1]])
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)
# q = np.array([[0,0],[0,0.0002]])
f.Q[0:2, 0:2] = q
f.Q[2, 2] = 0.0002
f.P = np.diag([0.1, 0.01, 0.1]) * 0.1
track = []
zs = []
for i in range(int(500 / 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 = 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(311)
plt.plot(time, track[:, 0])
plt.plot(time, xs[:, 0])
plt.legend(loc=4)
plt.xlabel("time (sec)")
plt.ylabel("x position (m)")
plt.subplot(312)
plt.plot(time, xs[:, 1] * 1000, label="UKF")
plt.plot(time, ms[:, 1] * 1000, label="RTS")
plt.legend(loc=4)
plt.xlabel("time (sec)")
plt.ylabel("velocity (m/s)")
plt.subplot(313)
plt.plot(time, xs[:, 2] * 1000, label="UKF")
plt.plot(time, ms[:, 2] * 1000, label="RTS")
plt.legend(loc=4)
plt.xlabel("time (sec)")
plt.ylabel("altitude (m)")
plt.ylim([900, 1100])
for z in zs[:10]:
p = R1.z_to_x(z[0], z[1])
# plt.scatter(p[0], p[1], marker='+', c='k')
p = R2.z_to_x(z[2], z[3])
# plt.scatter(p[0], p[1], marker='+', c='b')
plt.show()
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')
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])
cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)
v = cmds[-1][0]
