The goals / steps of this project are the following:
- Complete the Unscented Kalman Filter algorithm in C++.
- Ensure that your project compiles.
- Test your Kalman Filter against the sample data. Ensure that the px, py, vx, and vy RMSE are below the values specified in the rubric.
- Normalize angles
- Don't forget to tune parameters and initialize variables
- Check for divide by zero
Rubric Points
Here I will consider the rubric points individually and describe how I addressed each point in my implementation.
The code compiles without errors with cmake-3.7.2 and make-3.81 on macOS-10.12.4.
1. The px, py, vx, vy output coordinates must have an RMSE <= [.09, .10, .40, .30] when using the file: "obj_pose-laser-radar-synthetic-input.txt".
The computationally stable RMSE on obj_pose-laser-radar-synthetic-input.txt
is [0.0655022, 0.0817954, 0.148616, 0.208478]
. 5% of NIS estimates are higher than 3.942 (lidar), 6.12856 (radar) - it's well correlating with the Chi-squared distribution for df=2 (5.991) and df=3 (7.815) respectively. Changing the process noise standard deviation of longitudinal (1 m/s^2) and yaw (Pi/5 rad/s^2) acceleration only decreases the overall accuracy.
1. Your Sensor Fusion algorithm follows the general processing flow as taught in the preceding lessons.
The implemented solution follows the Sensor Fusion method, including the Unscented Kalman Filter, which is described in the lesson.
The first measurement is handled by UkfTracker::operator()
defined in ukf_tracker.cpp
.
The method UkfTracker::Predict
is called before UkfTracker::Update
in UkfTracker::operator()
.
In ukf_tracker.cpp
, function PredictRadarMeasurement
computes sigma points, the predicted measurement mean and covariance in radar measurement space. Function PredictLidarMeasurement
does the same in lidar measurement space. The rest of the code is generic, agnostic to the sensor type.
- All duplicating expensive computations (like floating point multiplications and divisions) are done only once and then the result is reused.
- All computed constants and matrices are moved to a nameless namespace of
ukf_tracker.cpp
. - Note using kAugWeights.asDiagonal() before multiplying the weight vector with diff-matrices. Even though turning the weights vector into a diagonal might seem inefficient, but this vectorized multiplication demonstrates a significantly better performance, than an explicit loop over sigma points. When all occurrences of such multiplication are replaced with the loop shown in programming quizzes, the completion time over 8000 samples is about 3s (on an Intel i7). The vectorized completion time is 2.1s. It must be a result of the optimal Eigen matrix multiplication algorithm.
- The code is complying with the Google C++ Style Guide.
- The main Sensor Fusion class UkfTracker is made a functor, which is easy to plug into an algorithm generating estimations out from measurements, see the use of
std::transform
in functionmain
ofmain.cpp
- The C++11 automatic type deduction is used wherever appropriate.
- All function/method comments are made Doxygen-friendly