Dynamics-Based Modified Fast Simultaneous Localization and Mapping for Unmanned Aerial Vehicles with Joint Inertial Sensor Bias and Drift Estimation

Abstract

In this paper, the problem of simultaneous localization and mapping (SLAM) using a modified Rao Blackwellized Particle Filter (RBPF) (a modified FastSLAM) is developed for a quadcopter system. It is intended to overcome the problem of inaccurate localization and mapping caused by inertial sensory faulty measurements (due to biases, drifts and noises) injected in the kinematics (odometery based) which is commonly used as a motion model in FastSLAM approaches. In this paper, the quadcopter's dynamics with augmented bias and drift models is employed to eliminate these faults from the localization and mapping process. A modified FastSLAM is then developed in which both Kalman Filter (KF) and Extended Kalman Filter (EKF) algorithms are embedded in a PF with modified particles weights to estimate biases, drifts and landmark locations, respectively. In order to make the SLAM process robust to model mismatches due to parameter uncertainties in the dynamics, measurements are incorporated in the PF and in the particle generation process. This leads to a cascaded two-stage modified FastSLAM in which the extended FastSLAM 1.0 (to include dynamics and sensory faults) is employed in first stage and the results are used in second stage in which probabilistic inverse sensor models are incorporated in the particle generation process of the PF. The efficiency of the proposed approach is demonstrated through a co-simulation between MATLAB-2019b and Gazebo in the robotic operating system (ROS) in which the quadcopter model is simulated in Gazebo in ROS using a modified version of the Hector quadcopter ROS package. The collected pointcloud data using LiDAR is then utilised for feature extraction in the Gazebo. The simulation environment used to this aim is validated based on experimental data.