Works better when ignoring heading sensor updates... But there still is the 5 meters shift.