Extended Kalman Filter
This is about an interesting project I did as a part of Udacity Infosys Connect Self Driving Car Nanodegree program. Currently, I am in term 2 of the program which includes Sensor Fusion(LIDAR & RADAR),Localization and Controllers(PID & MPC). All the projects in this term will be in C++. So lets get our hands dirty in C++. In Extended Kalman Filter(EKF) project, we are provided simulated lidar and radar measurements detecting a bicycle that travels around your vehicle. You will use a Kalman filter, lidar measurements and radar measurements to track the bicycle’s position and velocity.
What is Kalman Filter ?
It’s an iterative estimation algorithm that uses a series of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates of unknown variables that tend to be more accurate than those based on a single measurement alone, by estimating a joint probability distribution basically Gaussian over the variables for each timeframe. Not understood this definition ?
Well don’t worry. Its basically a series of matrix equations to quickly estimate the true value of the object being measured(in our case position) when the measured values(i.e sensor data) has lots of uncertainty or error. Lets try out with an example, suppose we are measuring the temperature(T) of object using a thermometer(it has a certain amount of uncertainty or error). Please refer below diagram. Initially Kalman filter starts out with random estimate temperature with more uncertainty then it gets sensor data(small circles) it quickly narrows down near the actual temperature and finally over time t it gets more accurate estimate of the temperature.
Why use Kalman Filter and not a simple average of measurements?
Whenever we use sensor data it includes a certain amount of error or uncertainty may be due to climatic /weather conditions or manufacturing error or error in processing. We need to estimate those measurements as accurate as possible.
When we take average of the measurements , we need all the measurements at once. Also, if we consider previous measurements few of measurements can shift the average value from actual value. For eg. we have measurements as 5 , 6, 4, 4 and the actual value is 4.5. When we take average of these values its 4.75. In this case, we are getting our estimate close to actual value but what if we have one outlier measurement 9 included , then our average becomes 5.5. So our error increases due to one such outlier value.
In real scenario where we are tracking an moving object from our car or any other vehicle , if our sensors send erroneous data or suppose if it didn’t send any data for a particular time we can’t rely on average of previous values. Kalman filter overcomes this problem of outliers. At a particular time, it only considers one sensor data and compares it with previous values such that it gives more weightage to the previously estimated measurement if it has low error and gives more weightage to the newly taken value from sensor if this has low error. In this way it removes the problem of outlier since it only considers a fraction of newly taken value at each time step.
Where it is used?
Well Kalman filter is used in predicting most of measurements where we get data from hardware i.e sensors etc and we are not certain how accurate our data is. Typically these are the applications of Kalman filter : autopilot in aeroplanes, robotics, Satellite navigation systems, Tracking of objects in computer vision, 3D modeling, Battery state of charge (SoC) estimation etc.
So how does it actually works ?
Here is the general process flow of Kalman filter algorithm:
Kalman filter algorithm can be basically organized into following steps:
- Initialize state & covariance matrices : We initialize the state(position & velocity) of the moving object i.e. bicycle when we receive the first sensor measurements.
- Prediction step: We make a prediction of a state, based on some previous values and model.
- Updation step : The predicted location and the measured location are combined to give an updated location. The Kalman filter will put more weight on either the predicted location or the measured location depending on the uncertainty of each value.
- And the process repeats as the car gets another sensor measurements.
Here is the full overview of the Kalman Filter process with matrices & equations :
You may be wondering how this matrix equations came from. Well, these are just the prediction and updation steps using our motion model equations which are as follows:
When represented in Matrix form,
where X` is the newly predicted state produced by multiply previous state X with F as the transition matrix and adding matrix U which considers acceleration(a_x and a_y). However for this project we are considering velocity as constant hence we are not including acceleration term. However we are still incorporating some uncertainty as state prediction noise W.
Kalman Gain(KG) : This is fraction is responsible for giving the Kalman filter where to put more weight on either the predicted location or the measured location depending on the uncertainty of each value. If error is measurement received from sensor is low, KG will be close to 1 meaning it will take larger fraction of new sensor measurement in updation step; on the other hand if the error/uncertainty is more it will be close to 0 meaning it will take only a small fraction of it and incorporate in previous estimation.
Also, when we are estimating there may be some amount of noise or uncertainty or error , this we are incorporating as Q.
Remember from the definition that Kalman filter considers this noise matrices in the form of Gaussian distribution with mean and std deviations which is explained below:
The std deviation represents the uncertainty/noise in our estimate. As we follow a number of prediction & updation steps our std deviation becomes less(a narrow Gaussian) meaning we are less uncertain about our measurements and hence gives accurate results. Thats the magic of Gaussians and thats how Kalman Filter works.
Now we got a gist of Kalman Filter, so what’s extended version of it ?
In Kalman filter, we are assuming that prediction & updation steps are linear functions but most real world problems involve non linear functions. In most cases, the system is looking into some direction and taking measurement in another direction. This involves angles and sine, cosine functions which are non linear functions which then lead to problems. We receive these non-linear measurements especially from radar as laser sensors represent measurements in cartesian coordinate while the radar sensors represent in polar coordinate. Hence, we have to be able to linearly map from the polar coordinate to the cartesian coordinate to help work on Kalman Filter . In Extended Kalman Filter(EKF), it uses the method called first order Taylor expansion to obtain linear approximation of the polar coordinate measurements in the update. In this process, a Jacobian matrix is produced, which represents the linear mapping from polar to cartesian coordinate, applied at the update step. Hence, the conversion matrix H becomes the Jacobian matrix Hj, while we will use non-linear state transition and measurement functions for both prediction and update respectively. Rest all is same as Kalman filter.
I have explained above few of these equations but EKF in itself is a vast topic hence cannot be put in a single post. I recommend to check the following links for more detailed explanation on the Kalman filter especially the video tutorials series which explains it elaborately :
- The Extended Kalman Filter: An Interactive Tutorial for Non-Experts
- Special Topics — The Kalman Filter (Video Tutorial) by Michel
Here is the result(video) of my project. In this, Lidar measurements are red circles, radar measurements are blue circles with an arrow pointing in the direction of the observed angle, and estimation markers predicted by our code are green triangles. Our aim was to minimize the RMSE (Root mean squared error) values as low as possible to give more accurate results.
Also, there is one related topic, the Unscented Kalman filter or Sigma point filter which solves the non-linearity problem in Kalman filter by using the concept of sigma points. When the prediction & updation steps are highly non-linear, EKF will give relatively poor performance. The Unscented Kalman filter uses a deterministic sampling technique known as the unscented transform (UT) to pick a minimal set of sample points (called sigma points) around the mean. The sigma points are then propagated through the non-linear functions, from which a new mean and covariance estimate are then formed.
Feel free to try this project, here is my github link(my code is in src folder) :
Reach out to me incase you have any issues regarding implementation or setting up environment . My linkedin profile : here