I recently decided to build a quadricopter from scratch using Arduino and now I'm faced with an orientation estimation problem.
I bought a cheap 10DOF sensor with 3 axis magnetometer, 3 axis accelerometer, 3 axis gyro and a barometer and the complementary filter that I use to get orientation returns usable but noisy values.
I tried the Madgwick fusion filter too, but it returns unstable values that diverges from the ones I get with complementary filter. Given that the Madgwick filter implementation is correct, I pass acceleration values measured in Gs, gyro values measured in rps (radians per second) and Magnetometer values measured in uT, while sampling time is the same of my loop cycle. Is there anything I have missed?
Is there any advantage using Kalman filter?
My problem was due to an wrong choice of sampling time and now seems to work, but convergence is very very slow (i.e. it takes about 3 seconds to reach the right value after a quick flip of the IMU). Rising value of Kp adds to much noise. I also tried to repeat filter update step more than once per cycle but it requires too much time exceeding the sampling time.
Here some graphs, from top to bottom Complementary filter, Madgwick filter and Madgwick filter with high Kp:
Different values probably are caused by cable plug and unplug. Anyway raw data example from my sensor can be downloaded here