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

Question

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.

Answer 1

Here is a paper on the subject. By doing the proper maths with the SO(2) Lie group to represent the angle, they show that the very simple approach of wrapping the predicted state angle to $[-\pi, \pi)$ with $$ \mod(\alpha + \pi, 2 \pi)-\pi $$ is equivalent, so you don't have to worry about Lie algebras.

Answer 2

This is an interesting question and I ran into the same issue. First, let me mention that there are libraries that let you change the residual function (i.e., innovation). For instance, see the residual argument of the update function of the Extended Kalman Filter implementation of filterpy. However, I understand you may be stuck with a given library that does not let you change how the residual is calculated.

You were thinking about substituting the state space variable $\alpha$ by $[\cos(\alpha), \sin(\alpha)]$, which certainly would solve the issue. I went for another solution approach that modifies the sensor values before they get fed into the Kalman filter, i.e., it addresses the problem at the equation $\alpha_\text{new} = -170° \equiv 190°$. The idea is that if your sensor would report $190°$ rather than $-170°$, the calculation of the residual would give the correct value of $20°$. Therefore, I define a rotation counter variable $r=0$ that gets incremented/decremented each time $\alpha^{\text{s}}$ jumps across the discontinuity (the superscript s indicates that this is the raw sensor value). The jumps are detected via the conditions $\alpha_{t+1}^{\text{s}} - \alpha_t^{\text{s}} > \Delta$ and $\alpha_{t+1}^{\text{s}} - \alpha_t^{\text{s}} < -\Delta$. (I have set $\Delta = \pi$, but $\Delta$ could also be set closer to $2\pi$.) This, of course, requires that your compass needle does not make more than half a rotation between two subsequent measurements - but I assume that your measurement frequency is high enough (in my case it certainly was). The modified angle (superscript m) that gets fed into the Kalman filter is then $\alpha^{\text{m}} = \alpha^{\text{s}} + r \cdot 2\pi$. The Kalman filter output (state estimate/prediction) can be transformed back to $[-\pi,\pi)$ if you like.

Some python pseudo code:

alphaSOld = np.nan
r = 0

while doMeasure:
    alphaS = getAngle() # returns angle in range [-pi, pi)

    if not np.isnan(alphaSOld):
        if alphaS - alphaSOld < -math.pi:
            r += 1
        elif alphaS - alphaSOld > math.pi:
            r -= 1

    alphaM = alphaS + r * 2 * math.pi
    kf.update(alphaM)

    alphaSOld = alphaS

Answer 3

In my case, I generally write my own Kalman Filter stuff. My very first solution for this problem would be straight forward. I would put the angles at the desired interval before/after any angle update operation. In the case this not perform accordingly to the specifications I would recur to some mathmagic, like unit vector as you did. In this case it is up to you to establish the frequency of reorthogonalization. With 3D-roation matrices I use 1000 iterations for a 64bit computer.

Another solution would be in code. Can't you simply inherit the kalman filter class and alter only the method performing the update/inovation step?

And also, there is Lie Algebra and the Twist coordinates. Very helpful when comes to specifying 3D movement.