Normalize heading

This is done for the Runge-Kutta 2nd order integration, but we didn't do it for the exact integration.

Note that in any case, if we don't normalize the angle, the odometry that is finally published uses a quaternion for the heading, so it gets always normalized at that point (I've verified this on the robot).