Building an Autonomous Vehicle Part 4.3: Sensor Fusion and Object Tracking using Unscented Kalman Filters
This post is in continuation to my last two posts on Kalman Filter and Extended Kalman Filters. Today we will look at another member of Kalman Filter Family: The Unscented Kalman Filter.
Short Summary:
Kalman Filter: It is a tool to predict values using a bunch of mathematical equations under the assumptions that our data is in the form of Gaussian Distribution and we apply linear equations to that Gaussian distribution.
Extended Kalman Filter: In real world, we have non linear equations, because we may be predicting in one direction while our sensor is taking reading in some other direction, so it involves angles and sine cosine functions which are non linear. So EKF takes helps of Taylor Series (and Jacobian Matrix further) to linearly approximate a non linear function around the mean of the Gaussian and then predict the values.
In EKF, we know that the distribution we get after nonlinear transformation is not a Gaussian. Nevertheless, we pretend it as a Gaussian and try to find the best approximation of real distribution. To do that we take the mean of the original Non-Linear Gaussian and pass it through a Non Linear Function to predict the best approximate Gaussian. Since here we approximate around only one point i.e. mean, the Gaussian we get doesn’t give us a good approximation.
️Unscented Kalman Filter (UKF) proposes a solution ✔ to this problem️. In UKF we have a concept of Sigma Points. Here we take some points(known as Sigma Points) on source Gaussian and map them on the target Gaussian after passing those points through some non linear function and then we calculate the new mean and variance of transformed Gaussian.
Now you must be wondering why are we not considering all points instead we are taking some random points and transforming them to predict the Gaussian? 🤔
Good Question… 😋
Actually we can also use all points in Original non-linear Gaussian and pass them through non linear function to get the best approximate Linear Gaussian but that would consume a lot of computation power and time 😓 and hence can not be used in real time. Also sigma points method gives us the result pretty close to the expected one 😜.
Now since we have successfully predicted the position of an object in the surrounding, from here onward we can use Kalman Filter Update equation to update the existing belief of the object’s position and the process of predict and update will continue as discussed in my first blog on Kalman Filters.
Conclusion and Discussion: 😎
Here the main difference from EKF is that in EKF we take only one point i.e. mean and approximate, but in UKF we take a bunch of points called sigma points and approximate with a fact that more the number of points, more precise our approximation will be!
In addition to sigma points, these points also have weights, so these are weighted sigma points. The points near the mean will have more weight the the points away from the mean.
When a Gaussian is passed through a non linear function, it does not remains a Gaussian anymore but we approximate the Gaussian from the resulting figure, so in UKF a process called Unscented Transform helps us to perform this task.
To summarize here are the below steps the unscented transform performs:
1. Compute Set of Sigma Points
2. Assign Weights to each sigma point
3. Transform the points through non linear function
4. Compute Gaussian from weighted and transformed points
5. Compute Mean and Variance of the new Gaussian.
Well, that’s all folks 😋. I have tried to explain the basic overview of the Kalman Filter Family in a very simple manner 😝. Hope you gained something after reading the Posts 😉. If you want to get deeper 🕵️ into mathematical part of the Extended Kalman Filters then I would suggest you to go through these amazing video lectures series on Kalman Filters by Michel van Biezen 👦.
You can find my implementation on UKF and other Self-Driving Car related projects on my GitHub here 👻. Please feel free to provide me any suggestions, corrections or additions in the comments :)
