I adopted and modified the Kalman filter algorithm that was developed by a group from autopilot.sourceforge.net. The algorithm is part of the autopilot onboard code package. All of the software is free; can be distributed and/or modified under the terms of the GNU General Public License as published by the Free Software foundation.

The graphs below illustrate how Kalman filter can be useful in fusing single-axis angular attitudes collected separately, one by an accelerometer unit consisting of two accelerometers and the other by a gyro unit. The accelerometer unit gives angular attitude by using atan2. The gyro unit uses basic Euler integration to calculate angle from angular rate. Ideally a jig would be required to provide an absolute reference angle at any given moment, but rather than building one, I managed to compare data from the two sources, accelerometer unit and gyro unit, with the Kalman filtering result.

The figure above is a response to a steady movement. Y-axis indicates the angle in degrees, and X-axis is the step count or sampling count from the microcontroller. The microcontroller was operated at its full capacity allowing the Euler Integration to produce higher quality results. It can be observed that both units reflect the angle quite similarly. The experiment was not long enough to catch the drift of the gyro but overall it did not seem to be interesting. So, I decided to shake it.

This figure shows an interesting result. I had the IMU board not only rotated radically but also shaken in order to generate lateral acceleration. The lateral acceleration introduces noise to the system so that by using accelerometers alone you cannot get a clue where about the angular attitude is. At the end of the shaking process the gyro reveals an offset to the angle provided by the accelerometer. In this case accelerometer data is more reliable, and the offset can be considered as an integration propagation error or a continuous drift in angle.

It is very interesting to see that Kalman filter is following well the patterns of the gyro in a very noisy environment, and when the environment calms down it pertains to the more accurate accelerometer data.