Interactive web site of Télécom ParisTech's ELECINF344/ELECINF381 Robotics and Embedded Systems classes (a.k.a. ROSE, 2012 session).


A complementary filter for a IMU

I recently talked about getting informations from the FreeIMU board. This gives us raw data from acceleroscope and gyroscope. Both of them can give some informations about the attitude (pitch, roll) of the board, but they both have some flaws, even if they are perfect sensors (which they aren’t):

The gyroscope only gives the rotation speed, so the software has to integrates the value, which means there will most likely have integration errors, and thus a drift.
The accelerometer gives the acceleration + g, so when the device is not moving, we get g, which gives us pitch and roll. But when there is some acceleration, the values can’t be used.
The gyroscope’s drift is mostly slow, whereas there is an acceleration only on short period of times, so we can combine both of those sensors to get the actual attitude.

One usual way to merge them is to use a Kalman filter. It is a very effective way of merging, and the result can be quite incredible. But it is really complicated to understand and implement. The theory behind it assumes the knowledge of physics of the problem, which doesn’t help its complexity. It actually works without this knowledge, so it’s still a possible solution.

Anyway, as I mentioned before, the noise from the gyroscope and from the accelerometer is not a white noise, but has some specific spectrum, and gyroscope and accelerometer’s noises have quite a complementary spectrum, which leads to another possible filter to merge gyroscope and accelerometer informations (it can also filter magnetometer informations actually): It’s called a complementary filter.

The idea is to apply a low-pass filter on accelerometer, since its values are correct only at low frequencies, and add the complementary (ie the sum of the high-pass filter’s transfer function and the one of the low-pass filter is equal to one) on gyroscope’s data. That way, we filter out high frequency noise of the accelerometer, and low frequency noise of the gyroscope.

So, that’s the theory. To implement such a thing, an easy way we’ve chosen is to use an exponential filter: v_{n+1} = v_n*t + x_n*(1-t), where t is a number between 0 and 1. I’ve chosen t = 10/11 for a sensor frequency of 60Hz. The complementary filter is w_n = x_n-v_{n+1}.

Again, to have as simple code as possible, the current implementation doesn’t even use a FQA, which obviously leads to problems on some specific angles. For our bot, this is no problem, since the range of possible pitch and roll are short, we can take care of being in a safe area.

Today, we also got the Kalman filter to work, so we could compare both approaches. Even thoguh Kalman is still working better, the complementary filter is working properly, and should be enough to control the bot properly, at a way lower cpu processing cost.

Pierre-Hugues Husson

BallE Kalman

During the last two weeks, the Kalman filter for BallE has evolved. We now have the mathematical model as well as the Matlab implementation.  3 crucial questions were raised before reaching this point.

System and measurement equations:

x_k= F_(k-1)*x_(k-1) + G_(k-1)*u_(k-1) + w_(k-1)

y_k= H_k*x_k+v_k

where v_k and w_k are the measurements’ noise and the process’ noise respectively

X_k describes the state of the system at moment k, Y_k the measurements, F_k the transition matrix, H_k the measurement matrix, u_k the command (in our case we consider we don’t have any command applied for the time being)


1) What type of Kalman filter should we use?

After a literature review and the study of the possible filters, I decided that the Sequential Kalman Filter (SKF) was the most appropriate for our problem: SKF does not require matrix inversion for computing the gain matrix (K). However, in order to use this type of filter we must be sure that the measurement covariance is a diagonal matrix.

2) Knowing that we obtain data from the accelerometer, gyroscope and magnetometer, how can we model the state? How should we deal with the gyroscope’s drift?

To answer this question, I first analyzed filters with 6DOF where the state is defined by X_k= [alpha bias]k (as presented  here: http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data ). However, as we have the magnetometer we can use only the accelerometer and magnetometer to compute the Euler angles (Y_k = [roll pitch yaw]k ). We can put as well the bias of the gyroscope in X_k but what exactly should define the state of the system?

Starting on this path, we searched more information about the approach used by Copterix last year (http://copterix.perso.rezel.net/?page_id=26). After studying “Airborne attitude estimation using a Kalman filter”, the Master thesis of Matthieu Marmion(1), Richard Murray’s lectures on Sensor Fusion and Factorized Quaternion Algorithm, we used the following model:

X_k=[bias quaternion]k

Y_k=[roll pitch yaw]k

F_k the same as the one presented in (1)

H_k the Jacobian of the Euler angles in fonction of the quaternion

3) Do we really need to compute H_k or can we find a simpler form for it?

We know the conversions for Euler to quaternion/ quaternion to Euler, therefore we can write directly the Euler angles in fonction of the X_k. However, we will further need H_k for the filter (for computing the gain matrix and the a posteriori state estimate and a posteriori state covariance). I computed the Jacobian and implemented the whole approach in Matlab.

After testing it with real data obtained from our IMU, the filter didn’t give satisfactory results…Once I finished my code and after calibration (measurement noise, initialization, time step) and after performing the tests I tried to understand where the problem might be. Yesterday I looked at the Copterix implementation and noticed that the set Y_k= [quaternion]. This simplifies enormously H_k. If we take Y_k=[quaternion]k we obtain H_k=[O4x3 I4]. However I wonder if the measurement noise covariance is still diagonal in this case.

Therefore, after today’s presentations I did the necessary changes to my code to work with H_k=[O4x3 I4]. The results are better but the filter doesn’t seem to work properly yet…

During the holiday I will do my best to find out what is the problem and to fix it. If you have any suggestions with what may be wrong in this approach please let me know:D


Otilia Anton