# Extended Kalman Filter With Quaternions for Attitude Estimation

During my Master’s degree, I found a complete tutorial for the quaternion algebra. The authors explained the process of implementing a Kalman filter for attitude estimation with 6 degrees of freedom. In this post, I show an implementation in Matlab. I’ve also made a Python version of the code, so write to me if you want to have it!

Before we start, please note that the coordinate system of the quaternions used are following the left-hand rule instead of the regular right-hand rule system. Still, you should not have any huge problem using it. The only thing is that you will need to be extra careful when you’ll convert your data from and to quaternions.

Here’s the paper I used. If you want an annotated version, I’ve made one while I studied this paper.

Ok now, let’s start with the operations that are used in the filter when I manipulate the quaternions.

Here’s a function that gets a skew matrix from a quaternion:

and in order to clean up the code, here’s a one liner that invert the quaternion in the current convention.

Also, according to the quaternion convention, here’s how the paper said we could convert a quaternion into a rotation matrix. Unfortunately, it’s a left-hand-rule rotation matrix.

Because of this fact, I tend to use axis-angle conversions instead of the above function. My program needed right-hand-rule matrix, so I did not make this function work.

Here’s how to do the conversion of a quaternion from and to an axis-angle rotation.

That last code snippet called a function called NormalizeV. It’s a simple shortcut that normalize any vector. It helps to increase the readability of the main code.

I also implemented a function for the matrix Omega, which appears in the product of a vector and a quaternion.

The next function allows the multiplication of two quaternions in the current convention.

And finally what you were looking for, the densely documented EKF filter using quaternions. Note that I added a lot of comments. Although Uncle Bob would probably be mad at me for this, I think they are useful. The numbers in the comments tell what steps in the paper I am implementing.

Enjoy and I hope it helps someone someday! Ask me if you need the code in Python.