Hello and welcome everyone!

Some time ago influenced by DJI Phantom I started to build my own Quadcopter.
To understand the basic principles of aerophysics and RC I started to build my own FlightController based on Arduino Mega 2560 r3, Pololu AltIMU v5 and FlySky iA6 Receiver.

Working with RC Receiver was quite a good feeling, it wasn't that hard to understand.

However, working with the gyro, accelerometer and magnetometer is painful.

I surfed the web for about two weeks and didn't found anything to solve the problem that I came across recently.

I started to write an IMU code for AltIMU, it's looking like so: http://pastebin.com/rPEjMM9y

In fact, when I start the Arduino, launch the Serial Mon, angles are OK, but after some period they started to drift dramatically.
For the first time I though that it's connected with the resolution of gyro, but after changing it appropriately, it still exists.  
I also looked on the AHRS from Pololu, but the time consumption for all the calculation is probably too long for the quadcopter to maintain the stable flight. (It's meant for the 50Hz, but after watching whole YMFC series I thought that 250Hz is rather the minimum).

I'm stuck and I really don't know what to do now.

Kind regards.

Views: 521

Reply to This

Replies to This Discussion

This pdf explains much of the math, including gyro drift



Thank you very much for this well of information.

I've tried to use Kalman Filter with the following code:
Angle, which was then produced, wasn't really good (on spirit level it shows pitch correctly but the roll was messy and after a while, both angles goes like crazy).

Next, I wanted to use simple Complimentary Filter with that code:

but after any movement, it looks like no drift has been corrected.

Did I do something wrong with math or LSM6 is too sensitive?
Any help appreciated.

Used Kalman filter algorithm: http://pastebin.com/QDht6RaW in courtesy of Kristian Lauszus, TKJ Electronics

I have tried all these things and when it comes to noise there is a lot of it.

Did you try building the self balancing robot.  It's not as easy as it looks.  The math needed is the same math you need to balance a copter but it only has one direction.

The motors generate a lot of noise that the gyro sees and reports back as movement.  Whether you use Complimentary Filter or Kalman that noise has to be dampened.  Also the Accelerometer doesn't like vibrations either.

Most copter code use Quaternions to balance the copter.  This calculation tries to represent the position of the object in 3 dimensions so that you can balance more effectively.  Using Euler angles the adjustment in those three direction can be determined.

One would have you believe that if you have a sensor that can give you the angle that it's just a simple mater to correct for that angle.  The real world has something to say about that.

Good luck as I went down your path and failed badly.


I did make my own radio transmitter and receiver and did connect it to a copter and it did get off the ground but it didn't really fly.

Thanks for your replay.

After lecture of most pitch,roll,yaw I managed to write rather simple fc (base on brokking complimentary filter) but it still contains huge drift. I will try to add some of gathered date to ilustrate that problem.

Data is made in this way:

angle_pitch angle_roll


Let's have a look:

Used code:

Plus RadioController profiler which basically converts us to deg <-167.66 ; +167.66>

Are these formulas wrong?

Thanks in advance!

Gyro with low-pass filter could possibly reduce some of the shaking noise. I also use rubber pods under motors in addition to balancing them works quite well. :) Give it a try!

My experience has been similar Evorowy.
Accelerometers and magnetometers are extremely noisy and gyros drift.
That's just a fact with any of those that isn't thousands of dollars each.
There's also plenty resources are ways to combine these pieces to form something useful.
That doesn't mean they don't still have noise though. 
Because the input data isn't perfect the essential filtering method is to combine the good aspects of the individual measurements, i.e. Kalman or complementary filter. 
Beyond that, there's only filters that balance between a responsive or a smooth output, these all filters over time i.e. a running average.
Before spending too much time on it, be sure the amplitude of the noise or drift your seeing is actually consequential.
There's a really awesome rundown of the behavior of acc, gyro, mag here: https://youtu.be/C7JQ7Rpwn2k

I think you're using a raw gyro and accelerometer, these things are not accurate and have a lot of drift, to solve the drift problem you will need some sort of anchor, like a megnetometer, which is very slow but relatively accurate.

In MPU6050 there's a built in magnetometer, gyro and accelerometer AND a DSP on top of that, the code for the DSP is proprietary and therefore not available as open source, the way to combine inputs from multiple sensors and derive the output is called sensor fusion, which is probably what you're missing.

No amount of noise filtering will fix the drift problem for you due to the way MEMS sensors work.

You can even see the way these drifts are being corrected by the DSP if you move a quadcopter in a certain way, the accelerometer, gyro and magnetometer will get out of alignment and the quadcopter will be unstable for a second or two.

If you're interested to try a different IMU, try the MPU6050 or MPU9150, both have great performance and some flight controllers use them already and you can get sensor fusion done in hardware by the DSP.

Take a look at these libraries:



A couple of tips:

  Gyro integration speed is critical to stability (400Hz is reasonable for an 8bit micro).  Drift correction can happen at a slower rate to save computation time (~100Hz).

  You have to translate Mag, and Accel readings from sensor reference frame to world reference frame before computing corrections.

  You have way too many trig functions, they cost too much computation time.  Everything in a complemntary filter for IMU can be done with vectors and not trigfunctions.  The units for accelerometers should be G's and the units for gyro should be radians/s.  Magnetometer units are special and should be turned to a unit vector after magnetic inclination is eliminated.

  Avoid square root functions as well.  I needed only one in my IMU and it is in a 50Hz loop.  Approximations can be used for most cases.

Good Luck,


Reply to Discussion


© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service