Account for gyroscope drift

[nixpulvis]

To get around the gyroscope drift we will incorporate the on board accelerometer.

Great info here.

[dcalacci]

I've spent time researching and developing a kalman filter model. Working-copy documentation is in the docs/algorithms pull request.

[nixpulvis]

Start looking at imu_visualization that is where we want to imprint this first. Inside the IMU class.

[dcalacci]

If you're ready now, don't hesitate to use code from somewhere else. we can always change the stabilization algorithms as we move forward - that may mean using a complimentary filter now, and a kalman filter later.

[nixpulvis]

I was thinking of hacking together a comp filter just to see it work. All the code for filtering this data will need to be rewritten in C on the arduino after we prove it works in ruby.

On Jun 13, 2013, at 12:37 PM, Dan Calacci notifications@github.com wrote:

If you're ready now, don't hesitate to use code from somewhere else. we can always change the stabilization algorithms as we move forward - that may mean using a complimentary filter now, and a kalman filter later.

— Reply to this email directly or view it on GitHub.

[dcalacci]

Here's a really simple one:

• filter_angle is the angle calculated by the filter
• dt is the time interval between readings, in seconds (sample period)
• tau is the relative duration of signals the filter will act on. If it's a low-pass filter, for example, signals significantly longer than tau are not altered.
• angle is the angle measured by the accelerometer
• rate is the gyroscope measurement
• a = tau / (tau + dt) (the filter constant)
• c_angle is the previously calculated/filtered angle.

tau = <some small value close to 1, like .085< tau < .099 >
def comp_filter(angle, rate, dt)
  a = tau/(tau+dt)
  return a * (c_angle + rate * dt) + (1-a) * angle

c_angle = (c_angle + rate * dt) calculates the new angle position, as calculated by the angular speed measured by the gyroscope. It can be seen as a parallel to position = position + speed * dt for a non-angular calculation.

From what i've read, a (the filter constant) essentially serves as a confidence measure for the gyroscope and accelerometer. If a = 0.98, for example, then the filter would look like this:

0.96 * (c_angle + rate*dt) + (0.04)*angle

where 96% of the "confidence" lies in the integrated gyroscope reading (c_angle + rate * dt), and 4% lies in the angle reading from the accelerometer ((0.04)*angle). It's called a complementary filter because the first part of the filter, 0.96 * (c_angle + rate * dt), acts as a high-pass filter, and the second part, (1-0.96) * angle acts as a low-pass filter.

We then tweak tau or a depending on how our actual rotors react, since they're dependent on one another:

tau = (a*dt)/(1-a), and a = tau/(tau+dt)

So, both of these constants are not set in stone. If we had the filter I described above, where a = 0.96, and we wanted to take samples 100 times per second (dt = .01sec), tau would be:

tau = (0.96 * 0.01) / .04 = 0.24 seconds

It seems that tau represents a threshold for what signals to alter, and what signals to "pass" through the high and low-pass filters. I think this means that for the low-pass filter acting on the accelerometer readings, for example, readings that are constant over a time interval greather than 0.24 seconds will not be altered, but readings that are constant below that interval will be. The opposite would be true of a high-pass: readings that are constant for less than 0.24 seconds will not be altered.

It's obvious from the filter itself that no real filtering is being done - the function I defined above seems to approximate the behavior of a more complex filter.

Here are some good resources

[nixpulvis]

tau is the relative duration of signals the filter will act on. If it's a low-pass filter, for example, signals significantly longer thantau are not altered.

Can you explain this a little more?

[dcalacci]

Does the updated comment answer some of your questions?

[nixpulvis]

Yea, really good info. I'm skeptical about how well this style of filtering will do to account for drift properly.

[dcalacci]

so am I, but it looks like lots of other quadrocopter projects have successfully used it, so well see how it works out -dc

On Fri, Jun 14, 2013 at 11:46 AM, Nathan Lilienthal notifications@github.com wrote:

Yea, really good info. I'm skeptical about how well this style of filtering will do to account for drift properly.

Reply to this email directly or view it on GitHub: https://github.com/nixpulvis/quadrocopter/issues/9#issuecomment-19464939

[nixpulvis]

This document has a lot of data we need. Linking here for reference. http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf

[nixpulvis]

Reading through the MPU6050 library I found some really interesting functions.

mpu.dmpGetQuaternion
mpu.dmpGetYawPitchRoll
mpu.dmpGetEuler
mpu.dmpGetAccel
mpu.dmpGetGravity
mpu.dmpGetLinearAccel

My knowledge of 3D math is pretty basic, so I'm still having trouble getting these values to be mapped to an OpenGL rotation matrix. Maybe we can look at this together @dcalacci.