# Kalman-Filter: how to solve angles near +/-pi?

I'm trying to get into Kalman filters. I've noticed an issue with Euler angles near -180°/180° (or -pi/pi) and wonder how to correctly resolve this. Its often said you need to normalize the angles into this range. However, this isn't as easy as it seems at first sight. Especially, when using a Kalman filter class from a library (e.g. OpenCV).

Let's take a very simple examle. We have only a compass sensor, which will give us a heading in the XY-plane. And our state keeps track of the current heading and its first derivative (speed of change).

## Matrix Algebra

First, I'll give all information in the typical matrix form to show how this actually relates to the Kalman filter. Later I'll reduce this to simple formulas for better reasoning.

We define our state space vector as:

$$x_{k} = \begin{bmatrix} \alpha \\ \dot{\alpha} \end{bmatrix}$$

Where $$\alpha$$ is the current heading in degrees (for simplicity) and $$\dot{\alpha}$$ is the current change of the heading in degrees per second.

The new a priori state estimate (prediction) is calculated by:

$$\hat{x}_{k|k+1} = F_k x_k$$ where $$F_k = \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix}$$

Our observations look like this:

$$z_k = \begin{bmatrix} \alpha_{new} \end{bmatrix}$$

And the correction / update step looks like this:

$$\tilde{y}_k = z_k - H_k \hat{x}_{k|k+1}$$ where $$H_k = \begin{bmatrix} 1 & 0\end{bmatrix}$$

Later the Kalman gain $$K_k$$ is calculated and influences the state estimate like this:

$$\hat{x}_{k+1} = \hat{x}_{k|k+1} + K_k \tilde{y}_k$$

### Simplifications

$$\hat{x}_{k|k+1} = \begin{bmatrix} \alpha + \Delta t \dot{\alpha} \\ \dot{\alpha}\end{bmatrix}$$

$$\tilde{y}_k = \alpha_{new} - (\alpha + \Delta t \dot{\alpha})$$

$$\hat{x}_{k+1} = \hat{x}_{k|k+1} + K_k \tilde{y}_k$$

Now lets only focus only on the estimation of $$\alpha$$:

$$\alpha_{k|k+1} = \alpha_k + \Delta t \dot{\alpha}_k$$

$$\alpha_{k+1} = \alpha_{k|k+1} + K_k^1 (a_{new} - \alpha_{k|k+1})$$

### The Issue and Semi-Solution

For simplicity I'll use angles in degrees. Let's assume, we've got the following situation:

$$\alpha_k = 160°$$, $$\dot{\alpha}_k = 100°/s$$, $$\Delta t = 0.1 s$$, $$a_{new} = -170°$$ ($$\equiv$$ 190°) and $$K_k^1 = 0.1$$.

Hence, $$\alpha_{k|k+1} = 160° + 0.1 * 100° = 170°$$ and therefore $$a_{new} - \alpha_{k|k+1} = -170° - 170° = -340° \equiv 20°$$.

The updated estimate would therefore (incorrect estimate) be:

$$\alpha_{k+1} = 170° + 0.1 * (-340°) = 170° - 34° = 136°$$

If we normalize $$\alpha_{k|k+1}$$ to [-180°, 180°] we get the equivalent 20° and the result is (correctly estimated):

$$\alpha_{k+1} = 170° + 0.1 * 20° = 170° + 2° = 172°$$

The normalization would fix the issue. However, if I use a normal Kalman filter class, I usually cannot influence how the innovation $$\tilde{z}_k$$ is calculated and the filter class cannot know that this is an Euler angle that needs special handling. Thus, I would need to write it my own... Or is there any way around this? A quick solution might be to use a unit vector $$\begin{bmatrix} cos(\alpha) \\ sin(\alpha) \end{bmatrix}$$ instead of the euler angle $$\alpha$$, which I would have to renormalize every now and then. I know that one could maybe use quaternions for this, but I currently can't grasp the math behind it to use it. Also I saw someone argue that it would be a bad idea to use quaternions in the state space of a Kalman filter (e.g. see comments to this question). This could also apply to my unit vector approach. Hence, I would like you to discuss the issue and give me some clues on how this is usually resolved.