7 years, 5 months ago.

IMU Complementary filter


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


void ComplementaryFilter(float *d, float *comp)
#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

1 Answer

7 years ago.

Hello, can you explain me the d[] ? what is is ? i have no clue..

d in this case is an array of values returned from the IMU. d[0-2] is the accelerometer, d[3-5] is the gyroscope and d[3-8] is the magnetometer. The array is passed in as a pointer to this funtion

posted by Tom A 23 Jul 2014