Object Tracking with Sensor Fusion-based Extended Kalman Filter

Objective

Utilize sensor data from both LIDAR and RADAR measurements for object (e.g. pedestrian, vehicles, or other moving objects) tracking with the Extended Kalman Filter.

---

Code & Files

1. Dependencies & environment

• cmake >= 3.5
• All OSes: click here for installation instructions
• make >= 4.1
  • 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]((https://developer.apple.com/xcode/features/)
  • Windows: recommend using MinGW
• Eigen library

2. My project files

• CMakeLists.txt is the cmake file.

• data folder contains test lidar and radar measurements.

• Docs folder contains docments which describe the data.

• src folder contains the source code.

3. Code Style

• Google’s C++ style guide.

4. How to run the code

1. Clone this repo.
2. Make a build directory: mkdir build && cd build
3. Compile: cmake .. && make
   • On windows, you may need to run: cmake .. -G "Unix Makefiles" && make
4. Run it by either of the following commands:
   • ./ExtendedKF ../data/obj_pose-laser-radar-synthetic-input.txt ./output.txt
   • ./ExtendedKF ../data/sample-laser-radar-measurement-data-1.txt ./output.txt

System details

1. How does LIDAR measurement look like

The LIDAR will produce 3D measurement px,py,pz. But for the case of driving on the road, we could simplify the pose of the tracked object as: px,py,and one rotation. In other words, we could only use px and px to indicate the position of the object, and one rotation to indicate the orientation of the object. But in real world where you have very steep road, you have to consider z axis as well. Also in application like airplane and drone, you definitely want to consider pz as well.

2. How does RADAR measurement look like

3. Comparison of LIDAR, RADAR and Camera

Sensor type	LIDAR	RADAR	Camera
Resolution	median	low	high
Direct velocity measure	no	yes	no
All-weather	bad	good	bad
Sensor size	large	small	small
sense non-line of sight object	no	yes	no

Note:

• LIDAR wavelength in infrared; RADAR wavelength in mm.
• LIDAR most affected by dirt and small debris.

One comparison Figure from another aspect.

4. How does the Extended Kalman Filter Work

5. Extended Kalman Filter V.S. Kalman Filter

• x is the mean state vector.
• F is the state transition function.
• P is the state covariance matrix, indicating the uncertainty of the object’s state.
• u is the process noise, which is a Gaussian with zero mean and covariance as Q.

• Q is the covariance matrix of the process noise.

• y is the innovation term, i.e. the difference between the measurement and the prediction. In order to compute the innovation term, we transform the state to measurement space by measurement function, so that we can compare the measurement and prediction directly.
• H is the measurement function.
• z is the measurement.
• R is the covariance matrix of the measurement noise.
• I is the identity matrix.
• K is the Kalman filter gain.
• Hj and Fj are the jacobian matrix.

All Kalman filters have the same three steps:

1. Initialization
2. Prediction
3. Update

A standard Kalman filter can only handle linear equations. Both the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF will be disuccsed in the next project) allow you to use non-linear equations; the difference between EKF and UKF is how they handle non-linear equations: Extended Kalman Filter uses the Jacobian matrix to linearize non-linear functions; Unscented Kalman Filter, on the other hand, does not need to linearize non-linear functions, insteadly, the unscented Kalman filter takes representative points from a Gaussian distribution.