Moving Horizon Estimation SLAM for Agile Vehicles in 3-D Environments

The ability for a robot to be able to construct a map of the environment and recognize its position on it was one of the biggest developments in robotics. Simultaneous localization and mapping (SLAM) framework builds onto the perception of the robot, giving it the possibility to online calculate its trajectory and avoid obstacles. Moreover, the continuous development of processing units has given the possibility for previously hardware exhausting solutions to be considered as an option for the localization and mapping problem. With this in mind, this work is focused on developing a SLAM solution for a 6 DoF vehicle operating on a 3-D environment using moving horizon estimation (MHE). Throughout the paper it is tested the applicability of the proposed solution in a simulation environment of two loop square-shaped corridors with stationary landmarks, whilst comparing the obtained results with another probabilistic approach, the EKF, which is commonly used but loses stability on extremely nonlinear dynamics. Each of the algorithms is simulated in MATLAB.