Fix the 0=2pi bug in EKF demo.