Udacity Self-Driving Car Engineer Nanodegree Program. Term 2, Project 1.
The rubric followed for this project can be viewed Here.
Pictures and equations referenced in this writeup are from lessons in the Udacity Self-Driving Car Engineer Nanodegree Program. Link To The Program.
- 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
- Clone this repo.
- Make a build directory:
mkdir build && cd build - Compile:
cmake .. && make- On windows, you may need to run:
cmake .. -G "Unix Makefiles" && make
- On windows, you may need to run:
- Run it:
./ExtendedKF
This project code connects to a simulator to output its calculations, and therefore cannot show output without access to it. Example output from the simulator can be viewed below.
This project is to demonstrate the implementation of a kalman filter on noisy radar and lidar measurements to estimate the state of a moving object. The predictions will be measured for accuracy by calculating RMSE (Root-Mean-Square Error). This is further discussed below.
Radar and lidar are used in conjunction for this project to get accurate measurements of object position. Since radar and lidar have different strengths and weaknesses, using both can give us a more accurate measurement for object position than only using one or the other.
Lidar (Light Detection and Ranging): Lidar uses infared laser beams to determine the distance from the sensor to a nearby object. Lidar swivels and projects pulses of these infared beams in the area around the sensor. Theses are then reflected off of nearby objects to create a point cloud.
Radar (Radio Dection and Ranging): Radar similarily uses pulses that reflect or "echo" off surfaces. Radar works over longer distances than lidar, but has a poorer resolution-- especially vertically.
| Specification | Lidar | Radar |
|---|---|---|
| Resolution | OK | Bad |
| Noise | Bad | Bad |
| Velocity | Bad | Good |
| All-Weather | Bad | Good |
| Size | Bad | Good |
Resolution: Lidar has an OK resolution, while radar has poor resolution. Lidar has a more focussed laser and uses a higher level of vertical scan layers to create a dense point cloud. Radar has especially poor vertical resolution.
Noise: Neither sensor is particularly good with noise. Radar can be highly reflective with certain materials or surfaces, which throws off measurements. Lidar has a relatively higher resolution-- as stated before, which can pick up extra noise.
Velocity: Lidar can not currently measure direct velocity, and must be calculated separately. Radar can currently measure direct velocity by the doppler effect.
All-Weather: Lidar is more susceptible to different weather conditions. The sensor must be kept clean. Radar is generally good in all weather conditions.
Size: Lidar is bulkier and harder to integrate. Radar is smaller and can be mounted behind bumpers or other non-visible locations.
This program follows the flow of this chart:
- R: measurement noise.
- H: measurement function.
-
Yes: Initialize state x. Convert from polar to cartesian if a radar measurement. Initialize uncertainty covariance P. Initialize state transition matrix F.
-
No: Compute elapsed time delta t. (Current - Previous) / 1000000.
- F: Computed with new delta t.
- Q: Process covariance matrix. Computed with delta t and x and y noise.
-
Run a prediction with previously defined matrices.
(u is not used for prediction step in this program as u = 0)
The above initialization steps can be viewed in FusionEKF.cpp. Prediction function in kalman_filter.cpp.
-
Radar: Linearize measurement function by calculating jacobian matrix Hj:
Run measurement update with Extended Kalman Filter equations:
Image referenced is of Kalman Filter equations. Extended Kalman Filter equation is modified to be y = z - h(x), and uses Hj in place of any other H. h(x) is defined as:
-
Lidar: Update function run from standard Kalman Filter equations:
^T = Tranpose. ^-1 = Inverse.
The measurement function can be found in kalman_filter.cpp and the CalculateJacobian() in tools.cpp.
To calculate the accuracy of estimated postitions from the Kalman Filter, we use RMSE. This is calculated from the following equation:
This equation takes the difference between the estimates and the ground truth, then squares them so larger differences are weighted more. These squared differences are then summed, averaged, and taken the root of to get the final value.
I took the final RMSE of 3 distinct simulations estimated points. One with only radar measurements, one with only lidar measurements, and then both measurements combined. These are the final outcomes for each distinct simulation after one full run with data set 1:
(Smaller RMSE = More accurate)
| Lidar |
|---|
| X: 0.1473 |
| Y: 0.1153 |
| VX: 0.6383 |
| XY: 0.5346 |
| Radar |
|---|
| X: 0.2357 |
| Y: 0.3110 |
| VX: 0.5679 |
| XY: 0.7818 |
| Radar + Lidar |
|---|
| X: 0.0996 |
| Y: 0.0837 |
| VX: 0.4517 |
| XY: 0.4419 |
As can be seen above, lidar is the most accurate standalone measurement. As to be expected: both sensors combined has a much better accuracy than either on its own. My passing threshold was <= [.11, .11, 0.52, 0.52] for this project, which my sensor fusion passes.
The simulator is a flat overhead view of a car that displays simulated radar and lidar measurements:
The red circles represent lidar measurements, blue circles are radar, and the green triangles are the estimated postitions from the Kalman Filter.
The simulator has two datasets to run. The first is a figure 8 line, the second is a reverse figure 8 line:
Dataset 1, both sensors:
As can be seen above, my estimated line is fairly accurate. My final RMSE values here are the ones discussed above in the RMSE section.
Dataset 2, both sensors:
I think this project was fun to tinker with. Simulator based projects are the best-- in my opinion, because I have a real time representation of the code I wrote. It gives me a lot of insight into what needs to be improved, as well as also telling me what works and doesn't work. I am happy with the RMSE values based on my current knowledge of Kalman Filters. This exercise will definitely be helpful for Unscented Kalman Filters.