How do you handle angle discontinuities in estimation problems?

When one is implementing a state estimator in a system that involves kinematics, will inevitably face the problem of angle discontinuities, i.e., the fact that the angles have to be wrapped between within [-pi -> pi), or between ( 0 -> 2pi ], or else estimation algorithms such as a KF will not work due to the possibility of the error state becoming biased.

Roughly speaking, imagine the following

$$\hat{e}_k = \hat{\theta}_k - \theta_k, \quad \text{where} \quad\hat{\theta}_k=2\pi \quad \text{and} \quad \theta_k = 0$$

In that case the estimated pose $$\hat{\theta}$$ matches the actual pose $$\theta$$, but the estimator is creating an error equal to $$\hat{e}_k = 2\pi$$. This error is propagated into the estimators correction step and is creating a correction to the pose that shouldn't occur.

What kind of problems have you in general faced concerning this discontinuity issue and what was your solution?

Thanks a lot, A student form Denmark

• Why wouldn't you use quaternions for orientation? – Long Smith Jul 6 '20 at 11:09
• It is an option, but in my opinion much less intuitive and I am having a hard time formulating the equations in quaternion form. – D Dim Jul 14 '20 at 12:29