class EKF:
def __init__(self):
self.is_initialized = False
self.previous_timestamp = 0
# Initialize measurement covariance matrix - laser
self.R_laser = np.array([[0.0225, 0.0], [0.0, 0.0225]],
dtype=np.float32)
# Initialize measurement covariance matrix - radar
self.R_radar = np.array(
[[0.09, 0.0, 0.0], [0.0, 0.0009, 0.0], [0.0, 0.0, 0.09]],
dtype=np.float32)
# Initialize state variable
x_init = np.zeros(4, dtype=np.float32)
# Initialize state covariance matrix
P_init = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# System model - dt not yet taken into account
F_init = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# Transformation from state variable to measurement
H_init = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float32)
# Covariance matrix of process noise - dt not yet taken into account
Q_init = np.array([
[1, 0, 1, 0],
[0, 1, 0, 1],
[1, 0, 1, 0],
[0, 1, 0, 1],
],
dtype=np.float32)
# Initialize our Kalman filter
self.ekf = KalmanFilter(x_init, P_init, F_init, H_init, self.R_laser,
Q_init)
# Set the process noise constants
self.noise_ax = 9.0
self.noise_ay = 9.0
def process_measurement(self, m):
if not self.is_initialized:
# First measurement
if m['sensor_type'] == 'R':
# Convert radar data in polar to cartesian coordinates
# (Note that rho_dot from the very first measurement is
# not used))
px = m['rho'] * np.cos(m['phi'])
py = m['rho'] * np.sin(m['phi'])
self.ekf.x = np.array([px, py, 0.0, 0.0])
self.previous_timestamp = m['timestamp']
elif m['sensor_type'] == 'L':
# Initialize state variable
px, py = m['x'], m['y']
self.ekf.x = np.array([px, py, 0, 0])
self.previous_timestamp = m['timestamp']
self.is_initialized = True
return
# Prediction
# Update state transition matrix (time measured in seconds)
dt = (m['timestamp'] - self.previous_timestamp) / 1000000.0
self.previous_timestamp = m['timestamp']
self.ekf.F[0][2] = dt
self.ekf.F[1][3] = dt
# Set the process noise covariance
dt2 = dt * dt
dt3 = dt2 * dt
dt4 = dt3 * dt
self.ekf.Q = np.array(
[[dt4 * self.noise_ax / 4.0, 0.0, dt3 * self.noise_ax / 2.0, 0.0],
[0.0, dt4 * self.noise_ay / 4.0, 0.0, dt3 * self.noise_ay / 2.0],
[dt3 * self.noise_ax / 2.0, 0.0, dt2 * self.noise_ax, 0.0],
[0.0, dt3 * self.noise_ay / 2.0, 0.0, dt2 * self.noise_ay]],
dtype=np.float32)
self.ekf.predict()
# Measurement update
if m['sensor_type'] == 'R':
# Radar updates
z = np.array([m['rho'], m['phi'], m['rho_dot']], dtype=np.float32)
self.ekf.R = self.R_radar
self.ekf.update_ekf(z)
elif m['sensor_type'] == 'L':
# Laser updates
z = np.array([m['x'], m['y']], dtype=np.float32)
self.ekf.R = self.R_laser
self.ekf.update(z)
