Error State Extended Kalman Filter Localization for Underground Mining Environments

Abstract

The article addresses the issue of mobile robotic platform positioning in GNSS-denied environments in real-time. The proposed system relies on fusing data from an Inertial Measurement Unit (IMU), magnetometer, and encoders. To get symmetrical error gauss distribution for the measurement model and achieve better performance, the Error-state Extended Kalman Filter (ES EKF) is chosen. There are two stages of vector state determination: vector state propagation based on accelerometer and gyroscope data and correction by measurements from additional sensors. The error state vector is composed of the velocities along the x and y axes generated by combining encoder data and the orientation of the magnetometer around the axis z. The orientation angle is obtained from the magnetometer directly. The key feature of the algorithm is the IMU measurements’ isolation from additional sensor data, with its further summation in the correction step. Validation is performed by a simulation in the ROS (Robot Operating System) and the Gazebo environment on the grounds of the developed mathematical model. Trajectories for the ES EKF, Extended Kalman Filter (EKF), and Unscented Kalman Filter (UKF) algorithms are obtained. Absolute position errors for all trajectories are calculated with an EVO package. It is shown that using the simplified version of IMU’s error equations allows for the achievement of comparable position errors for the proposed algorithm, EKF and UKF.