Taking the Kalman Filter Further: Extended Kalman Filter for Self-Driving Cars

After the previous blog on Kalman filter where I demonstrated a simple program to grasp the concept, I thought of advancing on the topic and write a code that considers a practical scenario. So I went on to take the scenario where Kalman Filter serves as one of the most fundamental and crucial programs: Self-Driving Cars.

[embed]The Simplest Kalman Filter You’ll Ever See (And It’s in C++) If you’ve worked with sensors, robotics, or anything that moves and needs to know its position, someone has thrown the…cmodi306.medium.com

To the ones who think Kalman filter might be some kind of coffee filter or high-frequency filter in electronics, this has nothing to do with either of them.

A better way to think about it: consider the equations of motion that can be used to determine the position of a vehicle given its speed, steering angle, and heading. When determining the coordinates of a self-driving car, such a motion model calculates the current position step by step over time. Another way to determine the vehicle’s location is to use a GNSS-RTK sensor, which provides highly accurate and precise position measurements. So one might wonder — should I rely on the motion model or the sensor?

The catch is, if only the motion model is used, uncertainty grows over time as minor error in one variable can compound over time, leading to drastic difference between the calculated and real position. On the other hand, sensors, no matter how accurate and precise, have some level of noise. Hence, the relying only on sensors can also lead to incorrect estimations.

At this point, Kalman filter says, why not use both! It will take in the prediction (motion equation) and the sensor measurement and provide an estimation. It will lay more weight on the side with lower uncertainty. Does the topic sound interesting? Know more about the fundamentals of Kalman filter here.

Why Extended?

Because the real world does not like linearity. The standard Kalman filter works beautifully — but only for linear systems. In reality, a car’s motion, however, involves trigonometric functions of its heading angle (used to calculate x and y coordinates for instance), making the model nonlinear. Now, in mathematics, whenever we see a non-linear problem, you know what we like to do everytime, CALCULATING THE DERIVATIVES! This linearizes the function at that particular point.

The Extended Kalman Filter also handles the non-linearity by linearizing the system at every time step using a Jacobian matrix — just a fancy word for a matrix that contains partial derivatives of the nonlinear function at the current state.

So to grasp the concept, we are going to:

Understand Extended Kalman Filter with a realistic example

This is a hands-on exercise to implement an EKF for a moving car. We are given noisy GPS readings and must estimate the car’s true state over time.

Before proceeding, I would advise cloning the code first from Github and have it open parallel to this blog so you understand what’s going on:

git clone https://github.com/cmodi306/kalman-filter.git

Among the files, the one you need for this blog is the extended_kalman_filter.hpp and extended_kalman_filter.cpp in include and src folders respectively.

The State Vector and Inputs

The vehicle’s state is tracked as four variables:

x = [pos_x, pos_y, heading, velocity]ᵀ

Note the order of the attributes in x. X-coordinate is placed first, then the Y-coordinate, followed by the heading angle and ultimately comes the velocity. The order is important as it will be useful later when we update the values.

Alongside the state, the filter takes a control input u(steering angle and acceleration in this case):

u = [steering_angle, acceleration]

Now that we have the basic entities, we start with the first step, which is:

Step 1: Initialization

First, we define a function initialize_variables() that initializes the required variables. Some variables are self-explanatory but let's look at the matrices defined and understand what each matrix means:

• Q (process noise covariance) — how much we confidence we put in our own motion model. Small diagonal values (0.01 for position, 0.001 for heading, 0.1 for velocity) reflect reasonable confidence in the kinematic model.
• P (state covariance) — initial uncertainty about where the vehicle is. Large values say “we’re quite unsure at the start.”
• R (measurement noise covariance) — how noisy the GNSS sensor is. Values of 0.5 on both x and y represent moderate sensor noise. Some industrial level GNSS sensors like ubox do provide covariance values in sensor output.
• H (measurement Jacobian) — maps the state space to measurement space. Since the GNSS directly gives us (x, y), H is simply a 2×4 matrix that picks out the first two state variables.

The initial state x_hat = [0, 0, 0, 2] is basically the initial known value of the vehicle state (which in this case is just an assumption). It places the car at the origin, heading north, moving at 2 m/s.

Now that we have initialized the necessary variables, we move on to step 2 which is:

Step 2: Prediction

The motion_model() function is the part where Newton’s equation of motion come into play, along with some trigonometry. The function determines the current state of the vehicle by using the values from previous state and the delta time which is assumed to be 0.1 s in this example.

x_pred(0) = pos_x + velocity * std::cos(heading) * dt;
x_pred(1) = pos_y + velocity * std::sin(heading) * dt;
x_pred(2) = heading + (velocity/wheelbase) * std::tan(steering_angle) * dt;
x_pred(3) = velocity  + acceleration * dt;

Context for those wondering about these equations — this is known as the bicycle kinematic model, a standard abstraction used in vehicle development.

As you might observed in the equations, the cosand sinfunctions bring non-linearity to the system. This nonlinearity is why we need the EKF.

To make the functions linear, we calculate the Jacobian F = ∂f/∂x in compute_F():

F = | 1  0  -v·sin(θ)·dt   cos(θ)·dt           |
    | 0  1   v·cos(θ)·dt   sin(θ)·dt           |
    | 0  0   1             tan(δ)·dt/wheelbase |
    | 0  0   0             1                   |

The predicted covariance is then: P_pred = F · P · Fᵀ + Q

Now that we have the prediction step, we move on to:

Step 3: Updating with GNSS Measurement

When a GNSS measurement z = [measured_x, measured_y] arrives, the filter computes the innovation — the difference between what was measured and what was predicted. In the example, we already have a list of measurements:

Eigen::Vector2f z(measurements[i][0], measurements[i][1]);
Eigen::Vector2f z_pred(x_check[0], x_check[1]);
Eigen::Vector2f y = z - z_pred;

The Kalman Gain K determines how much to trust the measurement versus the prediction:

S = H · P_pred · Hᵀ + R
K = P_pred · Hᵀ · S⁻¹

A high R (noisy sensor) leads to lower value of K, trusting the prediction more. Inversely, a low R (precise sensor) increases the value of K, thereby trusting the measurement more. The state and covariance are then corrected:

x_updated = x_pred + K · y
P_updated  = (I - K · H) · P_pred

The updated state is fed back for the next iteration and the estimation slowly converges towards the ground truth. You’ll notice the value of K and diagonal values of P shrinking over time. This is because the filter is getting more confident as it fuses measurements with predictions.

The figure below shows the comparison between deviation of noisy measurements and EKF estimations from the ground truth values.

Running It Yourself

Clone the repository, build with CMake:

git clone https://github.com/cmodi306/kalman-filter.git
cd kalman-filter
mkdir build && cd build
cmake .. && make
./extended_kalman_filter

Once you understand the code, try tweaking the noise matrices. Double the values in R to simulate a worse GNSS. Alternatively, set Q entries higher to say “I don’t trust my motion model”. The filter will trust the measurements more aggressively. This tuning intuition is exactly what engineers spend time on in real autonomous vehicle development.

Summary

Kalman Filter combines the predictions and the sensor measurements and considers uncertainties in both the cases, and gives the best estimate of the vehicle’s state. When non-linearity is involved, standard kalman filter may underperform and that is where extended kalman filter comes in. This blog demonstrated how to implement the latter and estimate the current state.

One last thing for you to know is more views on my content shows me that people enjoy my blogs and can take something valuable. This gives me the motivation to write more articles. So if you enjoyed this one, as a sign of appreciation, do subscribe and follow me:

Send me a coffee, maybe? | Medium | Substack | Bluesky