IMU Filtering

The next task was to take the noisy raw acceleration and drifting angular rate outputs, and combine them into a noiseless non-drifting signal, ideally with no phase shift. The first approach was to implement a kalman filter, a complex cycle eating process. However despite hours of tuning, I struggled to fully eliminate the accelerometer noise whilst still keeping the phase shift to a minimum. This may just be down to me misunderstanding the filters operation, but given how much cpu time it demanded a simpler solution was required. After a brief moment in thought I came up with the following equation: ANGLE=(PREVIOUS_ANGLE+GYRO_RATE*(dt*57.295))*0.99 + ACCEL_ANGLE*0.01. It simply functions by giving the filters a 1:99 weighting, which appears to be just enough for the accelerometer to cancel out the gyroscopes drift:














The system had been allowed to run for a few minutes before this screenshot to allow the gyro drift to build up (red). The blue trace shows the noisy accelerometer reading, and the green is the filtered value.

I have no idea if this type of filter has been used before, but to me it very elegantly utilizes the gyroscopes accuracy and the accelerometers lack of drift, without introducing any negative effects.


Comments (7) Trackbacks (0)
  1. Where is the green line?

    I do the same thing to correct for gyro drift on my tricopter, with a variable accelerometer weight that is a function of the measured magnitude of acceleration’s deviation away from 1 g.

    Does that graph show gyro/accelerometer readings while your quadrotor is hovering, or did you leave it on a tabletop? I have been having a hard time isolating my sensors from vibrations, which causes my PD-PD controller to react rather violently. Even the gyro seems affected by these vibrations. How have you mounted your sensors?

    Your latest flight video looks great—good luck!

    • It’s hard to see, but the green line more or less follows the blue accelerometer angle, but with the noiseless shape of the gyroscope. These readings were taken on a table, then using my phone I introduced some vibrations towards the end of the graph.
      What sensors are you using? Gyroscopes have a linear acceleration sensitivity property, which can vary a lot between models. The MPU6050 I’m using has this rated at 0.1ยบ/s/g, meaning even with the motors running there is basically no noise on the gyroscope output. However if your gyroscopes aren’t rated as good, you will pick up acceleration noise on them, something you will just have to filter out.
      As for mounting, up to now the board was simply screwed into the frame with nylon spacers, but I have since acquired some of these silicon mounting pins, which I hope will further reduce the accelerometer noise

  2. Hi, great job!, Is it possible view the code?

    • Sure, give me your email and il send it over

      • Hi, thanks, I’m also building the quadricopter, I am beginning.
        I use the chrobotics sensor (UM6-LT) has EKF at 300Hz update.
        do not know if sensor is okay for quadricopter, I do test it.
        Now I’m developing a simple telemetry for tuning PID, (more difficult) because I had a hard experience with the balancing robot pid tuning.
        My email is

  3. Hi, that is a great project. I am also using a MPU-6050 and a dsPIC33 but model dsPIC33FJ64MC802 to control servo motors for a bio-mechanical/exoskeleton project of mine. After reading your posts it sounds like you had some fun with the I2C driver and the MPU-6050 driver/initialization. All of my parts are in the mail but should arrive soon.

    If you have any code or information you would like to share that may help me get started that would be greatly appreciated. My email is Matthew.Carton@Colorado.EDU

    • Thanks, I must say yours sounds very intriguing, does this happen to be anything similar to an iron man suit?
      I’ve got the full source for the quad listed here You should find the mpu6050 files useful, plus the I2C driver, although that is heavily based off a standard microchip example. I’ve also got a brief tutorial on using the mpu6050 listed in the top menu of this site, which should explain what my code does a little better. The only other quirk I can remember is that if you are too quick to try to write to the chip after power up it will sometimes not implement write commands, so I run the configuration/error check functions in a loop until no error is produced (it normally only takes two attempts).

      If you get stuck with anything later on feel free to email me at, I’m normally pretty fast to respond, and good luck!

Leave a comment

No trackbacks yet.