This project is part of Udacity’s Self-Driving-Car Nanodegree. The project resources and build instructions can be found here, the required simulator here.
Using sensor fusion to track an object
The goal of this project is to develop an Extended Kalman Filter capable of tracking a moving object near our vehicle. The Kalman Filter will continuously predict the object’s state based on its system dynamics and correct the predicted state based on measurements provided by the vehicle’s lidar and radar sensors.
In this project, the object of interest is a bicycle, represented by the blue car in the video below. The blue markers illustrate the radar measurements, the red ones the lidar measurements, and the green triangles represent the estimates of the bicycle’s state generated by the Kalman filter.
We will divide the implementation of the Kalman Filter into two main parts:
- In the first step, we will define the required system equations, namely the bicycle motion model and the lidar and radar measurement models
- Then, we will implement these models into the process equations of the Extended Kalman Filter
The C++ code of the Kalman Filter is located in the src folder. It can be
run following these instructions.
Dynamic system equations
The bicycle - A linear motion model
| motion model | process noise |
|---|---|
 |
 |
The bicycle is represented by a simple linear motion model with
- a state x that tracks the positions and velocities in x and y directions for the time steps k
- the transition matrix F that propagates the previous state into the new predicted state using the kinematic of the bicycle
- a possible control term represented by the control matrix B and the control vector u, which may cover known external influences such as the acceleration when pushing a cars throttle. However, we will omit this term for the bicycle model as we have no information on how it is accelerating. Instead we will incorporate the acceleration into the process noise
- the process noise v is drawn from a normal distribution with a zero-mean and an uncorrelated process covariance Q. The noise covers unknown external influences such as wind, road bumps, or in our case, the bicycle’s acceleration.
Let’s take a closer look at the parameters of the motion model. We will highlight in red the parts that needed to be integrated into the C++ code of the Kalman filter to pass this project.
Each new state is predicted by the motion model using the previous state and the bicycle kinematics as follows
The acceleration components are extracted and included in the process noise. The noise vector can be divided into the matrix G, which contains no random variables, and the vector a, that contains the unknown accelerations.
The process covariance is defined by
Since the covariance is assumed to be uncorrelated, the correlated terms are set to zero. Performing the matrix multiplication results in
The process covariance is updated at each new time step. Since the accelerations are unknown, longer time steps result in higher uncertainty as the bicycle changes its state in unpredictable ways.
The values for the acceleration variances are provided as input to this project and are defined as follows
This concludes the defintion of the motion model. Let’s now take a look on the second model required for the Kalman filter: the measurement model. Since our car tracks the bicycle with both its lidar and radar sensors, we will define two measurement models.
Lidar - A linear measurement model
| LIDAR measurement model | LIDAR noise |
|---|---|
 |
 |
The measurement model describes how the sensor observes the true state of the bicycle. This observation does not necessarily have to be in the same units or scale as the state. For the lidar, however, this is partially the case since
- the lidar is able to measure the x and y positions of the bicycles state.
- No sensor is 100% precise. The sensor’s uncertainty is represented by the measurement noise w.
- As with the motion model, the noise is drawn from a normal distribution with zero-mean and an uncorrelated covariance R.
- The observation or sensor matrix H maps the bicycle’s state into the measurement space. In other words, the sensor matrix transforms the state into the format in which the sensor observes the true state. In the case of the lidar, this only means that the velocity components of the bicycle state are ommited, since the sensor only measures the bicycle’s location.
The measurement model is linear, since the mapping can be performed by a simple matrix-vector multiplication.
The sensor covariance is usually specified by the manufacturer and is defined as follows in this project
Radar - A nonlinear measurement model
Unlike the lidar model, the radar model is nonlinear. This is due to the fact that it measures the bicycle’s state in polar coordinates.
| Radar measurement model | Radar noise |
|---|---|
 |
 |
The mapping of the cartesian state into the polar sensor space is not possible by a simple linear transformation. Some nonlinear trigonometric transformations are required, which are performed by the observation model h.
A detailed derivation of this function is shown in Mercedes’ EKF references.
Finally the radar covariance is provided as follows
The Kalman filter process
After defining the motion and measurement models, let’s look at the Kalman filter process. It can be divided into the following parts:
- Initialization of the state and its covariance
- State prediction at the current time step using the motion model
- Determination of the Kalman gain and the innovation for the incoming measurement
- Updating the predicted state by incorporating the measurement information
- New cycle for the next time step
1. State initialization
Before the Kalman filter cycle begins, the state x of the bicycle and its covariance P need to be initialized. The first incoming measurement is used for this task. If it is a lidar measurement, it can be used directly to initialize the state’s location. The radar measurement must be converted from polar to cartesian coordinates. In both cases, the velocity is initialized to zero.
| Initialization by lidar measurement | Initialization by radar measurement |
|---|---|
 |
 |
Since we are more certain about the initial position of the bicycle than its velocity, we will initialize the state covariance according to our uncertainty: low variances for the positions and high variances for the velocities. The correlated variances will be initialized to zero.
2. Prediction
| motion model | process noise |
|---|---|
 |
 |
