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 re-considers data associations during the search, enabling robust pose estimation even under varying roll and pitch angle of the robot enabling mapping on rough terrain. The 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 has been 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 higher sophisticated offline methods, such as Rao-Blackwellized SLAM.