In this project we will utilize a kalman filter to estimate the state of a moving object of interest with noisy lidar and radar measurements. We will use RMSE as the error metric.
Note: The current version as of 22th February, 2019 is tested on Ubuntu. I also assume that you have other dependencies installed on your system.
- Download the simulator for this project from here
- Clone this repository using below command on the shell
git clone https://github.com/rake93/CarND-Extended-Kalman-Filter-Project.git- Navigate to the project top directory i.e. udacity-ExtendedKalmanFilters.
- create a directory to build the project using below command and navigate to it
mkdir build; cd build- Build the project
cmake .. && make- Run the project
./ExtendedKF- Since the server is now running, open the simulator and run it. Choose the Graphics Quality as Fastest and Screen Resolution as 800x600. This configuration ensures the system runs smoothly and does not end up consuming a lot of cpu resources.
- Choose Project 1/2: EKF and UKF and press Select.
- Finally, check if Connected!! is displayed on the console where you ran ExtendedKF in the 6th step and if everything is ok as expected, click Start. You will be able to see the car navigate and at the same time, the RMSE for x, y, vx, vy getting updated due to the calculations at each instance or rather input.
- cmake >= 3.5
- All OSes: click here for installation instructions
- make >= 4.1 (Linux, Mac), 3.81 (Windows)
- Linux: make is installed by default on most Linux distros
- Mac: install Xcode command line tools to get make
- Windows: Click here for installation instructions
- gcc/g++ >= 5.4
- Linux: gcc / g++ is installed by default on most Linux distros
- Mac: same deal as make - install Xcode command line tools
- Windows: recommend using MinGW
- uWebSocketIO => Download for either Linux or Mac systems.
Below is the result for the dataset 1
| State Vector | Calculated Value | Max Threshold |
|---|---|---|
| x | 0.0974 | 0.11 |
| y | 0.0855 | 0.11 |
| vx | 0.4517 | 0.52 |
| vy | 0.4404 | 0.52 |
Note: Read below only if you seek additional technical details about Kalman Filters
Below I discuss about Kalman Filters, their applications, components that constitute the Kalman Filters and other relevant details. It is an optional and lengthy read, and is a work in progress. If I happen to convert into a blog series, will update the links here. Lets get rolling!
Q. What is a kalman filter?
It's a repetitive/iterative mathematical process that has some inputs plugged into equations to predict/estimate the next value in terms of position, velocity, etc of any object being measured, when the measured values contain unpredicted errors, uncertainty, or variation.
I am sure, an application based explanation would help you understand its usage better.
Suppose that we are building a self-driving car, and for this, we are using sensors to track the pedestrians, vehicles, objects on the road. Now, at every instance, we get a reading from the sensors. Now, our task is to make the car ride on the road without colliding. So, primarily we need to tell the car, what its position and velocity should be at t+1 where t is the current instance. We use the measurements from the sensors and a bunch of equations to predict what the next position and velocity of the car should be. This prediction of position and velocity happens till we reach the destination which means its an iterative/repetitive/continuous process as mentioned in the formal definition.
Now, there are some errors when we depend on sensors, there are errors when mathematical calculations are involved. So, the kalman filter also has a solution to rectify the noise/error (not completely but to a manageable extent).
Q. What is a typical workflow of the entire process?
- calculate the kalman gain
- calculate the current estimate
- recalculate the new error in the estimate
- repeat the process(1-3) iteratively
Calculate the Kalman Gain
In this step, we use the error in estimate (our calculations) and error in data (measurements, sensor specific) to find the kalman gain. We give more emphasis to the error that is minimum as it would contribute better to ensure we stay close to the actual/true value.
Calculate the Current Estimate
We have the kalman gain that we calculated in the previous step. This kalman gain is fed to the next step along with the previous estimate and the stream of input data we receive at each instance, we calculate the current estimate. The kalman gain will decide how much weight to put on estimate or the input data. So the task of the kalman gain is to put relative importance on the previous estimate and input data.
Calculate the Error in the Estimate
We use the calculated current estimate and the kalman gain to find out the error so that we can feed it back when calculating the kalman gain for the next instance of time.
Now for each of the above step, we get an estimate which says how the object should be moving. Let's look at the mathematics behind this in the below sections.
KG = Kalman Gain
Eest = Error in estimate
Emea = Error in data (measurement)
KG = Eest / (Eest + Emea)
0 <= KG <= 1
Lets say,
ESTt = current estimate
ESTt-1 = previous estimate
MES = Measurement
then,
ESTt = ESTt-1 + KG[MEA - ESTt-1]
Higher the KG, higher the Eest and lower the Emea
Lower the KG, lower the Eest and higher the Emea
So, by looking at the above proportions, it should make sense to understand that, over the time, the KG should get smaller and smaller as our estimates will keep getting better, the error in data (measurement) being constant.
Eest_t = ( Emea * Eest_t-1 ) / (Emea + EestP_t-1)
Sometimes, the above eqn can also be written as,
Eest_t = (1 - KG)(Eest_t-1)
With the above eqns, we can definitely say that the error in the estimates will always get smaller.