## IMU Complementary filter

Hi

I'm trying to implement a complimentary filter on my IMU for pitch, roll and yaw. The code I have so far is:

#### CompFilter

```void ComplementaryFilter(float *d, float *comp)
{
#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536

#define M_PI 3.14159265359

#define dt 0.01                                             // 10 ms sample rate!

float pitchAcc, rollAcc;

// Integrate the gyroscope data -> int(angularSpeed) = angle
*(comp+0) += (d[0] / GYROSCOPE_SENSITIVITY) * dt;      // Angle around the X-axis
*(comp+1) -= (d[1] / GYROSCOPE_SENSITIVITY) * dt;      // Angle around the Y-axis

// Compensate for drift with accelerometer data if in accepted range
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192    //Change so sensitivity of +/- 4G
int forceMagnitudeApprox = abs(d[3]) + abs(d[4]) + abs(d[5]);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f(d[4], d[5]) * 180 / M_PI;
*(comp+0) = *(comp+0) * 0.98 + pitchAcc * 0.02;

// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f(d[3], d[5]) * 180 / M_PI;
*(comp+1) = *(comp+1) * 0.98 + rollAcc * 0.02;
}
}
```

Code taken from: http://www.pieter-jan.com/node/11 Then modified to have my IMU gyro data: d[0-2], gryo: d[3-5] and comp[] as the output.

My question is how do I modify this to include yaw. I've been reading up and have found a website saying this means problems such as gimbal lock and Quaternions, but that's all I can find.

Thanks Any help is appreciated, even a website I haven't found would be good

Also as I'm passing in a pointer to d[], where I've put d[] in the equations, I think I should be using this form *(d+0). However I get an error when I do..?

posted by Tom A 25 Feb 2014

Hi, did you find the answer?

posted by Alexis MASLYCZYK 02 Sep 2015