We present a prototype system intended to be used for positioning and mapping by soldiers or first responders in unknown and dangerous environments. The system is based on inertial navigation combined with images and dense distance measurements from a range camera using active illumination. Although it is a prototype, the system can be used for evaluation in real-time, presenting results on a tablet computer. The hardware, software, and algorithms are described in the paper. Results from a number of experiments are also shown, demonstrating that the system provides good positioning and mapping performance in various indoor environments, including in darkness and smoke.