Abstract: The present disclosure provides a system for tracking a target device in three-dimensional space. The system includes a light detection and ranging (LiDAR) sensor physically attached to a target device and configured to generate LiDAR data of an environment at a first frequency, and an inertial measurement unit (IMU) configured to generate IMU data at a second frequency higher than the first frequency. At least one processor is configured to perform simultaneous localization and mapping (SLAM) using the LiDAR data to generate SLAM pose data at at least the first frequency, perform sensor fusion of the SLAM pose data and the IMU data to generate fused pose data at a third frequency higher than the first frequency and lower than the second frequency, and output the fused pose data to provide position and orientation of the target device.