Integrated mapping module. Fixed time gap bug caused by wrong convertsion to nanoseconds. Heading is obviously erroneous most of the time.