As far as i understand, the complimentary filter takes two inputs (one thats noisy and one that drifts) and uses an alpha variable (>0 & <1) to combine these two inputs. Usually this alpha variable is very low in the world of IMU's, like 0.02 or lower. Supposedly what this does is prioritize integrating a gyroscope, and use an accelerometer to "correct" for the bias in the gyro. The higher the alpha value, the more the estimation relies on the accelerometer for correcting the gyro.
However my code doesn't seem to do that. any alpha value less than 1 results in zero correction from the accelerometer. Is there something wrong with my understanding?
#include <Adafruit_FXAS21002C.h>
#include <Adafruit_FXOS8700.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h>
Adafruit_FXAS21002C gyro = Adafruit_FXAS21002C(0x0021002C);
Adafruit_FXOS8700 accelmag = Adafruit_FXOS8700(0x8700A, 0x8700B);
sensor_t accel, mag; //declares sensor types?
sensor_t sensor;
double currentTime, timeDif, prevTime, rawaccelX;
float rollAccelRad, rollEstimated;
int alpha = 0.9; // relativly exremely high but to no avail
void setup() {
// put your setup code here, to run once:
gyro.getSensor(&sensor);
accelmag.getSensor(&accel, &mag);
Serial.begin(9600);
if (!gyro.begin()) {
Serial.println("No gyro detected");
while (1)
;
}
if (!accelmag.begin()) {
Serial.println("No accelmag detected");
while (1)
;
}
}
void loop() {
// put your main code here, to run repeatedly:
sensors_event_t gevent, aevent, mevent;
gyro.getEvent(&gevent);
accelmag.getEvent(&aevent, &mevent);
rawaccelX = (aevent.acceleration.y / 9.8); //prepares the accelerometer data
currentTime = millis();
timeDif = (currentTime - prevTime) / 1000;
rollAccelRad = asin(rawaccelX); //converts acceleration to radians
//heres where things go wrong
rollEstimated = (rollAccelRad * alpha) + (1 - alpha) * (rollEstimated + timeDif * (gevent.gyro.x-0.004)); //slight bias offset because im lazy but it works
prevTime = currentTime;
Serial.println("Comp estimate");
Serial.println(rollEstimated * 57296 / 1000);
Serial.println("accel");
Serial.println(rollAccelRad * 57296 / 1000);
delay(100);
}
Heres a youtube video timestamped with the equation i used : Complementary Filter - Sensor Fusion #2 - Phil's Lab #34 - YouTube
And here's an image of a block diagram with the same code:
Imgur: The magic of the Internet
PS the bias offset was determined with some trial and error by setting the alpha to 0 and finding which value produced the least amount of drift while sitting.
With an alpha of .9 and the IMU sitting still on my desk, the estimated value never approaches the measured accelerometer value. This makes no sense to me.
Thanks