1
$\begingroup$

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.

$\endgroup$
1
$\begingroup$

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.

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