Introduction
Self-Localization technology is very essential for autonomous driving system to know the vehicle's position and pose. In this entry, I'm introducing a summary of Extended Kalman Filter(EKF) which is commonly used for self-localization algorithm and the Python sample program.
Table of contents
- Introduction
- Table of contents
- Source code at GitHub
- Basis of Kalman filter
- Why EKF?
- Implementation of Python program
- Simulation result
Source code at GitHub
All source codes are located at the following GitHub repository.
Extended Kalman Filter localization module
github.com
Self-Localization simulation
github.com
Basis of Kalman filter
Kalman filter is one of the optimization estimation algorithm and it is mainly used for the following purposes.
- Estimating a state which can not be observed directly based on a sensor's observation and a system model.
- Estimating the plausible state by fusing multiple observations.
I published this entry to introduce a simple sample program of 1D position and velocity estimation by Kalman filter. If you want to study the basis of Kalman filter, please refer to this entry.
www.eureka-moments-blog.com
Why EKF?
Basically, Kalman filter can be used only for a linear system, but almost system in the world is nonlinear. So, when we want to estimate a vehicle's state by Kalman filter, we need to linearize the system's state equation and observation equation. Kalman filter with the system's linearization is called Extended Kalman Filter(EKF). If you want to understand EKF in detail, please refer to this entry.
en.wikipedia.org
Implementation of Python program
From here, I'm explaining about implementing Python program of self localization by EKF.
1. Initialization
First, an initialization method of EKF localization module class is implemented as follow. This module can estimate a vehicle's state including position(x, y), yaw angle and velocity. So, a state vector is defined as 4 x 1 vector. And then, an error's covariance matrix can also be estimated. So, the matrix variable is defined as 4 x 4 identity matrix, "cov_mat". INPUT_NOISE_VAR_MAT is a variance matrix of acceleration input and yaw rate input for state equation. This is used for generating random noise of those input data. OBSRV_NOISE_VAR_MAT is a variance matrix of position(x, y) which is observed with GNSS. This is used for generating random noise of the observation data. JACOB_H is a Jacobian of observation equation. This is used for linearizing the observation equation. DRAW_COLOR is a color type to plot estimation result.
class ExtendedKalmanFilterLocalizer: """ Self localization by Extended Kalman Filter class """ def __init__(self, accel_noise=0.2, yaw_rate_noise=10.0, obsrv_x_noise=1.0, obsrv_y_noise=1.0, color='r'): """ Constructor color: Color of drawing error covariance ellipse """ self.state = np.zeros((4, 1)) self.cov_mat = np.eye(4) self.INPUT_NOISE_VAR_MAT = np.diag([accel_noise, np.deg2rad(yaw_rate_noise)]) ** 2 self.OBSRV_NOISE_VAR_MAT = np.diag([obsrv_x_noise, obsrv_y_noise]) ** 2 self.JACOB_H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) self.DRAW_COLOR = color
2. State module class
Next, an state module class is implemented as follow. This is used for updating and managing a vehicle's state including position(x, y), yaw angle and velocity. When this class is initialized, 4 initial state parameters, x, y, yaw and velocity are passed to the initialization method. An argument, color is to use set the color to plot computed position(x, y). The maximum/minimum values and stop value to limit the computed velocity are also defined as member constant values.
class State: """ Vehicle state(x, y, yaw, speed) data and logic class """ def __init__(self, x_m=0.0, y_m=0.0, yaw_rad=0.0, speed_mps=0.0, color='k'): """ Constructor x_m: Vehicle's position, x[m] y_m: Vehicle's position, y[m] yaw_rad: Vehicle's yaw angle[rad] speed_mps: Vehicle's speed[m/s] """ self.STOP_SPEED_MPS = 0.5 / 3.6 # 0.5[km/h] self.MAX_SPEED_MPS = 60 / 3.6 # 60[km/h] self.MIN_SPEED_MPS = -10 / 3.6 # -10[km/h] self.DRAW_COLOR = color self.x_m = x_m self.y_m = y_m self.yaw_rad = yaw_rad self.speed_mps = speed_mps self.x_history = [self.x_m] self.y_history = [self.y_m]
A state equation to compute state is defined as follow. This is defined based on constant acceleration linear motion model and output a new vehicle's state at the next time step. In the case of Kalman filter, this method is used for predicting the vehicle's state at the next time step.
@staticmethod def motion_model(state, input, time_s): """ Static function of motion model of vehicle state state: Vehicle's state (x, y, yaw, speed) object input: Motion input (acceleration, yaw rate) object time_s: Time interval per cycle[sec] """ # to fix DeprecationWarning: Conversion of an array with ndim > 0 # to a scalar is deprecated, and will error in future. # Ensure you extract a single element from your array # before performing this operation. (Deprecated NumPy 1.25.) yaw_rad = state.item(2) # do not extract an element like state[2] A = np.array([[1, 0, 0, cos(yaw_rad) * time_s], [0, 1, 0, sin(yaw_rad) * time_s], [0, 0, 1, 0], [0, 0, 0, 1]]) B = np.array([[(cos(yaw_rad) * time_s**2) / 2, 0], [(sin(yaw_rad) * time_s**2) / 2, 0], [0, time_s], [time_s, 0]]) return A @ state + B @ input
3. Observation model
Then, an observation equation is defined as follow. This is used for generating GNSS observation data and predicting an observation at the next time step based on the predicted vehicle's state.
def _observation_model(self, state): """ Private function of observation model state: Predicted state Return Predicted observation data """ H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) x = np.array([[state[0, 0]], [state[1, 0]], [state[2, 0]], [state[3, 0]]]) return H @ x
4. Jacobian
Then, the state equation's Jacobian F and G are defined here. These Jacobians are used to linearize those equation and predict an error covariance matrix at the next time step.
Jacobian F
def _jacobian_F(self, state, input, time_s): """ Private function to calculate jacobian F state: Predicted state input: Input vector[accel, yaw rate] from controller time_s: Simulation interval time[sec] Return: Jacobian F """ yaw = state[2, 0] spd = state[3, 0] acl = input[0, 0] t = time_s sin_yaw = sin(yaw) cos_yaw = cos(yaw) jF = np.array([[1, 0, -spd*sin_yaw*t-acl*sin_yaw*t**2/2, cos_yaw*t], [0, 1, spd*cos_yaw*t+acl*cos_yaw*t**2/2, sin_yaw*t], [0, 0, 1, 0], [0, 0, 0, 1]]) return jF
Jacobian G
def _jacobian_G(self, state, time_s): """ Private function to calculate jacobian G state: Predicted state time_s: Simulation interval time[sec] Return: Jacobian G """ yaw = state[2, 0] t = time_s jG = np.array([[cos(yaw) * t**2 / 2, 0], [sin(yaw) * t**2 / 2, 0], [0, t], [t, 0]]) return jG
5. Predict and Update
Then, Kalman filter's main process to predict and update the vehicle's state is implemented here. The process in this method is separated into the following steps.
- Generate acceleration and yaw rate inputs with random gaussian noise
- Predict the vehicle's state and error covariance at the next time step based on motion model and jacobian F, G
- Predict the GNSS's observation data and error covariance at the next time step based on observation model and jacobian H
- Compute Kalman gain to correct the vehicle's state
- Update the vehicle's state and error covariance at the next time step based on the computed Kalman gain
def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss): """ Function to update data state: Last extimated state data accel_mps2: Acceleration input from controller yaw_rate_rps: Yaw rate input from controller time_s: Simulation interval time[sec] Return: Estimated state data """ last_state = np.array([[state.get_x_m()], [state.get_y_m()], [state.get_yaw_rad()], [state.get_speed_mps()]]) # input with noise input_org = np.array([[accel_mps2], [yaw_rate_rps]]) input_noise = self.INPUT_NOISE_VAR_MAT @ np.random.randn(2, 1) next_input = input_org + input_noise # predict state pred_state = State.motion_model(last_state, next_input, time_s) jF = self._jacobian_F(pred_state, next_input, time_s) jG = self._jacobian_G(pred_state, time_s) Q = self.INPUT_NOISE_VAR_MAT last_cov = self.cov_mat pred_cov = jF @ last_cov @jF.T + jG @ Q @ jG.T # predict observation pred_obsrv = self._observation_model(pred_state) jH = self.JACOB_H R = self.OBSRV_NOISE_VAR_MAT pred_obsrv_cov = jH @ pred_cov @ jH.T + R # kalman gain k = pred_cov @ jH.T @ np.linalg.inv(pred_obsrv_cov) # update inov = gnss - pred_obsrv est_state = pred_state + k @ inov est_cov = pred_cov - k @ pred_obsrv_cov @ k.T self.state = est_state self.cov_mat = est_cov return est_state
6. Error covariance elipse
And finally, a method to draw the estimated error covariance as an elipse is implemented here. This is to represent the shape of error covariance visually. The covariance is approximated to Gaussian distribution.
def draw(self, axes, elems, pose): """ Function to draw error covariance ellipse axes: Axes objects of figure elems: List of plot object pose: Vehicle's pose[x, y, yaw] """ eig_val, eig_vec = np.linalg.eig(self.cov_mat) if eig_val[0] >= eig_val[1]: big_idx, small_idx = 0, 1 else: big_idx, small_idx = 1, 0 a, b = sqrt(3.0 * eig_val[big_idx]), sqrt(3.0 * eig_val[small_idx]) angle = atan2(eig_vec[1, big_idx], eig_vec[0, big_idx]) t = np.arange(0, 2 * pi + 0.1, 0.1) xs = [a * cos(it) for it in t] ys = [b * sin(it) for it in t] xys = np.array([xs, ys]) xys_array = XYArray(xys) transformed_xys = xys_array.homogeneous_transformation(pose[0, 0], pose[1, 0], angle) elip_plot, = axes.plot(transformed_xys.get_x_data(), transformed_xys.get_y_data(), color=self.DRAW_COLOR) elems.append(elip_plot)
Simulation result
A simulation with the above implemented program is executed as follow. You can see that the vehicle's state position(x, y) is estimated accurately and the error covariance is minimized while moving.
