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

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

Inertial sensor self-calibration in a visually-aided navigation approach for a micro-AUV

This paper presents a new solution for underwater observation, image recording, mapping and 3D reconstruction in shallow waters. The platform, designed as a research and testing tool, is based on a small underwater robot equipped with a MEMS-based IMU, two stereo cameras and a pressure sensor. The data given by the sensors are fused, adjusted and corrected in a multiplicative error state Kalman filter (MESKF), which returns a single vector with the pose and twist of the vehicle and the biases of the inertial sensors (the accelerometer and the gyroscope). The inclusion of these biases in the state vector permits their self-calibration and stabilization, improving the estimates of the robot orientation. Experiments in controlled underwater scenarios and in the sea have demonstrated a satisfactory performance and the capacity of the vehicle to operate in real environments and in real time