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:

Complementary filter: fast but a bit noisy

Madgwick filter

Madgwick filter with high kp value


Different values probably are caused by cable plug and unplug. Anyway raw data example from my sensor can be downloaded here

  • $\begingroup$ Can you post some figures/plots/picture of the Madgwick implementaion-results. I do not know the paper/implementation but the inputs to the system look quite fine, internally the implementation normalizes them anyway (the Matlab code does, haven't looked at the C version). Do you have tuned the filter? There should be two gains to tune for the MARG-implementation $\endgroup$
    – TobiasK
    Commented Jun 25, 2015 at 6:10
  • $\begingroup$ Hello, I will prepare some plots with complementary data and madgwick data in the afternoon. Anyway i tried changing Ki and Kp with lower values (it should give slower convergence I think) but it remains unstable. $\endgroup$ Commented Jun 25, 2015 at 8:58
  • $\begingroup$ If you are able to provide raw data, this would be very appreciated $\endgroup$
    – TobiasK
    Commented Jun 25, 2015 at 10:04
  • $\begingroup$ Well, I found the main error gathering raw data from matlab: the sampling time was badly redefined in a function and, after rewriting that function, it starts to work with a very slow convergence speed. So now I'm trying to tune PID. Thank you for help! $\endgroup$ Commented Jun 26, 2015 at 9:19
  • $\begingroup$ Added graphs and data, I can't get reasonable speed with madgwick filter $\endgroup$ Commented Jun 27, 2015 at 8:57


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.