An Indoor Navigation System using Stereo Vision, IMU and UWB Sensor Fusion

In recent years, indoor localization has found many applications in civil and military fields. As Global Navigation Satellite Systems (GNSS) are not available indoors, the state-of-art trend is to fuse low-cost/low-power accelerometer/gyroscope with other sensors such as vision sensors. To further enhance the accuracy, ranging technologies such as Ultra-wide-band (UWB) radio ranging have been introduced as an indoor replacement of GNSS. However, robust localization continues to be challenging as accelerometer/gyroscope sensors suffer from random biases. Similarly, UWB ranging suffers from noise and multipath. Visual sensors depend upon lighting conditions and they suffer from odometry drifts. To accurately and efficiently fuse these sensors, this paper presents a robust fusion framework based on Extended-Kalman Filter (EKF). The proposed design applies non-holonomic speed-aided motion constraints to minimize positional errors. This design enabled the direct fusion of speed estimated from stereo vision into EKF which is more immune to vision-related orientation errors. The overall positional drifts are further controlled by positional updates whenever a reliable UWB solution is available. Our developed system was tested in typical indoor environment using physical data and the experimental results showed robust sub-meter localization accuracy even under multiple outages.