def __init__(self, vmax):
self.vmax_ = vmax # rospy.get_param('~vmax', 6.0)
sigma_points = MerweScaledSigmaPoints(4, 1e-3, 2, -2)
#sigma_points = JulierSigmaPoints(4, 5-2, sqrt_method=np.linalg.cholesky)
self.ukf_l_ = UKF(
dim_x=4, # x,y,vx,vy
dim_z=2, # x,y
dt=0.01, # note: dynamic
hx=ukf_hx,
fx=ukf_fx,
points=sigma_points)
self.ukf_r_ = UKF(
dim_x=4, # x,y,vx,vy
dim_z=2, # x,y
dt=0.01, # note: dynamic
hx=ukf_hx,
fx=ukf_fx,
points=sigma_points)
self.l_lost_ = 0
self.r_lost_ = 0
def __init__(self, dt=0.25, w=1.0):
self.fx_filter_vel = 0.0
self.fy_filter_vel = 0.0
self.fz_filter_vel = 0.0
self.fsigma_filter_vel = 0.0
self.fpsi_filter_vel = 0.0
self.fphi_filter_vel = 0.0
self.U_init = []
self.w = w
self.dt = dt
self.t = 0
self.number_state_variables = 6
# Q Process Noise Matrix [x, y, z, sigma, psi, phi]
self.Q = np.diag([0.5**2, 0.5**2, 0.5**2, 0.1**2, 0.1**2, 0.05**2])
# R Measurement Noise Matrix [xf, xr, yf, yr, zf, zr, za, delta, psi, phi]
self.R = np.diag([
3.5**2,
3.5**2, # x
3.5**2,
3.5**2, # y
1.5**2,
1.5**2,
3.5**2, # z
0.05**2,
0.05**2,
0.5**2
]) # delta - psi - phi
# Init Sigma points
self.sigma = [self.alpha, self.beta, self.kappa] = [0.7, 2.0, -2.0]
self.points = MerweScaledSigmaPoints(n=self.number_state_variables,
alpha=self.alpha,
beta=self.beta,
kappa=self.kappa)
# Create UKF filter based on filterpy library
self.kf = UKF(dim_x=self.number_state_variables,
dim_z=10,
dt=self.dt,
fx=self.f_bicycle,
hx=self.H_bicycle,
points=self.points)
# Q Process Noise Matrix
self.kf.Q = self.Q
# R Measurement Noise Matrix
self.kf.R = self.R
self.kf.P = np.eye(
self.number_state_variables) * 10 # Covariance matrix
self.P = self.kf.P
# only for consistency
self.K = np.zeros((6, 6)) # Kalman gain
self.y = np.zeros((6, 1)) # residual
self.H = np.zeros((10, 6)) # 10 measurements x 6 state variables
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 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 test_linear_2d_simplex():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = SimplexSigmaPoints(n=4)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 0.0001
zs = []
for i in range(20):
z = np.array([i + randn() * 0.1, i + randn() * 0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
if DO_PLOT:
zs = np.asarray(zs)
#plt.plot(zs[:,0])
plt.plot(Ms[:, 0])
plt.plot(smooth_x[:, 0], smooth_x[:, 2])
print(smooth_x)
def 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 _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_linear_2d_merwe():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 1.1
# test __repr__ doesn't crash
str(kf)
zs = [[i + randn() * 0.1, i + randn() * 0.1] for i in range(20)]
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
if DO_PLOT:
plt.figure()
zs = np.asarray(zs)
plt.plot(zs[:, 0], marker='+')
plt.plot(Ms[:, 0], c='b')
plt.plot(smooth_x[:, 0], smooth_x[:, 2], c='r')
print(smooth_x)
def __init__(self, param, SS_model, dim_x=2, dim_u=2, dim_y=2):
super().__init__(param)
self.SS_model = SS_model
self.last_time = 0
self.last_commend = {}
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2, kappa=1)
self.ukf = UKF(dim_x=dim_x,
dim_z=dim_y,
fx=self.fx,
hx=self.hx,
dt=param['dt'],
points=points)
self.ukf.R = np.diag([
0.0672, #Pressure transducer (DP) noise 0.0672
0.0187
]) #Voltage noise 0.00187
self.ukf.Q = np.diag([
0.15**2, #Pressure disturbance
0.2**2
]) #Torque disturbance
self.initialize_filter([param['q_pred'], param['w_pred']],
param['P_init'])
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
points = ukf.weights(1, 0.)
assert len(points) == 3
mean = 5
cov = 9
Xi = ukf.sigma_points(mean, cov, kappa)
xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, ukf.W):
m += x * w
assert abs(m - mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0, 0] - cov) < 1.e-12
assert Xi.shape == (3, 1)
assert len(ukf.W) == 3
def 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 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_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 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 filter_init(self):
## UKF filter parameters
n=self.n
m=self.m
dt = 0.01 #
self.dim_x = 20 #
self.dim_z = 20 #
#unscented points
self.points = sigma_points.SimplexSigmaPoints(n=self.dim_x) #
# initial cov
Pos_cov = 10*np.eye(m*n)
Vel_cov = np.eye(m*n)
P_up = np.concatenate((Pos_cov, 0*np.eye(self.m*self.n)),axis=1)
P_down = np.concatenate((0*np.eye(self.m*self.n), Vel_cov), axis=1)
P = np.concatenate((P_up,P_down),axis=0)
# initial process cov
Q = np.eye(self.m*self.n*2)*(dt**2.)
#meas cov
GPS_cov = 10*np.eye(m*n)
ACC_cov = np.eye(m*n)
R_up = np.concatenate((GPS_cov, 0*np.eye(m*n)),axis=1)
R_down = np.concatenate((0*np.eye(m*n), ACC_cov), axis=1)
R = np.concatenate((R_up,R_down),axis=0)
R = np.array(R) #
self.filter = UKF(dim_x=self.dim_x, dim_z=self.dim_z, dt=dt, hx=self.hx, fx=self.fx, points=self.points)
# self.filter.x = X
self.filter.P = P
self.filter.Q = Q
self.filter.R = R
def test_batch_missing_data():
""" batch filter should accept missing data with None in the measurements """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 0.0001
zs = []
for i in range(20):
z = np.array([i + randn() * 0.1, i + randn() * 0.1])
zs.append(z)
zs[2] = None
Rs = [1] * len(zs)
Rs[2] = None
Ms, Ps = kf.batch_filter(zs)
def test_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 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 __init__(self, sensor_std, state0, state):
'''
预测量：x, y, z, q0, q1, q2, q3
:param sensor_std:【float】sensor的噪声标准差[μV]
:param state0: 【np.array】初始值 (7,)
:param state: 【np.array】预测值 (7,)
'''
self.stateNum = 7 # 预测量：x, y, z, q0, q1, q2, q3
self.dt = 0.01 # 时间间隔[s]
self.points = MerweScaledSigmaPoints(n=self.stateNum,
alpha=0.3,
beta=2.,
kappa=3 - self.stateNum)
self.ukf = UKF(dim_x=self.stateNum,
dim_z=self.measureNum,
dt=self.dt,
points=self.points,
fx=self.f,
hx=self.h)
self.ukf.x = state0.copy() # 初始值
self.x0 = state # 计算NEES的真实值
self.ukf.R *= sensor_std
self.ukf.P = np.eye(self.stateNum) * 0.01
for i in range(3, 7):
self.ukf.P[i, i] = 0.01
self.ukf.Q = np.eye(
self.stateNum) * 0.001 * self.dt # 将速度作为过程噪声来源，Qi = [v*dt]
for i in range(3, 7):
self.ukf.Q[i, i] = 0.01 # 四元数的过程误差
self.pos = (round(self.ukf.x[0], 3), round(self.ukf.x[1],
3), round(self.ukf.x[2], 3))
self.m = q2R(self.ukf.x[3:7])[:, -1]
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 sensor_fusion_kf():
global hx, fx
# create unscented Kalman filter with large initial uncertainty
points = JulierSigmaPoints(2, kappa=2)
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points)
kf.x = np.array([100, 100.])
kf.P *= 40
return kf
def 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 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 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 create_filter():
dim_x = 5
points = MerweScaledSigmaPoints(dim_x, alpha = 1e-3, beta = 2, kappa = 0.0)
ukf = UKF(dim_x = dim_x, dim_z = 2, dt = 0.1, hx = hx, fx = fx, points = points)
ukf.Q *= np.identity(5) * 0.2
std_las = 0.15
ukf.R *= std_las * std_las
ukf.x = np.zeros(5)
ukf.P *= 0.2
return ukf
def __init__(self, dt=0.25, w=1.0):
self.fx_filter_vel = 0.0
self.fy_filter_vel = 0.0
self.fz_filter_vel = 0.0
self.fsigma_filter_vel = 0.0
self.fpsi_filter_vel = 0.0
self.fphi_filter_vel = 0.0
# Q Process Noise Matrix
self.Q = np.diag([0.5**2, 0.5**2, 0.5**2, 0.1**2, 0.1**2,
0.05**2]) # [x, y, z, sigma, psi, phi]
# R Measurement Noise Matrix
self.R = np.diag([8.5**2, 8.5**2, 8.5**2, 1.8**2, 8.5**2,
1.8**2]) # [x, y, z, sigma, psi, phi]
# Sigma points
self.sigma = [self.alpha, self.beta, self.kappa] = [0.9, 0.5, -2.0]
self.w = w
self.dt = dt
self.t = 0
self.number_state_variables = 6
self.points = MerweScaledSigmaPoints(n=self.number_state_variables,
alpha=self.alpha,
beta=self.beta,
kappa=self.kappa)
self.kf = UKF(dim_x=self.number_state_variables,
dim_z=self.number_state_variables,
dt=self.dt,
fx=self.f_bicycle,
hx=self.H_bicycle,
points=self.points)
# Q Process Noise Matrix
self.kf.Q = self.Q
# R Measurement Noise Matrix
self.kf.R = self.R
# H identity
self.H = np.eye(6)
self.K = np.zeros((6, 6)) # Kalman gain
self.y = np.zeros((6, 1)) # residual
self.kf.x = np.zeros((1, self.number_state_variables)) # Initial state
self.kf.P = np.eye(
self.number_state_variables) * 10 # Covariance matrix
self.P = self.kf.P
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 __init__(self, trueTrajectory, dt=.1, noise=0.):
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.kalman import MerweScaledSigmaPoints as SigmaPoints
self.dt = dt
sigmas = SigmaPoints(3, alpha=.1, beta=2., kappa=0.)
self.KF = UKF(dim_x=3,
dim_z=2,
fx=f_kal_a,
hx=h_kal,
dt=dt,
points=sigmas)
self.KF.Q = np.diag([1., 0.5, 0.2])
self.KF.R = np.diag([2., 1.12]) * noise + np.diag([.05, .05])
self.first = True
def test_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])