Kalman filters

The algorithm used to merge the data is called a Kalman filter.

The Kalman filter is one of the most popular algorithms in data fusion. Invented in 1960 by Rudolph Kalman, it is now used in our phones or satellites for navigation and tracking. The most famous use of the filter was during the Apollo 11 mission to send and bring the crew back to the moon.

When to use a Kalman Filter ?

A Kalman filter can be used for data fusion to estimate the state of a dynamic system (evolving with time) in the present (filtering), the past (smoothing) or the future (prediction). Sensors embedded in autonomous vehicles emit measures that are sometimes incomplete and noisy. The inaccuracy of the sensors (noise) is a very important problem and can be handled by the Kalman filters.

A Kalman filter is used to estimate the state of a system, denoted x. This vector is composed of a position p and a velocity v.

State of a system

At each estimate, we associate a measure of uncertainty P.

By performing a fusion of sensors, we take into account different data for the same object. A radar can estimate that a pedestrian is 10 meters away while the Lidar estimates it to be 12 meters. The use of Kalman filters allows you to have a precise idea to decide how many meters really is the pedestrian by eliminating the noise of the two sensors.

A Kalman filter can generate estimates of the state of objects around it. To make an estimate, it only needs the current observations and the previous prediction. The measurement history is not necessary. This tool is therefore light and improves with time.

How it looks like

State and uncertainty are represented by Gaussians.

Gaussian

A Gaussian is a continuous function under which the area is 1. This allows us to represent probabilities. We are on a probability to normal distribution. The uni-modality of the Kalman filters means that we have a single peak each time to estimate the state of the system.

unimodal vs multimodal (source)

We have a mean μ representing a state and a variance σ² representing an uncertainty. The larger the variance, the greater the uncertainty.

Different gaussians

Gaussians make it possible to estimate probabilities around the state and the uncertainty of a system. A Kalman filter is a continuous and uni-modal function.

Bayesian Filtering

In general, a Kalman filter is an implementation of a Bayesian filter, ie a sequence of alternations between prediction and update or correction.

Bayes filter

Prediction: We use the estimated state to predict the current state and uncertainty.

Update: We use the observations of our sensors to correct the predicted state and obtain a more accurate estimate.

To make an estimate, a Kalman filter only needs current observations and the previous prediction. The measurement history is not necessary.

Maths

The mathematics behind the Kalman filters are made of additions and multiplications of matrices. We have two stages: Prediction and Update.

Prediction

Our prediction consists of estimating a state x’ and an uncertainty P’ at time t from the previous states x and P at time t-1.

Prediction formulas

F: Transition matrix from t-1 to t

ν: Noise added

Q: Covariance matrix including noise

Update

The update phase consists of using a z measurement from a sensor to correct our prediction and thus predict x and P.

Update formulas

y: Difference between actual measurement and prediction, ie the error.

S: Estimated system error

H: Matrix of transition between the marker of the sensor and ours.

R: Covariance matrix related to sensor noise (given by the sensor manufacturer).

K: Kalman gain. Coefficient between 0 and 1 reflecting the need to correct our prediction.

The update phase makes it possible to estimate an x and a P closer to reality than what the measurements provide.

A Kalman filter allows predictions in real time, without data beforehand. We use a mathematical model based on the multiplication of matrices for each time defining a state x (position, speed) and uncertainty P.

Représentation Prior/Posterior

This diagram shows what happens in a Kalman filter.

Kalman filter estimation (source)

Predicted state estimate represents our first estimate, our prediction phase. We are talking about prior.

represents our first estimate, our prediction phase. We are talking about prior. Measurement is the measurement from one of our sensors. We have better uncertainty but the noise of the sensors makes it a measurement that is always difficult to estimate. We talk about likelihood.

is the measurement from one of our sensors. We have better uncertainty but the noise of the sensors makes it a measurement that is always difficult to estimate. We talk about likelihood. Optimal State Estimate is our update phase. The uncertainty is this time the weakest, we accumulated information and allowed to generate a value more sure than with our sensor alone. This value is our best guess. We speak of posterior.

What a Kalman filter implements is actually a Bayes rule.

Bayes rule

In a Kalman filter, we loop predictions from measurements. Our predictions are always more precise since we keep a measure of uncertainty and regularly calculate the error between our prediction and reality. We are able from matrix multiplications and probability formulas to estimate velocities and positions of vehicles around us.

“Extended/Unscented” filters and non-linearity

An essential problem arises. Our mathematical formulas are all implemented with linear functions of type y = ax + b.

A Kalman filter always works with linear functions. On the other hand, when we use a Radar, the data is not linear.

Functionment of a Radar

The Radar sees the world with three measures:

ρ (rho) : The distance to the object that is tracked.

: The distance to the object that is tracked. φ (phi) : The angle between the x-axis and our object.

: The angle between the x-axis and our object. ρ ̇ (rhodot): The change of ρ, resulting in a radial velocity.

These three values make our measurement a nonlinear given the inclusion of the angle φ.

Our goal here is to convert the data ρ, φ, ρ ̇ to Cartesian data (px, py, vx, vy).

If we enter non-linear data in a Kalman filter, our result is no longer in uni-modal Gaussian form and we can no longer estimate position and velocity.

Gaussian vs non-linearity

So we use approximations, which is why we are working on two methods:

- Extended Kalman filters use Jacobian and Taylor series to linearize the model.

- Unscented Kalman filters use a more precise approximation to linearize the model.