Teleoperation is a difficult task, particularly when controlling robots from an isolated operator station. In general, the operator has to solve ??nearly blindly?? the problems of mission planning, target identification, robot navigation, and robot control at the same time. The goal of the proposed system is to support teleoperated navigation with real-time mapping. We present a novel scan matching technique that reconsiders data associations during the search, allowing robust pose estimation even under a varying roll and pitch angle of the robot enabling mapping on rough terrain. This approach has been implemented as an embedded system and extensively tested on robot platforms designed for teleoperation in critical situations, such as bomb disposal. Furthermore, the system was evaluated in a test maze by first responders during the Disaster City event in Texas 2008. Finally, experiments conducted within different environments show that the system yields comparably accurate maps in real-time when compared to more highly sophisticated offline methods, such as Rao-Blackwellized SLAM.