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 __init__(self, initx=0., inity=0.,inith=0):
inith = float(inith)
# xfilter smoothes the movement on the x axis
self.xfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.xfilter.x = np.array([[initx], [0.]])
self.xfilter.F = np.array([[1., 1.], [0., 1.]])
self.xfilter.H = np.array([[1., 1]])
self.xfilter.P *= 10 ** 4
self.xfilter.R = 50.0
self.xfilter.Q = Q_discrete_white_noise(2, 1.0, 1.0)
# yfilter smoothes the movement on the y axis
self.yfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.yfilter.x = np.array([[inity], [0.]])
self.yfilter.F = np.array([[1., 1.], [0., 1.]])
self.yfilter.H = np.array([[1., 50.]])
self.yfilter.P *= 10.0 ** 4
self.yfilter.R = 50.0
self.yfilter.Q = Q_discrete_white_noise(2, 1.0, 1.0)
# hfilter or heightfilter smoothes out the height changes of the boxes
self.hfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.hfilter.x = np.array([[inith],[.5]])
self.hfilter.F = np.array([[1., 1.],[0., 1.]])
self.hfilter.H = np.array([[1., 1.]])
self.hfilter.P *= 10.0**4
self.hfilter.R *= 100.0
self.hfilter.Q *= 0.001
def __init__(self, pred_window, dataset_path, results_path):
config_path = os.path.join(os.getcwd(), 'config.toml')
self.cfg = toml.load(config_path)
self.dt = self.cfg['dt']
self.pred_window = pred_window * 1e-3 # convert to seconds
self.dataset_path = dataset_path
self.results_path = results_path
self.coords = self.cfg['pos_coords'] + self.cfg['quat_coords']
self.kf = KalmanFilter(dim_x=self.cfg['dim_x'],
dim_z=self.cfg['dim_z'])
setattr(self.kf, 'x_pred', self.kf.x)
# First-order motion model: insert dt into the diagonal blocks of F
f = np.array([[1.0, self.dt], [0.0, 1.0]])
self.kf.F = block_diag(f, f, f, f, f, f, f)
# Inserts 1 into the blocks of H to select the measuremetns
np.put(self.kf.H, np.arange(0, self.kf.H.size, self.kf.dim_x + 2), 1.0)
self.kf.R *= self.cfg['var_R']
Q_pos = Q_discrete_white_noise(dim=2,
dt=self.dt,
var=self.cfg['var_Q_pos'],
block_size=3)
Q_ang = Q_discrete_white_noise(dim=2,
dt=self.dt,
var=self.cfg['var_Q_ang'],
block_size=4)
self.kf.Q = block_diag(Q_pos, Q_ang)
def __init__(self, bbox, start_time):
"""
Initialises a tracker using initial bounding box
dt is the time between two measurements in seconds.
"""
#define constant velocity model
self.kf = KalmanFilter(dim_x=5, dim_z=3)
dt = 1. / 30
self.kf.F = np.array([[1, dt, 0, 0, 0], [0, 1, 0, 0, 0],
[0, 0, 1, dt, 0], [0, 0, 0, 1, 0],
[0, 0, 0, 0, 1]])
self.kf.H = np.array([[1, 0, 0, 0, 0], [0, 0, 1, 0, 0],
[0, 0, 0, 0, 1]])
std_x, std_y, std_z = 2, 2, 0.8 #TODO : tune
self.kf.R = np.diag([std_x**2, std_y**2, std_z**2])
self.kf.P[1, 1] = self.kf.P[3, 3] = 1000.
#give high uncertainty to the unobservable initial velocities
self.kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt,
var=1.2) #x-accel
self.kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt,
var=1.2) #y_accel
self.kf.Q[4, 4] = (1.5 * dt)**2 #z-vel
self.kf.x[[0, 2, 4], 0] = bbox[:3]
self.dims = [0.4, 1.78]
self.current_time = start_time
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 test_procedural_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0])
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.eye(2) * 1000.
f.R = np.eye(1) * 5
f.Q = Q_discrete_white_noise(2, 1., 0.0001)
f.test_matrix_dimensions()
x = np.array([2., 0])
F = np.array([[1., 1.], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2) * 1000.
R = np.eye(1) * 5
Q = Q_discrete_white_noise(2, 1., 0.0001)
zs = [13., None, 1., 2.] * 10
m, c, _, _ = f.batch_filter(zs, update_first=False)
n = len(zs)
mp, cp, _, _ = batch_filter(x, P, zs, [F] * n, [Q] * n, [H] * n, [R] * n)
for x1, x2 in zip(m, mp):
assert np.allclose(x1, x2)
for p1, p2 in zip(c, cp):
assert np.allclose(p1, p2)
def test_procedural_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0])
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.eye(2) * 1000.
f.R = np.eye(1) * 5
f.Q = Q_discrete_white_noise(2, 1., 0.0001)
f.test_matrix_dimensions()
x = np.array([2., 0])
F = np.array([[1., 1.], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2) * 1000.
R = np.eye(1) * 5
Q = Q_discrete_white_noise(2, 1., 0.0001)
zs = [13., None, 1., 2.] * 10
m, c, _, _ = f.batch_filter(zs, update_first=False)
n = len(zs)
# test both list of matrices, and single matrix forms
mp, cp, _, _ = batch_filter(x, P, zs, F, Q, [H] * n, R)
for x1, x2 in zip(m, mp):
assert abs(sum(sum(x1))) - abs(sum(sum(x2))) < 1.e-12
for p1, p2 in zip(c, cp):
assert abs(sum(sum(p1))) - abs(sum(sum(p2))) < 1.e-12
def publish(self):
# loop to publish the noisy odometry values
while not rospy.is_shutdown():
Qp = Q_discrete_white_noise(3,dt=0.1,var=1e-2,block_size=3, order_by_dim=False)
Qo = Q_discrete_white_noise(2,dt=0.1,var=1e-4,block_size=3, order_by_dim=False)
print(Qp)
print(Qo)
self.rate.sleep()
class CarTracker:
ID = 0
dt = 1.0 / 15.0
R_x_std = 0.047 # update to the correct value
Q_x_std = 0.35 # update to the correct value
R_y_std = 0.052 # update to the correct value
Q_y_std = 0.42 # update to the correct value
time_since_last_update = 0.0
p_x = KalmanFilter(dim_x=3, dim_z=1)
p_y = KalmanFilter(dim_x=3, dim_z=1)
p_x.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
p_y.F = np.array([[1., dt, 0.5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
p_x.H = np.array([[1., 0., 0.]])
p_y.H = np.array([[1., 0., 0.]])
p_x.Q = Q_discrete_white_noise(dim=3, dt=dt, var=Q_x_std**2)
p_y.Q = Q_discrete_white_noise(dim=3, dt=dt, var=Q_y_std**2)
p_x.R *= R_x_std**2
p_y.R *= R_y_std**2
def __init__(self, dt, ID, position_x, position_y):
self.dt = dt
self.ID = ID
self.p_x.x = np.array([[position_x], [0.], [0.]])
self.p_y.x = np.array([[position_y], [0.], [0.]])
self.p_x.P *= 1000. # can very likely be set to 100.
self.p_y.P *= 1000. # can very likely be set to 100.
def update_pose(self, position_x, position_y):
self.time_since_last_update = 0.0 # reset time since last update
self.p_x.update([[position_x]])
self.p_y.update([[position_y]])
def predict_pose(self):
self.time_since_last_update += self.dt #update timer with prediction
self.p_x.predict()
self.p_y.predict()
def get_last_update_time(self):
return self.time_since_last_update
def get_position(self):
return [self.p_x.x[0], self.p_y.x[0]]
def get_current_error(self):
return [(self.p_x.P[0])[0], (self.p_y.P[0])[0]]
def get_total_speed(self):
# calculate magnitude of speed
return np.sqrt(self.p_x.x[1]**2 + self.p_y.x[1]**2) #sqrt(v_x²+v_y²)
def get_ID(self):
return self.ID
def test_circle():
def hx(x):
return np.array([x[0], x[3]])
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.]])
def fx(x, dt):
return np.dot(F, x)
x = np.array([50., 0., 0, 0, .0, 0.])
P = np.eye(6)* 100.
f = EnKF(x=x, P=P, dim_z=2, dt=1., N=30, hx=hx, fx=fx)
std_noise = .1
f.R *= std_noise**2
f.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .001)
f.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .001)
measurements = []
results = []
zs = []
for t in range (0,300):
a = t / 300000
x = cos(a) * 50.
y = sin(a) * 50.
# create measurement = t plus white noise
z = np.array([x,y])
zs.append(z)
f.predict()
f.update(z)
# save data
results.append (f.x)
measurements.append(z)
#test that __repr__ doesn't assert
str(f)
results = np.asarray(results)
measurements = np.asarray(measurements)
if DO_PLOT:
plt.plot(results[:,0], results[:,2], label='EnKF')
plt.plot(measurements[:,0], measurements[:,1], c='r', label='z')
#plt.plot (results-ps, c='k',linestyle='--', label='3$\sigma$')
#plt.plot(results+ps, c='k', linestyle='--')
plt.legend(loc='best')
plt.axis('equal')
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 run(self):
last_state_read = time.time()
c = 0
while not self.stop:
state = self.memcached_client.get('state')
if state:
state = json.loads(state)
heading = state['yaw'] * np.pi / 180
# Pozyx increases CW, so make it CCW
heading = 2 * np.pi - heading
# Subject value Pozyx reads for heading when it should read 0
heading -= self.pozyx_offset
heading = normalize_angle(heading)
if self.pozyx_offset == 0:
print(heading)
x, y = state['x'], state['y']
now = time.time()
dt = now - last_state_read
dx, dy, dheading = self._compute_change_from_encoders(
dt, self.heading if self.heading else heading)
# Body fixed linear acceleration
lin_accel = np.array([state[f'd2{i}'] for i in 'xyz'])
# Pozyx fixed accelerations
# Start by getting quaternion
quart = Quaternion(*(state[f'q{i}'] for i in 'wxyz'))
# Then rotate by its inverse
pozyx_accel = quart.inverse.rotate(lin_accel)
# Y from Pozyx is vertical, so grab the Z value instead
d2x, d2y = pozyx_accel[0], pozyx_accel[2]
z = np.array([x, dx, d2x, y, dy, d2y, heading, dheading])
self.KF.F = self.state_transition_matrix(dt)
q = Q_discrete_white_noise(dim=3, dt=dt, var=0.001)
self.KF.Q = block_diag(
q, q, Q_discrete_white_noise(dim=2, dt=dt, var=0.001))
self.KF.predict()
self.KF.update(z)
self.lock.acquire()
self._state = self.KF.x
self.lock.release()
self.heading = self._state[6]
self.location = (self._state[0], self._state[3])
last_state_read = now
#if c % 5 == 0:
#print(f'{dt:.6f}: {x} {dx:6f} {d2x:6f} {y} {dy:6f} {d2y:6f} {heading:.6f} {dheading}')
#print(self)
c += 1
time.sleep(0.1)
def build_Q(self):
""" process noise """
var_pos = self.q_var_pos
var_size = self.q_var_size
q_pos = var_pos if self.order_pos == 0 else Q_discrete_white_noise(
dim=self.order_pos + 1, dt=self.dt, var=var_pos)
q_size = var_size if self.order_size == 0 else Q_discrete_white_noise(
dim=self.order_size + 1, dt=self.dt, var=var_size)
diag_components = [q_pos] * self.dim_pos + [q_size] * self.dim_size
return block_diag(*diag_components)
def run_standard_kf(zs, dt=1.0, std_x=0.3, std_y=0.3):
kf = KalmanFilter(4, 2)
kf.x = np.array([0.0, 0.0, 0.0, 0.0])
kf.R = np.diag([std_x**2, std_y**2])
kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
kf.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)
xs, *_ = kf.batch_filter(zs)
return xs
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1.,0.]])
P = np.eye(2)
R = np.eye(1)*std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range (2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append (x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def init_kf(self, pos): self.kf_x.x = np.array([[pos[0]], [0.]]) self.kf_x.F = np.array([[1., 1.], [0., 1.]]) self.kf_x.H = np.array([[1., 0.]]) self.kf_x.P = np.array([[1000., 0.], [0., 1000.]]) self.kf_x.R = 5 self.kf_x.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13) self.kf_y.x = np.array([[pos[1]], [0.]]) self.kf_y.F = np.array([[1., 1.], [0., 1.]]) self.kf_y.H = np.array([[1., 0.]]) self.kf_y.P = np.array([[1000., 0.], [0., 1000.]]) self.kf_y.R = 5 self.kf_y.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1) * std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append(x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def init_tracker(tracker): dt = 0.2 # time step 0.2 second dt2 = dt*dt; g = 9.96 #in m/s^2 var_mouse = 0.01 ** 2 #in metres var_hdg = 0.005 ** 2 var_ang_vel =0.002 ** 2 var_acceleration = (0.01*g)**2 tracker.F = np.array([[1, dt, .5*dt2, 0, 0, 0, 0, 0], [0, 1, dt, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, dt, .5*dt2, 0, 0], [0, 0, 0, 0, 1, dt, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 0, 0, 1]]) q1 = Q_discrete_white_noise(dim=3, dt=dt, var=g*(0.1)) #In practice we pick a number, run simulations on data, and choose a value that works well. q2 = block_diag(q1,q1); q3 = Q_discrete_white_noise(dim=2, dt=dt, var=1) tracker.Q = block_diag(q2, q3) tracker.H = np.array([[0, dt, 0, 0, 0, 0, 0, 0], #matrix to transform from state space to measurement space. [0, 0, 1/g, 0, 0, 0, 0, 0], [0, 0, 0, 0, dt, 0, 0, 0], [0, 0, 0, 0, 0, 1/g, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1]]) tracker.R = np.array([[var_mouse, 0, 0, 0, 0, 0], [0,var_acceleration, 0, 0, 0, 0], [0, 0,var_mouse, 0, 0, 0], [0, 0, 0,var_acceleration, 0, 0], [0, 0, 0, 0, var_hdg, 0], [0, 0, 0, 0, 0, var_ang_vel]]) tracker.x = np.array([[0, 0, 0, 0, 0, 0, 0, 0]]).T #initial state when bot starts from starting point. check for theta (magnetometer measurement) tracker.P = np.array([[0.1 **2, 0, 0, 0, 0, 0, 0, 0], #initial uncertainity in x_coordinate = +-10 m [0, 0.1**2, 0, 0, 0, 0, 0, 0], [0, 0, (g*0.01)**2, 0, 0, 0, 0, 0], [0, 0, 0, 0.1**2, 0, 0, 0, 0], [0, 0, 0, 0, 0.1**2, 0 , 0, 0], [0, 0, 0, 0, 0, (g*0.01)**2, 0, 0], [0, 0, 0, 0, 0, 0, 6.279**2, 0], [0, 0, 0, 0, 0, 0, 0, 0.01**2]]) return tracker
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 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 __init__(self, dt, state):
"""
"""
self.F = np.array([[1, dt], [0, 1]])
self.Q = Q_discrete_white_noise(dim=state.dim, dt=dt, var=2.35)
self.B = 0.0
self.u = 0.0
def initKalman(self, X, Y, dt_average):
'''
Description :
Input :
Output :
'''
k_filter = KalmanFilter(dim_x=4, dim_z=2)
dt = dt_average #time step in seconds
k_filter.x = np.array([X[0], X[1] - X[0], Y[0], Y[1] - Y[0]
]).T # initial state (x and y position)
k_filter.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]]) # state transition matrix
q = Q_discrete_white_noise(dim=2, dt=dt,
var=np.cov(X,
Y)[0][0]) # process uncertainty
k_filter.Q = block_diag(q, q)
k_filter.H = np.array([[1, 0, 0, 0], [0, 0, 1,
0]]) # Measurement function
k_filter.R = np.cov(X, Y) # state uncertainty
P = np.eye(4)
covar = np.cov(X, Y)
P[0][0] = covar[0][0]
P[1][1] = covar[1][0]
P[2][2] = covar[0][1]
P[3][3] = covar[1][1]
k_filter.P = P # covariance matrix
return k_filter
def kfilter(self):
tracker = KalmanFilter(dim_x=8, dim_z=8)
dt = 1.0 # time step
d3 = 0.0 #d-triangle, correlation between VXs
dc = 0.0 #d-circle, correlation between VYs
a = 1
#cx1=1,cx2=0.5,cx3=0.25
tracker.F = np.array([ #x1 vx y1 vy x2 vx2 y2 vy2
[1, dt, 0, 0, a, 0, 0, 0], [0, 1, 0, 0, 0, a, 0, 0],
[0, 0, 1, dt, 0, 0, a, 0], [0, 0, 0, 1, 0, 0, 0, a],
[0, 0, 0, 0, 1, dt, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 0, 0, 1]
])
tracker.u = 0.
tracker.H = np.array([[1, 0, 0, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0,
0], [0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0,
0], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1]])
tracker.R = np.eye(8) * self.R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=self.Q_std**2)
tracker.Q = block_diag(q, q) #tracker.Q.dim = 4
tracker.Q = block_diag(tracker.Q, tracker.Q) #tracker.Q.dim = 8
v = 0.00001
tracker.x = np.array([[0, v, 0, v, 0, v, 0, v]]).T
tracker.P = np.eye(
8
) * 225. # (3*5)^2, 25,35,75,85 araliginda 5er li dagilim in 3 sigma variansi.
return tracker
def init_filter(self, x, y):
"""
Initializes kalmanfilter at given position
:param x: start x position of the ball
:param y: start y position of the ball
"""
# initial value of position(x,y) of the ball and velocity
self.kf.x = np.array([x, y, 0, 0])
# transition matrix
self.kf.F = np.array([[1.0, 0.0, 1.0, 0.0], [0.0, 1.0, 0.0, 1.0],
[0.0, 0.0, self.velocity_factor, 0.0],
[0.0, 0.0, 0.0, self.velocity_factor]])
# measurement function
self.kf.H = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
# multiplying by the initial uncertainty
self.kf.P = np.eye(4) * 1000
# assigning measurement noise
self.kf.R = np.array([[1, 0], [0, 1]]) * self.measurement_certainty
# assigning process noise
self.kf.Q = Q_discrete_white_noise(dim=2,
dt=self.filter_time_step,
var=self.process_noise_variance,
block_size=2,
order_by_dim=False)
self.filter_initialized = True
def start_ct(self, std_r=1.0, v=0.1, vstart=None, dt=None, w=None):
if vstart is None:
# x vx y vy
self.x = np.array([0., 0., 0., 0.])
else:
self.x = np.array(vstart)
if dt is None:
dt = self._dt
else:
self._dt = dt
if w is None:
w = self._w
else:
self._w = w
self.setup_function()
# process noise
self.Q = Q_discrete_white_noise(dim=2, dt=dt, var=v, block_size=2)
# covariance estimation matrix
self.P = np.eye(4) * 2.
# measurement noise
self.R = np.diag([std_r**2, std_r**2])
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 Q_func(self, dt, theta):
# TODO: maybe expand this out for speed. For now, easier to program and debug
q3 = Q_discrete_white_noise(3, dt=dt, var=1.0)
q_sp_1 = np.zeros((5, 5))
q_sp_1[0:3, 0:3] = self.var_a_s * q3
q_sp_1[3:5, 3:5] = self.var_a_p * q3[0:2, 0:2]
trans1 = np.array([[1, 0, 0, 0, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0],
[0, 0, 1, 0, 0], [0, 0, 0, 0, 1]])
q_sp = trans1 @ q_sp_1 @ trans1.T
Q_spa = np.eye(8)
Q_spa[0:5, 0:5] = q_sp
Q_spa[5:8, 5:8] = self.var_a_a * q3
c = math.cos(theta)
s = math.sin(theta)
T = np.eye(8)
T[0, 0] = c
T[0, 1] = -s
T[1, 0] = s
T[1, 1] = c
Q_xya = T @ Q_spa @ T.T # @ is matrix multiply; Python >=3.5
return Q_xya
def predict(self, dt: float, *args, **kwargs):
# assuming constant deceleration due to friction loss, as an
# input to the system
velocities = self.x[[1, 3]]
friction_input = (
np.sign(apply_deadzone(velocities, self.friction_deadzone)) *
self.friction_decel)
if "u" in kwargs:
kwargs["u"] = friction_input + kwargs["u"]
else:
kwargs["u"] = friction_input
# F, Q, and B depend on dt which is not constant, so calculate
# new F, Q and B matrices based on current dt.
self.Q = Q_discrete_white_noise(dim=4,
dt=dt,
var=self.process_variance)
self.B[1, 0] = dt
self.B[3, 1] = dt
self.F[0, 1] = dt
self.F[2, 3] = dt
# update the timestep
self.timestamp += dt
# Run the actual prediction step
return super().predict(*args, **kwargs)
def __init__(self, x0):
dt = 1.0 # time step
self.kf = KalmanFilter(dim_x=2, dim_z=1)
# Initial state estimate
self.kf.x = np.array([x0, 0])
# Initial Covariance matrix
# An uncertainty must be given for the initial state.
self.kf.P = np.eye(2) * 1000.
# State transition function
self.kf.F = np.array([[1., dt], [0., 1.]])
# Process noise
self.kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1)
# Measurement noise
# This measurement uncertainty indicates
# how much one trusts the measured values of the sensors.
# If the sensor is very accurate, small values should be used here.
# If the sensor is relatively inaccurate, large values should be used here.
self.kf.R = np.array([[.1]])
# Measurement function
# The filter must also be told what is measured
# and how it relates to the state vector
self.kf.H = np.array([[1., 0.]])
def get_linear_tracker():
# KF related
dt = IMSHOW_SLEEP_TIME / 1000 # time step
R_std = 0.35
Q_std = 0.04
M_TO_FT = 1 / 0.3048
tracker = KalmanFilter(dim_x=4, dim_z=2)
tracker.F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0],
[0, 0, 0, 1]])
tracker.H = np.array([[M_TO_FT, 0, 0, 0], [0, M_TO_FT, 0, 0]])
tracker.R = np.eye(2) * R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
tracker.Q[0, 0] = q[0, 0]
tracker.Q[1, 1] = q[0, 0]
tracker.Q[2, 2] = q[1, 1]
tracker.Q[3, 3] = q[1, 1]
tracker.Q[0, 2] = q[0, 1]
tracker.Q[2, 0] = q[0, 1]
tracker.Q[1, 3] = q[0, 1]
tracker.Q[3, 1] = q[0, 1]
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
