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
