Robot heading uncertainty values

Question

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]$?

Answer 1

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.

Answer 2

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.