A mapping system, comprising an inertial measurement unit; a camera unit; a laser scanning unit; and a computing system in communication with the inertial measurement unit, the camera unit, and the laser scanning unit, wherein the computing system computes first measurement predictions based on inertial measurement data from the inertial measurement unit at a first frequency, second measurement predictions based on the first measurement predictions and visual measurement data from the camera unit at a second frequency and third measurement predictions based on the second measurement predictions and laser ranging data from the laser scanning unit at a third frequency.