2
$\begingroup$

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

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

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.

Improve this answer
$\endgroup$
3
  • $\begingroup$ For a single angle one doesn't need quaternions, just complex numbers would already be sufficient (or equivalently $(\sin\theta,\cos\theta)$). $\endgroup$
    – fibonatic
    Aug 31 at 7:32
  • $\begingroup$ 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. $\endgroup$
    – fibonatic
    Sep 1 at 11:58
  • $\begingroup$ 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. $\endgroup$
    – Tully
    Sep 2 at 20:57
0
$\begingroup$

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$.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.