# 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? 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. Jul 14 '20 at 12:29

As mentioned by @long-smith the standard solution is to use quaternions.

However if you specifically are asking how to deal with errors using angles that are modulo $$2\pi$$ you are going to want to add logic to compute the smallest angle between two angles which take into account wrapping.

An example is shorted_angular_distance from the ROS angles package which can bed used for your error metrics and deals with the wrapping edge cases.

• For a single angle one doesn't need quaternions, just complex numbers would already be sufficient (or equivalently $(\sin\theta,\cos\theta)$). Aug 31 at 7:32
• Using the shortest angle distance might sometimes cause issues if the error is close to $\pm\pi$ and there is either measurement noise or some other disturbance, such that the calculated error might often flip between a bit more than $-\pi$ and a bit less than $\pi$. Though, one does have a somewhat similar issue when using complex-numbers/quaternions near 180° estimation error. Sep 1 at 11:58
• There's no specific issue with shortest angle distance. All implementations will have the exact same problem when used on the back side of a continuous, that is an inherent problem with the manifold which wraps. With latency and noise in the system you could find yourself in a bad unstable equilibrium. But this is also quite academic for this use case as $\pm\pi$ is the absolute maximum of the possible error. If your estimator has an error this large, it is literally indicating the exact opposite of the real value and cannot get worse, as worse will wrap and actually be more accurate. Sep 2 at 20:57

The simplest way to have values between [0, 2$$\pi$$] is using the modulo operator.

You can simply do

$$\hat{e}_k = (\hat{\theta}_k - \theta_k) \% 2\pi$$.