Fixed the discontinuity issue. Lets try to reintegrate the heading sensor.
Works, last problem is the 5meters shift
Added visualization of the range sensor.
Added true range sensor vs predicted. Need to fix colors