Simultaneous Localization and Mapping Using Rao-Blackwellized PHD Filtering

Faced with the increasing demand for indoor localization, global navigation satellite systems perform significantly worse in indoor environments than in outdoor scenarios. Existing indoor algorithms also have high requirements for the environment and terminal devices. To address this, we propose a new simultaneous localization and mapping algorithm that improves the Rao-Blackwellized probability hypothesis density filter, achieving target tracking using only Time of Flight with the assistance of an inaccurate floor plan and we incorporate a new data association method into this framework. Additionally, given that most current tracking algorithms are based on a known initial position, we have proposed an algorithm to determine the target's initial position. Simulation results demonstrate the effectiveness of the algorithm.