Lidar and Radar Fusion with Kalman Filters in C++

We will build Extended Kalman Filter: extended means that it is capable of handling more complex motion and measurement models.

Overview of the Kalman Filter Algorithm Map

For your reference: a map of the Kalman Filter algorithm! Keep an eye out, because we'll add a little bit more detail to this later.

Imagine you are in a car equipped with sensors on the outside. The car sensors can detect objects moving around: for example, the sensors might detect a pedestrian, as described in the video, or even a bicycle. For variety, let's step through the Kalman Filter algorithm using the bicycle example.

The Kalman Filter algorithm will go through the following steps:

→ The 2 sensors (Lidar and Radar) information are used to estimate the state of a moving pedestrian. This state is represented by 2D positions and 2D velocity. Each time we receive a new measurement from a given sensor, the estimation function is triggered.

→ In prediction step, we predict the pedestrian state and its covariance. We do so by taking into account the elapsed time between the current and previous observations.

→ The measurement update step depends on the sensor type. If the current measurements are generated by a laser sensor, then we just apply standard Kalman Filter to update the pedestrian state. However, Radar measurements involve a non-linear measurement function, so when we receive radar measurement we use different tweaks to handle the measurement update (for instance we use Extended Kalman Filter).

The Kalman Filter is a two step estimation problem:

Two Sensors Observe the Same Pedestrian, How does that Change the Kalman Filter?

→ We can keep the same processing flow, with the difference that each sensor is going to have its own prediction update scheme.

Note: Prediction for radar is exactly the same as in the laser case, what changes in this case is the measurement update step. As Radar sees the world in polar coordinate system and Lidar sees the world in Cartesian coordinate system. So we have different measurement update functions.

As you saw, the Kalman filter is a two-step process: predict, and then update. If you receive two measurements simultaneously, you can use this process with either measurement and then repeat the process with the other measurement. The order does not matter!

Because we have already run a prediction-update iteration with the first sensor at time k+3, the output of the second prediction at time k+3 will actually be identical to the output from the update step with the first sensor. So, in theory, you could skip the second prediction step and just run a prediction, update, update iteration.

Kalman Filter Intuition

State Transition Function: models how the state is changed from t=k-1 to t=k

Measurement Function: model show the measurement is calculated and how it is related to the predicted state x.

Stochastic: represents the random noise that affect the prediction and measurement update steps.

In Prediction Step: We predict our state x and covariance P.

In Measurement Step (Correction Step): We use the latest measurement to update the state estimate and its uncertainty.

The Kalman equation contains many variables, so here is a high level overview to get some intuition about what the Kalman filter is doing.

Prediction:

Let's say we know an object's current position and velocity, which we keep in the x variable. Now one second has passed. We can predict where the object will be one second later because we knew the object position and velocity one second ago; we'll just assume the object kept going at the same velocity.

A Note About the State Transition Function: Bu

Kalman Filter Equations in C++