def kalman_filter(data,
col_num,
threshold,
Q: float,
EGM: bool,
EGM_window_width=800,
verbose=True):
'''
----------------
DESCRIPTION
----------------
Simple implementation of a Kalman filter based on:
http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
'''
Measurement = effectiv_trans.gradient_filter(data=data,
col_num=col_num,
threshold=threshold,
verbose=verbose)
P = np.diag(np.array(1.0).reshape(-1))
F = np.matrix(1.0)
H = F
R = np.matrix(0.1**2)
Q = Q
G = np.matrix(1.0)
Q = G * (G.T) * Q
Z = np.matrix(Measurement[0])
X = Z
kf = KalmanFilter(X, P, F, Q, Z, H, R)
X_ = [X[0, 0]]
for i in tqdm(range(1, len(Measurement)),
desc="Kalman filter...",
ascii=False,
ncols=75):
# Predict
(X, P) = kf.predict(X, P, w=0)
# Update
(X, P) = kf.update(X, P, Z)
Z = np.matrix(Measurement[i])
X_.append(X[0, 0])
if EGM:
EGM = [X_[0]]
n = EGM_window_width
for i in tqdm(range(1,
len(Measurement) - n + 1),
desc="EGM filter...",
ascii=False,
ncols=80):
EGM.append(np.mean(X_[i:i + n]))
EGM.extend(X_[len(Measurement) - n + 1:])
return EGM
else:
return X_
class FusionEKF:
"""
A class that gets sensor measurements from class DataPoint
and predicts the next state of the system using an extended Kalman filter algorithm
The state variables we are considering in this system are the position and velocity
in x and y cartesian coordinates, in essence there are 4 variables we are tracking.
In particular, an instance of this class gets measurements from both lidar and radar sensors
lidar sensors measure positions in cartesian coordinates (2 values)
radar sensors measure position and velocity in polar coordinates (3 values)
lidar sensor are linear and radar sensors are non-linear, so we use the jacobian algorithm
to compute the state transition matrix H unlike a simple kalman filter.
"""
def __init__(self, d):
self.initialized = False
self.timestamp = 0
self.n = d['number_of_states']
self.P = d['initial_process_matrix']
self.F = d['inital_state_transition_matrix']
self.Q = d['initial_noise_matrix']
self.radar_R = d['radar_covariance_matrix']
self.lidar_R = d['lidar_covariance_matrix']
self.lidar_H = d['lidar_transition_matrix']
self.a = (d['acceleration_noise_x'], d['acceleration_noise_y'])
self.kalmanFilter = KalmanFilter(self.n)
def updateQ(self, dt):
dt2 = dt * dt
dt3 = dt * dt2
dt4 = dt * dt3
x, y = self.a
r11 = dt4 * x / 4
r13 = dt3 * x / 2
r22 = dt4 * y / 4
r24 = dt3 * y / 2
r31 = dt3 * x / 2
r33 = dt2 * x
r42 = dt3 * y / 2
r44 = dt2 * y
Q = np.matrix([[r11, 0, r13, 0], [0, r22, 0, r24], [r31, 0, r33, 0],
[0, r42, 0, r44]])
self.kalmanFilter.setQ(Q)
def update(self, data):
dt = time_difference(self.timestamp, data.get_timestamp())
self.timestamp = data.get_timestamp()
self.kalmanFilter.updateF(dt)
self.updateQ(dt)
self.kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = self.kalmanFilter.getx()
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = self.radar_R
elif data.get_name() == 'lidar':
H = self.lidar_H
Hx = self.lidar_H * x
R = self.lidar_R
self.kalmanFilter.update(z, H, Hx, R)
def start(self, data):
self.timestamp = data.get_timestamp()
x = np.matrix([data.get()]).T
self.kalmanFilter.start(x, self.P, self.F, self.Q)
self.initialized = True
def process(self, data):
if self.initialized:
self.update(data)
else:
self.start(data)
def get(self):
return self.kalmanFilter.getx()
