🐳 ☕️ 🧧

INTRODUCTION

This is an unscented Kalman Filter implementation in C++ for fusing lidar and radar sensor measurements. A Kalman filter can be used anywhere where you have uncertain information about some dynamic system, and you want to make an educated guess about what the system is going to do next.

In this case, we have two 'noisy' sensors: - A lidar sensor that measures a tracked object's position in cartesian-coordinates

(x, y)

(rho, phi, drho)

We want to predict a tracked object's position, how fast it's going in what direction, and how fast it is turning (yaw rate) at any point in time. - In essence we want to get: the position of the system in cartesian coordinates, the velocity magnitude, the yaw angle in radians, and yaw rate in radians per second

(x, y, v, yaw, yawrate)

This unscented kalman filter does just that.

• NOTE: Compared with an Extended Kalman Filter with a constant velocity model, RMSE should be lower for the unscented Kalman filter especially for velocity. The CTRV model is more precise than a constant velocity model. And UKF is also known for handling non-linear equations better than EKF.
• Harvard Paper about UKF

CONTENTS

• Basic Usage
• Notes

BASIC USAGE

• Dependencies are same as in here
• Clone this repository

  $ git clone https://github.com/mithi/fusion-ukf/
  

• Go inside the

  build

  folder and compile:

  $ cd build
  $ CC=gcc-6 cmake .. && make
  

• To execute inside the

  build

  folder use the following format:

$ ./unscentedKF /PATH/TO/INPUT/FILE /PATH/TO/OUTPUT/FILE
$ ./unscentedKF ../data/data-3.txt ../data/out-3.txt

• Please use the following format for your input file ``` L(for lidar) mx my t rx ry rvx rvy, ryaw, ryawrate R(for radar) mrho mphi mdrho t rpx rpy rvx rvy, ryaw, r_yawrate

Where: (mx, my) - measurements by the lidar (mrho, mphi, mdrho) - measurements by the radar in polar coordinates (t) - timestamp in unix/epoch time the measurements were taken (rx, ry, rvx, rvy, ryaw, r_yawrate) - the real ground truth state of the system

Example: L 3.122427e-01 5.803398e-01 1477010443000000 6.000000e-01 6.000000e-01 5.199937e+00 0 0 6.911322e-03 R 1.014892e+00 5.543292e-01 4.892807e+00 1477010443050000 8.599968e-01 6.000449e-01 5.199747e+00 1.796856e-03 3.455661e-04 1.382155e-02

- The program outputs the predictions in the following format on the output file path you specified:

NOTES

If you take a look at settings you'll see the following:

//process noise standard deviations
const double STD_SPEED_NOISE = 0.9; // longitudinal acceleration in m/s^2
const double STD_YAWRATE_NOISE = 0.6; // yaw acceleration in rad/s^2

Here's the terminal output from the given data set

Here's a visualization of how it's performing

Here's a visualization of the Radar's NIS

Here's a visualization of the Lidar's NIS

Here's my UKF algorithm overview

And here's an overview of what the instantiated classes are doing

🐳 ☕️ 🧧