After initialization, a prediction of the new bicycle state is performed using the motion model. The predicted state is defined as the estimate of the true state. Note that we have omitted the control term for our bicycle model. Furthermore, since the process noise has a mean of zero, its estimated value is zero.
The predicted state is also called the a priori state, because the measurement information is not yet included. A priori states are indicated by a superscript minus sign. After the state prediction is updated with the measurement information, we get the a posteriori state, which is indicated by a superscript plus sign. The small hat “^” above the state means that it is an estimated state. The a priori state of the current time step is predicted based on the a posteriori state of the previous time step.
Since the acceleration of the bicycle is unknown, we cannot be 100% sure where the state of the bicycle will be after the time step. This uncertainty is represented by the state covariance P. The a priori state covariance is defined as the estimate of the state estimation error (the difference between the true state and the predicted state):
The increase of uncertainty is realized mathematically by adding the process covariance Q of the current time step. We recall that the process covariance contains the acceleration dependent variances.
3. Innovation and the Kalman gain
After the prediction, a laser or radar measurement may come in. This measurement is used to determine the innovation y. The innovation is defined as the difference between the measurement and the predicted state. It can be interpreted as the part of the measurement that introduces new information about the state. The state must be mapped into the measurement space by the sensor function h introduced above in the system equations section.
| innovation | lidar model | radar model |
|---|---|---|
 |
 |
 |
The second parameter determined in this step is the Kalman gain K. The Kalman gain combines the uncertainty of the state P with the uncertainty of the measurement R. It can be interpreted as a weighting factor that decides how much of the innovation is used to update the predicted state, depending on how much it trusts the measurement or the current state prediction. We will return to this in the next section.
For linear measurement models - as in the case of lidar - the Kalman gain can be computed using the sensor’s observation matrix H to map the state covariance to the measurement space.
However, this does not apply to nonlinear measurement models - as in the case of radar. This is because gaussian distributions represented by a nonlinear function are no longer gaussian. This is shown in the following figure
| linear model | nonlinear model | linearized model |
|---|---|---|
 |
 |
 |
Source: Robot Mapping Extended Kalman Filter
This is where the Extended Kalman Filter (EKF) comes into play. In order to use the nonlinear radar model, it must be linearized. We recall that the radar model is defined as follows
The EKF performs the linearization by a first-order Taylor Expansion using the most recent predicted state as operating point:
The determination of the Jacobian matrices Hj and Mj is necessary for the linearization. The derivation requires some analysis and can again be found in Mercedes’ EKF references.
| Measurement model jacobians |
|---|
 |
 |
The Kalman gain for the radar sensor can then be determined by swapping the linear observation matrix with the jacobian
4. Measurement update
After determining the innovation y and the Kalman gain K, the measurement information can be used to update the predicted a priori state and to determine the a posteriori state.
The a priori state covariance must also be updated. For linear measurement models such as lidar, this can be done using the sensor matrix H as follows
However, for the nonlinear measurement models such as radar, the sensor matrix must be replaced by the jacobian, as in the calculation of the Kalman gain
5. Cycle
After determining the a posteriori state and its covariance, a new cycle is started by converting the current state to the previous state and making a new prediction.
Intuition on the Kalman gain
We mentioned earlier that the Kalman gain can be interpreted as a weighting factor that decides how much of the innovation is used to update the predicted state, depending on how much it trusts the measurement or the current state prediction.
| Kalman gain (lidar) | Update step |
|---|---|
 |
 |
Let’s take a closer look at three edge cases to get an intuition for how the Kalman gain works.
A) Little trust into the measurement
When we have little confidence in the measurement, its covariance R is high and the Kalman gain tends towards
In these cases, the Kalman filter ignores large parts of the innovation. The updated state is close to the predicted state, regardless of the measurement information obtained. information.
B) High trust into the measurement
When we have high confidence in the measurement, its covariance R is small and the Kalman gain tends towards
In these cases the Kalman filter ignores large parts of state prediction. The updated state will be close to the measured state.
C) High trust into the predicted state
When we have high confidence in the state prediction, its covariance P is small and the Kalman gain tends towards
In these cases, the Kalman filter ignores large parts of the innovation. The updated state is close to the predicted state, regardless of the measurement information obtained.
This is not always desirable, since valuable measurement information can be lost in such cases. However, the process noise or process covariance Q helps to ensure that the estimated state covariance P does not become to small.
Note that the Kalman Filter requires the consideration of uncertainty to become more robust.
Project Results
The following videos show the results of applying the Extended Kalman filter on the
two data sets provided by the Term 2 Simulator. In both cases, compared to
the ground truth, the estimated state satisfies the requirement of an RMSE below or equal
to [.11, .11, 0.52, 0.52] after some “warm-up” time.
| Dataset 1 | Dataset 2 |
|---|---|
videos/dataset_1_close.mp4 |
videos/dataset_2_far.mp4 |
 |
 |