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 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_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2], [1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000)
sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3)
sp3 = SimplexSigmaPoints(n=2)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
w2, _ = sp2.weights()
w3, _ = sp3.weights()
Xi0 = sp0.sigma_points(x, P)
Xi1 = sp1.sigma_points(x, P)
Xi2 = sp2.sigma_points(x, P)
Xi3 = sp3.sigma_points(x, P)
assert max(Xi1[:, 0]) > max(Xi0[:, 0])
assert max(Xi1[:, 1]) > max(Xi0[:, 1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0],
(Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1],
color='blue',
label='Julier low $\kappa$')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0],
(Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1],
color='green',
label='Julier high $\kappa$')
# for i in range(Xi2.shape[0]):
# plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0],
# (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1],
# color='red')
for i in range(Xi3.shape[0]):
plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0],
(Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1],
color='black',
label='Simplex')
stats.plot_covariance_ellipse([1, 2], P)
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 = UnscentedKalmanFilter(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
mss = MerweScaledSigmaPoints(4, .1, 2., -1)
ukf = UnscentedKalmanFilter(dim_x=4,
dim_z=2,
dt=dt,
fx=fx,
hx=hx,
points=mss)
ukf.x = np.array([-1., 1., -1., 1])
ukf.P *= 1
x = kf.x
zs = []
for i in range(20):
x = fx(x, dt)
z = np.array([x[0] + randn() * 0.1, x[2] + randn() * 0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt)
if DO_PLOT:
zs = np.asarray(zs)
plt.plot(Ms[:, 0])
plt.plot(smooth_x[:, 0], smooth_x[:, 2])
print(smooth_x)
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, bbox):
"""
Initialises a tracker using initial bounding box.
"""
def fx(x, dt):
# state transition function - predict next state based on constant velocity model x = x_0 + vt
F = np.array([[1, 0, 0, 0, dt, 0, 0], [0, 1, 0, 0, 0, dt, 0],
[0, 0, 1, 0, 0, 0, dt], [0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement where measurements are [u, v, s, r]
return np.array([x[0], x[1], x[2], x[3]])
dt = 0.1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(7, alpha=.1, beta=2., kappa=-1)
#define constant velocity model
self.kf = UnscentedKalmanFilter(dim_x=7,
dim_z=4,
dt=dt,
fx=fx,
hx=hx,
points=points)
# Assign the initial value of the state
self.kf.x[:4] = convert_bbox_to_z(bbox)
# Covariance matrix (dim_x, dim_x), default I
self.kf.P[
4:,
4:] *= 1000. # Give high uncertainty to the unobservable initial velocities
self.kf.P *= 0.2
# Measuerment uncertainty (dim_z, dim_z), default I
self.kf.R *= 0.01
# Process noise (dim_x, dim_x), default I
self.kf.Q[-1, -1] *= 0.0001
self.kf.Q[4:, 4:] *= 0.0001
self.time_since_update = 0
self.id = KalmanBoxTracker.count
KalmanBoxTracker.count += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
def __init__(self, dt=1.0, w=1.0):
self.dim_x = 6
self.dim_z = 2
self._w = w
sigma_points = MerweScaledSigmaPoints(n=self.dim_x,
alpha=.3,
beta=2.,
kappa=-3)
super(ukfFilter, self).__init__(dim_x=self.dim_x,
dim_z=self.dim_z,
fx=self.f_ct,
hx=self.h_ca,
dt=dt,
points=sigma_points)
def initialize_ukf(self):
"""
Initialize the parameters of the Unscented Kalman Filter (UKF) that is
used to estimate the state of the drone.
"""
# Number of state variables being tracked
self.state_vector_dim = 7
# The state vector consists of the following column vector.
# Note that FilterPy initializes the state vector with zeros.
# [[x],
# [y],
# [z],
# [x_vel],
# [y_vel],
# [z_vel],
# [yaw]]
# Number of measurement variables that the drone receives
self.measurement_vector_dim = 6
# The measurement variables consist of the following vector:
# [[slant_range],
# [x],
# [y],
# [x_vel],
# [y_vel],
# [yaw]]
# Function to generate sigma points for the UKF
# TODO: Modify these sigma point parameters appropriately. Currently
# just guesses
sigma_points = MerweScaledSigmaPoints(n=self.state_vector_dim,
alpha=0.1,
beta=2.0,
kappa=(3.0 -
self.state_vector_dim))
# Create the UKF object
# Note that dt will get updated dynamically as sensor data comes in,
# as will the measurement function, since measurements come in at
# distinct rates. Setting compute_log_likelihood=False saves some
# computation.
self.ukf = UnscentedKalmanFilter(dim_x=self.state_vector_dim,
dim_z=self.measurement_vector_dim,
dt=1.0,
hx=self.measurement_function,
fx=self.state_transition_function,
points=sigma_points,
compute_log_likelihood=False)
self.initialize_ukf_matrices()
def __init__(
self,
filter_settings: RobotFilterSettings,
dt: float = 1 / 60,
points: Optional[Any] = None,
):
self._log = structlog.get_logger().bind(dt=dt, points=points)
self._settings = filter_settings
super().__init__(
dim_x=6,
dim_z=3,
dt=dt,
hx=self._hx,
fx=self._fx,
points=points
or MerweScaledSigmaPoints(4, alpha=0.1, beta=2.0, kappa=-1),
x_mean_fn=self._x_mean_fn,
z_mean_fn=self._z_mean_fn,
residual_x=self._residual_x,
residual_z=self._residual_z,
)
# set initial state to all zeros, and initialize the state
# covariances
self.set_state(np.zeros((6, 1)))
# TODO(dschwab): taken from robot tracker. I'm not sure this
# is 100% correct or at least 100% the best it can be. Seems
# to be assuming that given a perfect velocity, our process
# will give us a perfect position. At the least this does not
# account for bad friction models, pushing, slippage, etc
# where velocity integration over the timestep is not 100%
# correct.
self.Q = np.diag([
0,
0,
0,
self._settings.velocity_variance,
self._settings.velocity_variance,
self._settings.angvel_variance,
])
# TODO(dschwab): taken from robot tracker. I'm not sure this
# is 100% correct or at least 100% the best it can be.
self.R = np.diag([
self._settings.position_variance,
self._settings.position_variance,
self._settings.theta_variance,
])
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
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
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 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 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_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 _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 = 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.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()
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
s.to_array()
if DO_PLOT:
plt.plot(s.x[:, 0], s.x[:, 2])
def __init__(self, dt=1.0, w=1.0):
self.dim_x = 4
self.dim_z = 2
self._w = w
self._dt = dt
self.Ft = np.eye(self.dim_x)
self._T = 0.
sigma_points = MerweScaledSigmaPoints(n=self.dim_x,
alpha=.1,
beta=2.,
kappa=-1.)
super(ukfFilterct2, self).__init__(dim_x=self.dim_x,
dim_z=self.dim_z,
fx=self.f_ct,
hx=self.h_ca,
dt=dt,
points=sigma_points)
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 plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
def fx(x):
return x**3
def dfx(x):
return 3*x**2
mean = 1
var = .1
std = math.sqrt(var)
data = normal(loc=mean, scale=std, size=50000)
d_t = fx(data)
points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
Wm, Wc = points.Wm, points.Wc
sigmas = points.sigma_points(mean, var)
sigmas_f = np.zeros((3, 1))
for i in range(3):
sigmas_f[i] = fx(sigmas[i, 0])
### pass through unscented transform
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
ukf_mean = ukf_mean[0]
ukf_std = math.sqrt(ukf_cov[0])
norm = scipy.stats.norm(ukf_mean, ukf_std)
xs = np.linspace(-3, 5, 200)
plt.plot(xs, norm.pdf(xs), ls='--', lw=2, color='b')
try:
plt.hist(d_t, bins=200, density=True, histtype='step', lw=2, color='g')
except:
# older versions of matplotlib don't have the density keyword
plt.hist(d_t, bins=200, normed=True, histtype='step', lw=2, color='g')
actual_mean = d_t.mean()
plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo')
plt.axvline(ukf_mean, lw=2, ls='--', color='b', label='UKF')
plt.legend()
plt.show()
print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std()))
print('UKF mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
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 __init__(self, number_of_points, size, arena_origin_position):
self.tracker = ukf.MultiUnscentedKalmanFilter(np.array(
[0, 0, 0, 0, 0, 0, 2, 0]),
np.eye(8) * 500,
create_Q,
Q_generator_args=(0.1, ))
self.tracker.filters["pose_array"] = ukf.UnscentedKalmanFilter(
8, 4, 0, h_pose, f, MerweScaledSigmaPoints(8, 1, 3, -5))
stride = size / (number_of_points - 1.0)
self.number_of_features = number_of_points * number_of_points
self.features_positions = np.empty((self.number_of_features, 2))
for x in xrange(number_of_points):
for y in xrange(number_of_points):
self.features_positions[x + y * number_of_points] = np.array(
[x * stride, y * stride]) - arena_origin_position
self.lastMesuredStatus = None
def __init__(self, dt):
self.dt = dt
H = np.array([[0, 1, 0, 1], [0, 0, 1, 0]])
def h(x):
return np.dot(H, x)
sigmas = MerweScaledSigmaPoints(4,
alpha=self.alpha,
beta=self.beta,
kappa=self.kappa)
UKF.__init__(self,
dim_x=4,
dim_z=2,
fx=self.f_xu,
hx=h,
dt=dt,
points=sigmas)
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 test_propagate(self):
dt = 1
bike_lr, bike_lf, car_lr, car_lf, sigma_phi, sigma_v, sigma_d = (1.,
1.,
1.,
1.,
1.,
1.,
1.)
model = BicycleModel(dt, bike_lr, bike_lf, car_lr, car_lf, sigma_phi,
sigma_v, sigma_d)
dim_x = 5
dim_z = 2
points = MerweScaledSigmaPoints(dim_x, alpha=1, beta=2., kappa=2)
self.assertAlmostEqual(sum(points.Wm), 1)
print()
print(points.Wm)
dt = 1
measurement_model = np.array([[1, 0, 0, 0, 0], [0, 1, 0, 0, 0]])
measurement_noise = 1 * np.eye(2)
state = np.array([1., 1., pi / 2, 1., 0])
variance = np.eye(dim_x) * 0.2 # initial uncertainty\
kf = UKF(dim_x=dim_x,
dim_z=dim_z,
dt=dt,
points=points,
motion_model=model,
measurement_model=measurement_model)
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = np.eye(dim_x)
zs = [[i + np.random.randn() * z_std, i + np.random.randn() * z_std]
for i in range(50)] # measurements
_state, _var = kf.predict(state,
variance,
model,
motion_noise=kf.Q,
object_class='Car')
print("State: {}".format(_state))
print("Var: {}".format(_var))
def setup():
# set up sensor simulator
H = np.array([[1, 0, 0], [0, 1, 0]])
def h(x):
return np.dot(H, x)
x0 = np.array([[0], [-1], [0]])
sim = Sensor(x0, f, h)
# set up kalman filter
sigmas = MerweScaledSigmaPoints(3, alpha=.1, beta=2., kappa=1)
tracker = UKF(dim_x=3, dim_z=2, fx=f, hx=h, dt=dt, points=sigmas)
tracker.Q = np.diag((var_pos, var_pos, var_heading))
tracker.R = R_proto * measurement_var * filter_misestimation_factor
tracker.x = np.array([0, 0, 0])
tracker.P = np.eye(3) * 500
return sim, tracker
def __init__(self, g_params, dim_z=3):
self.sigmas = MerweScaledSigmaPoints(n=2, alpha=.1, beta=10., kappa=0.)
self.ukf = UnscentedKalmanFilter(dim_x=2,
dim_z=dim_z,
dt=1.,
hx=self.hx,
fx=self.fx,
points=self.sigmas)
self.s1params_a = g_params[0]
self.s1params_b = g_params[1]
self.s1params_c = g_params[2]
self.s2params_a = g_params[3]
self.s2params_b = g_params[4]
self.s2params_c = g_params[5]
self.s3params_a = g_params[6]
self.s3params_b = g_params[7]
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 smooth(data: ndarray, dt: float):
points = MerweScaledSigmaPoints(3, alpha=1e-3, beta=2.0, kappa=0)
noisy_kalman = UnscentedKalmanFilter(
dim_x=3,
dim_z=1,
dt=dt,
hx=state_to_measurement,
fx=state_transition,
points=points,
)
noisy_kalman.x = array([0, data[1], data[1] - data[0]], dtype="float32")
noisy_kalman.R *= 20**2 # sensor variance
noisy_kalman.P = diag([5**2, 5**2, 1**2]) # variance of the system
noisy_kalman.Q = Q_discrete_white_noise(3, dt=dt, var=0.05)
means, covariances = noisy_kalman.batch_filter(data)
means[:, 1][means[:, 1] < 0] = 0 # clip velocity
return means[:, 1]
