Madgwick algorithm unstable

  c++, imu, math, physics

I’am working with Madgwick algorithm. So I’ve a LSM6DSO32 sensor set at 208Hz data rate, I also set my Madgwick filter at 208Hz sample rate. But I have a delay between real angle and quaternion. I’ve found if I change betaDefdelay is reduced but Quaternion is unstable, how can I achieve a real time Quaternion ? What I’am doing wrong?

I used this lib here

And I’ve changed these lines

#define sampleFreqDef   200.0f          // sample frequency in Hz
#define betaDef         8.0f            // 2 * proportional gain

I set my captor like this (I use Adafruit lib here)

dso32.setAccelDataRate(LSM6DS_RATE_208_HZ);
dso32.setAccelRange(LSM6DSO32_ACCEL_RANGE_32_G);
dso32.setGyroDataRate(LSM6DS_RATE_208_HZ);
dso32.setGyroRange(LSM6DS_GYRO_RANGE_2000_DPS);

With this setup I’ve good result but unstable Quaternion. And I call in my main updateIMU each time my captor provide new values

Source: Windows Questions C++

LEAVE A COMMENT