I am building a quadcopter using the Arduino Uno with a 6dof accelerometer and gyro. I will be adding a separate 3 axis magnetometer soon for heading. I have successfully implemented the code that reads the data coming in from these sensors and prints them out.

I have also checked for bias by averaging 100 measurements. My code calculates the pitch from the accel and pitch from the gyro respectively:

pitchAccel = atan2((accelResult[1] - biasAccelY) / 256, (accelResult[2] - biasAccelZ) / 256)*180.0 / (PI);

pitchGyro +=((gyroResult[0] - biasGyroX) / 14.375)*dt;

I am then using a complementary filter to fuse the two readings together like this:

pitchComp = (0.98*pitchGyro) + (pitchAccel*0.02);

I am stuck on how to proceed from here. I am using the same procedure for roll, so I now have readings for pitch and roll from their respective complementary filter outputs.

I have read a lot of articles on the DCM algorithm which relates the angles from the body reference frame to the earth reference frame. Should that be my next step here? Taking the pitch and roll readings in the body reference frame and transforming them to the earth reference frame? Repeat the entire procedure for yaw using the magnetometer? If yes, how should I go about doing the transformations?

I understand the math behind it, but I am having a hard time understanding the actual implementation of the DCM algorithm code-wise.

Any help is appreciated!


4 Answers 4


I will shamelessly steal content from another anwser I gave:

  • First determine orientation based on information from both the gyroscope and accelerometer. The accelerometer is accurate in the long term while the gyroscope is accurate in the short term, so you will need some kind of sensor fusion algorithm to determine the"true" orientation of the quad. The most popular one is the Kalman Filter but many others are available to choose from.

You have already done this, I would suggest though looking into more sophisticated estimation techniques like the Kalman filter to improve performance

  • The next thing you will want to do is build a simple rate controller where you try to match the rate of rotation on a certain axis to the rate commanded by the controller

This should probably be your next step

  • after you have a rate controller you can build a simple stabilized controller, where instead of commanding the rate you command a certain angle. You take the desired angle and the current angle and based on those values calculate a desired rate to close that gap and then feed that into the rate controller.

  • You will probably need a RC controller and receiver set, you will need to read the PWM values from the receiver in order to take command input

Here is my favorite resource on the matter it helped me when I was making my first flight controller from scratch.

Good luck!

  • $\begingroup$ So essentially I can move on to the control part in terms of my algorithm design since I have the fused values for my orientation? I am confused because I always thought that the values would have to be transformed to an inertial frame since the IMU readings are in the UAV body frame. $\endgroup$
    – anomaly23
    Commented May 1, 2015 at 21:53
  • $\begingroup$ Yep its all tightly linked, do you have a test paltform? $\endgroup$
    – Mark Omo
    Commented May 1, 2015 at 21:54
  • $\begingroup$ A test platform as in some sort of simulation? $\endgroup$
    – anomaly23
    Commented May 1, 2015 at 21:55
  • $\begingroup$ Depends are yiu writing for a simulation now? Or some kind of HIL? Or do you have a quadrocopter? I assume you have at least an arduino and the sensor boards. $\endgroup$
    – Mark Omo
    Commented May 1, 2015 at 21:59
  • $\begingroup$ Yes, I do have an arduino and the IMU. I've mainly been playing around with those for now. Going back to my first comment, could you clarify the DCM algorithm part? Shouldn't IMU readings be transformed to an inertial frame? $\endgroup$
    – anomaly23
    Commented May 1, 2015 at 22:02

First the answer to the main question. The readings are the pitch and roll of your sensor, at the inertial frame. As your sensor is certainly not moving in your aircraft in time, you do not have to re-convert into any other frames. The readings are the aircraft pitch-roll-yaw data.

Coming to the Pitch computation from accelerations. I don't know if this method works for aircraft that can rapidly move. In a reference of a similar application , it is explained that the accelerometer is assumed NOT TO BE UNDER LINEAR ACCELERATION. The aircraft might experience some turbulence, without pitching, and the pitch_accel calculation would calculate some pitch, which would not be real. Another short note is to use the atan2 function, to arrive at realistic angles of pitch and roll.


I personally found very useful the procedure described at the following link, in order to have a total 3D estimation, without suffering any gimbal lock or other problems.


Inertial Navigation Systems (INSs) with 9DOF IMUs allow you to extract not only pitch and roll values but also yaw and most importantly are able to do so under sustained turns, where Coriolis acceleration is not negligible.

However, you'll have to completely overhawl your algorithm, instead of building it up incrementally.

DCM, as you said, is a traditional method. Ardupilot was using it until recently so the code is still in the repo and its initial author has written some good background reports. Start here and follow the relative links and publications of the author.

Another spinnoff of the Mahoney filter is the Madgwick filter which also works well and I have personally encoded it into arduino successfully, albeit for a plane. Here is the relevant report. You'll find more resources online.

Finally, Extended Kalman Filters are also making a comeback, thanks to cheaper, smaller more powerful computers. Ardupilot is using it lately as its main filter. However, EKF filters are usually tailor-made and I can't point to you to some ready-to-go code solution. I'm afraid you'll have to do a bit of searching. As a background read, this one seems fine.


Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.