This project implements the Error-State Extended Kalman Filter (ES-EKF) to localize a vehicle using data from the CARLA simulator. The following diagram shows a graphical representation of the system.
This project is the final programming assignment of the State Estimation and Localization for Self-Driving Cars course from Coursera. The starter code is provided by the University of Toronto.
The Kalman Filter algorithm updates a state estimate through two stages:
- prediction using the motion model
- correction using the measurement model
The vehicle state at each time step consists of position, velocity, orientation (parameterized by a unit quaternion), imu acceleration bias, imu angular velocity bias; the inputs to the motion model are the IMU specific force and angular rate measurements.
| Description | Equation |
|---|---|
| Vehicle State | |
| IMU Measurements |
This section of code initializes the variables for the ES-EKF solver.
p_est = np.zeros([imu_f.data.shape[0], 3]) # position estimates
v_est = np.zeros([imu_f.data.shape[0], 3]) # velocity estimates
q_est = np.zeros([imu_f.data.shape[0], 4]) # orientation estimates as quaternions
ab_est = np.zeros([imu_f.data.shape[0], 3]) # acc. bias estimates of imu acceleration
wb_est = np.zeros([imu_f.data.shape[0], 3]) # ang. vel. bias estimates of imu angular vel.
p_cov = np.zeros([imu_f.data.shape[0], 15, 15]) # covariance matrices at each timestep
# Set initial values.
p_est[0] = gt.p[0]
v_est[0] = gt.v[0]
q_est[0] = Quaternion(euler=gt.r[0]).to_numpy()
ab_est[0] = np.array([0.0, 0.0, 0.0]).reshape(1, 3)
wb_est[0] = np.array([0.0, 0.0, 0.0]).reshape(1, 3)
p_cov[0] = np.zeros(15) # covariance of estimate
gnss_i = 0
lidar_i = 0
gnss_t = list(gnss.t)
lidar_t = list(lidar.t)The main filter loop operates by first predicting the next state (vehicle pose, velocity, imu acceleration bias and imu angular velocity bias). The predicted vehicle state integrates the high-rate IMU measurements by using a nonlinear motion model.
The motion model of the vehicle is given by the following set of equations
| Description | Equation |
|---|---|
| Position | |
| Velocity | |
| Orientation | |
| Acceleration Bias | |
| Angular velocity Bias |
The predicted vehicle state is therefore given by the equations
| Description | Equation | Variable |
|---|---|---|
| Predicted State | x_check |
|
| Predicted Position | p_check |
|
| Predicted Velocity | v_check |
|
| Predicted Orientation | q_check |
|
| Predicted Acceleration Bias | ab_check |
|
| Predicted Angular velocity Bias | wb_check |
This section of code iterates through the IMU inputs and updates the state.
for k in range(1, imu_f.data.shape[0]): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
q_prev = Quaternion(*q_est[k - 1, :]) # previous orientation as a quaternion object
ab_prev = ab_est[k - 1, :] # previous acc. bias
wb_prev = wb_est[k - 1, :] # previous angular vel. bias
q_curr = Quaternion(axis_angle=((imu_w.data[k - 1] - wb_prev)*delta_t)) # current IMU orientation
c_ns = q_prev.to_mat() # previous orientation as a matrix
f_ns = (c_ns @ (imu_f.data[k - 1] - ab_prev)) + g # calculate sum of forces
p_check = p_est[k - 1, :] + delta_t*v_est[k - 1, :] + 0.5*(delta_t**2)*f_ns
v_check = v_est[k - 1, :] + delta_t*f_ns
q_check = q_prev.quat_mult_left(q_curr) # q_prev * q_curr
ab_check = ab_prev
wb_check = wb_prevIn the previous section, we propagated the nominal state (x_check) forward in time. However, this prediction ignores noise and perturbations. The ES-EKF algorithm captures these deviations in the error state vector.
The next step is to consider the linearized error dynamics of the system.
| Description | Equation |
|---|---|
| Error State | |
| Error Dynamics | |
| Measurement Noise |
The Jacobians and the noise covariance matrix are defined as follows
| Description | Equation | Variable |
|---|---|---|
| Motion Model Jacobian | f_jac |
|
| Motion Model Noise Jacobian | Fi_jac |
|
| IMU Noise Covariance | q_cov |
where I is the 3 by 3 identity matrix.
This section of code calculates the motion model Jacobian
# 1.1 Linearize the motion model and compute Jacobians
f_jac = np.eye(15) # motion model jacobian with respect to last state
f_jac[0:3, 3:6] = np.eye(3)*delta_t
f_jac[3:6, 6:9] = - c_ns @ skew_symmetric(imu_f.data[k - 1] - ab_prev)*delta_t
f_jac[3:6, 9:12] = - c_ns*delta_t
f_jac[6:9, 6:9] = np.eye(3) - skew_symmetric(imu_w.data[k - 1] - wb_prev)*delta_t
f_jac[6:9, 12:15] = - np.eye(3)*delta_tIn the previous section, we computed the Jacobian matrices of the motion model. We will use these matrices to propagate the state uncertainty forward in time.
The uncertainty in the state is captured by the state covariance (uncertainty) matrix. The uncertainty grows over time until a measurement arrives.
| Description | Equation | Variable |
|---|---|---|
| Predicted State Covariance | p_cov_check |
This section of code calculates the state uncertainty
# 2. Propagate uncertainty
q_cov = np.zeros((12, 12)) # IMU noise covariance
q_cov[0:3, 0:3] = delta_t**2 * np.eye(3)*var_imu_f
q_cov[3:6, 3:6] = delta_t**2 * np.eye(3)*var_imu_w
q_cov[6:9, 6:9] = delta_t * np.eye(3)*var_imu_f_bias
q_cov[9:, 9:] = delta_t * np.eye(3)*var_imu_w_bias
p_cov_check = f_jac @ p_cov[k - 1, :, :] @ f_jac.T + Fi_jac @ q_cov @ Fi_jac.TThe IMU data arrives at a faster rate than either GNSS or LIDAR sensor measurements.
The algorithm checks the measurement availability and calls a function to correct our prediction.
# 3. Check availability of GNSS and LIDAR measurements
if imu_f.t[k] in gnss_t:
gnss_i = gnss_t.index(imu_f.t[k])
# 3.1 compute the observation jacobian
X_dq[6:10, 6:9] = Q_del_theta(Quaternion(*q_check))
H_jac = H_x @ X_dq
p_check, v_check, q_check, ab_check, wb_check, p_cov_check = \
measurement_update(var_gnss, p_cov_check, gnss.data[gnss_i],
p_check, v_check, q_check, ab_check, wb_check, H_jac)
if imu_f.t[k] in lidar_t:
lidar_i = lidar_t.index(imu_f.t[k])
# 3.1 compute the observation jacobian
X_dq[6:10, 6:9] = Q_del_theta(Quaternion(*q_check))
H_jac = H_x @ X_dq
p_check, v_check, q_check, ab_check, wb_check, p_cov_check = \
measurement_update(var_lidar, p_cov_check, lidar.data[lidar_i],
p_check, v_check, q_check, ab_check, wb_check, H_jac)The measurement model is the same for both sensors. However, they have different noise covariance.
| Description | Equation |
|---|---|
| Measurement Model | |
| GNSS Measurement Noise | |
| LIDAR Measurement Noise |
Measurements are processed sequentially by the EKF as they arrive; in our case, both the GNSS receiver and the LIDAR provide position updates. Measurement Model Jacobian is obtained by chain rule as a product of Jacobian of h() with respect x and Jacobian of state with respect error state
| Description | Equation | Variable |
|---|---|---|
| Measurement Model Jacobian | H_jac |
|
| Jacobian of h() with respect x | H_x |
|
| Jacobian of x with respect |
X_dq |
|
| Q_del_theta | Q_del_theta |
|
| Sensor Noise Covariance | r_cov |
|
| Kalman Gain | k_gain |
This section of code defines the measurement update function
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check, ab_check, wb_check, H_jac):
# 3.1 Compute Kalman Gain
r_cov = np.eye(3)*sensor_var
k_gain = p_cov_check @ H_jac.T @ np.linalg.inv((H_jac @ p_cov_check @ H_jac.T) + r_cov)We use the Kalman gain to compute the error state. Considering the innovation or difference between our predicted vehicle position, p_check, and the measured position, y_k.
| Description | Equation | Variable |
|---|---|---|
| Error State | error_state |
The error state is then used to update the nominal state vector. We also calculate the corrected nominal state covariance matrix, p_cov_hat.
| Description | Equation | Variable |
|---|---|---|
| Corrected Position | p_hat |
|
| Corrected Velocity | v_hat |
|
| Corrected Orientation | q_hat |
|
| Corrected Acceleration bias | ab_hat |
|
| Corrected Angular velocity bias | wb_hat |
|
| Corrected State Covariance | p_cov_hat |
Finally, the function returns the corrected state and state covariants.
# 3.2 Compute error state
error_state = k_gain @ (y_k - p_check)
# 3.3 Correct predicted state
p_hat = p_check + error_state[0:3]
v_hat = v_check + error_state[3:6]
# q_hat = Quaternion(axis_angle=error_state[6:9]).quat_mult_left(Quaternion(*q_check))
q_hat = Quaternion(*q_check).quat_mult_left(Quaternion(axis_angle=error_state[6:9]))
ab_hat = ab_check + error_state[9:12]
wb_hat = wb_check + error_state[12:15]
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(15) - k_gain @ H_jac) @ p_cov_check
return p_hat, v_hat, q_hat, ab_hat, wb_hat, p_cov_hatWe evaluate the algorithm performance by comparing the estimated vehicle trajectory and the ground truth. The source code generates visualizations by running the following command in the terminal.
python es_ekf.pyThis figure shows a comparison between the trajectory estimate and the ground truth.
This figure shows our estimator error relative to the ground truth. The dashed red lines represent three standard deviations from the ground truth, according to our estimator. These lines indicate how well our model fits the actual dynamics of the vehicle and how well the estimator is performing overall. The estimation error should remain within the three-sigma bounds at all times.
