In this article we present a blue force tracking system for operators in underground structures. By fusing inertial data sensed by an Inertial Measurement Unit (IMU) with distance information from Ultra-Wideband (UWB) and map information from a 3D model, a robust tracking system is introduced. Since the needed infrastructure and map model cannot be assumed available, tools are developed to provide the necessary information in near real-time. Our tests in a tunnel at Zentrum am Berg (ZaB) showed that the developed system provides robust localization information.