I'm tracking the state of a robot using an EKF defined by: $$(x,y,\theta)$$ where $x$ and $y$ are the coordinates in the ground-plane and $\theta$ the heading angle. I initialized the covariance matrix $P$ to the following values: $$P = \begin{bmatrix} \sigma_{xx}^2 & 0 & 0 \\ 0 & \sigma_{yy}^2 & 0 \\ 0 & 0 & \sigma_{\theta \theta}^2 \\ \end{bmatrix}$$ where $\sigma_{xx}=1$, $\sigma_{yy}=1$ and $\sigma_{\theta \theta }=0.1$

In the system i'm using, valid measurements are rearely obtained so the predicted covariance matrix becomes very big. My question concerns the angle uncertainty, the angle is predicted from angle increments $\Delta_{theta}$ so, at each time stamp $k$: $$\theta=\theta+\Delta_{theta_{k}}$$ We wrap the value of the obtained angle to make $\theta\in[-\pi,\pi]$. Does the uncertainty have to be also wrapped to the interval $[-\pi,\pi]$? means that making $\sigma_{\theta }\in[-\pi,\pi]$?

• NO. Don't touch uncertainty matrix. Remember elements of the uncertainty matrix are eventually plotted as an ellipse via using eigenvalues and eigenvectors, therefore, small ellipse means small error and big ellipse means big error. Feb 7 '18 at 13:13
• The heading angle uncertainty I got is very big attaining 100 deg ..Is it logical to have such a big uncertainty? Feb 7 '18 at 15:15

No, the uncertainty should not be wrapped.

Remember, uncertainty is fundamentally different than angle. At the most trivial level, uncertainty cannot be negative or even zero (i.e. $\sigma_\theta > 0$).

At another level, the angles $\pi$ and $3\pi$ represent the exact same thing, and wrapping the angle is more a nicety than an actual requirement. But $\sigma_\theta=\pi$ implies less uncertainty than $\sigma_\theta = 3\pi$ so if you wrapped you'd be fundamentally changing the uncertainty.

• The heading angle uncertainty I got is very big attaining 100 deg ..Is it logical to have such a big uncertainty? Feb 7 '18 at 15:10
• Mathematically it's fine. Practically speaking it may be a problem but it could indicate an error in tuning or exactly what it implies: you aren't getting accurate measurements often enough to have good certainty of the angle. (Note that this is a separate question from the original you posted here) Feb 7 '18 at 15:17
• Thank you for giving me an indication to try to correct the problem..if it isn't corrected I'll post another question. In what I'm doing, measurements aren't often taken. That is, after 1000 m for example and the accumulation of drift and big covariance matrix, a correction is made Feb 7 '18 at 15:22

If your angle is oscillating by 0.2° between 259.9° and 0.1° and you apply wrapping to it before feeding back into the Kalman filter, you will get a large error; your description doesn't make it clear when you wrap $\theta$ - before calculating the error or after.

Also ensure $\mid\Delta_{theta}\mid < 180°$ so you don't do larger jumps (or, if large rotations in a time step are possible, have a heuristic to assume the direction is the same as last time if the steps were increasing in magnitude up to the wrap point). Eventually (depending on how many times you spin round and what precision you are working with) you'll have to deal with wind up and wrap the internal state of the filter; at that point you'll have to adjust the residual to allow for the step change.