arduino-libraries / MadgwickAHRS

Arduino implementation of the MadgwickAHRS algorithm
465 stars 196 forks source link

Changes and drift in Yaw when varying Pitch/Roll #44

Open fornaroale opened 1 year ago

fornaroale commented 1 year ago

Hi, I'm using a MPU9255 with an ESP12. Currently I'm able to use this filter, obtaining good results with the following input data: filter.update(gx*RAD_TO_DEG, gy*RAD_TO_DEG, gz*RAD_TO_DEG, ax, ay, az, mx, my, mz); The problem is that, in this way, I get the equivalent of an aeroplane reference system, with z axis pointing down.

Following the axis scheme of the MPU9255 and deciding to set y axis to North, I've changed the previous call with the following one using the NED reference system: filter.update(gy*RAD_TO_DEG, gx*RAD_TO_DEG, -gz*RAD_TO_DEG, ay, ax, -az, mx, my, mz); In this way, the sensor is giving good values with correct Roll and Pitch data.

The problem is that, whenever I start tilting the sensor (ie. incrementing Pitch), the Yaw also changes a lot, and it takes a very long time (tens of seconds) to get back in the right "pose", making the sensor useless. Any idea of why this happens? It doesn't happen with the first call reported above, which works perfectly.

Gyroscope and magnetometer have been calibrated, obtaining the results depicted in the following image (mag).

postcalibrazione_con_filtro

I also noticed that, whenever I rotate the sensor on a flat surface, the yaw is not the only value varying, but also roll/pitch. Is this normal?

Gyro and acc are running at 200Hz, mag (AK8963) at 100Hz and I've passed 200Hz to filter.begin() (I tried also with 25, 100 and 1000 values, getting equivalent results). Tried with Mahony filter, and a second IMU unit (Waveshare IMU 10DOF V2-C ), but got the same results! Data from MPU is retrieved using Bolderflight library.

Thank you.