State estimation is a vital part of any self-driving car software stack, which relies on the sensor measurements consisting of an IMU, a GNSS receiver, and a LiDAR to provide an accurate state estimation. However, since all of the sensors provide measurements of varying reliability and at different rates and reference frames, spatial and temporal multisensor calibration is required.
This project applied the Error-State Extended Kalman Filter (ES-EKF) on the state estimation pipeline to enhance the accuracy and reliability of the state estimation.
Where the motion model includes the specific force and rotational rates from our IMU and vehicle state consisting of position, velocity, and parametrization of
orientation using a unit quaternion.
More details on implementation are provided in implementation_detial.pdf
First of all, testing the performance of the estimation with normal sensor measurements. The model is able to provide accurate estimate within error range.
In orfer to test out the reliability of the state estimation, an intentional error is applied on the rotational matrix when transforming the reference frame of the LIDAR, resulting unreliable LIDAR measurements. By increasing the variance of the LIDAR estimated error, the model adapts and still performs well.
A portion of the GNSS and LIDAR data is intentionally erased to test the performance of the estimation when only IMU is available. (like vehicle entering a tunnel)
Although there exists a small shift in height estimation due to the wrong estimation of pitch angle, the model quickly adjusted state estimation of pitch angle and can perform as expected within a reasonable time and error range.
Install the official release of NumPy and matplotlib through pip:
python -m pip install -U pip
python -m pip install -U numpy
python -m pip install -U matplotlibpython es_ekf.pySpecial thanks to State Estimation and Localization for Self-Driving Cars by the University of Toronto for providing this excellent online course, and my mentor Dr. Tao from Texas A&M University for advising and sharing this information.