Magnetometer totally ruins quaternion
Opened this issue · 13 comments
Hello,
I've encountered huge issue during (lengthy) development of my device. When Im running madgwick without magnetometer readings, the orientation quaternions is pretty accurate, however obviously prone to drift (not huge). However if I use magnetometer readings in sensor fusion, the quaternion goes totally wild. It looks like magnetometer readings completely override gyro/acc measurements, and somehow "forces" orientation (if I rotate module, it goes back to specific, somehow, predetermined orientation as if magnetometer overriden acc/gyro after some cycles). Axes of sensors are aligned correctly (im using ICM-20948 chip, and aligned axes according to documentation, and many other ICM-20948 repositories I've found). When not using magnetometer readings, calculated quaternion is correct, so it narrows everything down to magnetometer readings. I've even tried all possible combinations of X,Y,Z (including inverting signs of axes), without any success. Magnetometer is calibrated with "Magneto" app, and the results seems to be good:
Im struggling with this issue for days, and I thought maybe someone here could put me out of misery... I've also tested few different implementation of both Madgwick and Mahony filters, yet the same issue occurs. Am I missing something?
Thanks!
Poor accuracy usually results from poor calibration [...]
Picture I've sent in previous message presents a before/after of magnetometer calibration. "Before" a significant both hard and soft iron distortions can be observed, caused by mechanical (sadly, crucial) elements next to PCB board. After implementing a calibration proposed by app called "Magneto 1.2", the results seems to be ok (however final output seems to contradict it).
Example values and calibration process of magnetometer:
float biases[3];
float scaleFactorTop[3];
float scaleFactorMid[3];
float scaleFactorDown[3];
biases[0] = 380.369407f;
biases[1] = 262.456571f;
biases[2] = -819.497395f;
scaleFactorTop[0] = 1.246254f;
scaleFactorTop[1] = -0.036161f;
scaleFactorTop[2] = 0.213955f;
scaleFactorMid[0] = -0.036161f;
scaleFactorMid[1] = 1.306548f;
scaleFactorMid[2] = 0.074080f;
scaleFactorDown[0] = 0.213955f;
scaleFactorDown[1] = 0.074080f;
scaleFactorDown[2] = 0.998964f;
float Xm_off = magRawReadings.x - biases[0];
float Ym_off = magRawReadings.y - biases[1];
float Zm_off = magRawReadings.z - biases[2];
target->x = scaleFactorTop[0] * Xm_off + scaleFactorTop[1] * Ym_off + scaleFactorTop[2] * Zm_off;
target->y = scaleFactorMid[0] * Xm_off + scaleFactorMid[1] * Ym_off + scaleFactorMid[2] * Zm_off;
target->z = scaleFactorDown[0] * Xm_off + scaleFactorDown[1] * Ym_off + scaleFactorDown[2] * Zm_off;
target->x *= sensorConfig->magSensitivity;
target->y *= sensorConfig->magSensitivity;
target->z *= sensorConfig->magSensitivity;
Results after the calibrations seems to be pretty "round and centered", which would be a desired outcome. Madgwick inputs of magnetometer (at least in implementation used by me) are also normalized, so I'm pretty sure the sensitivity values could be omitted (however they are picked according to the documentation).
Poor accuracy usually results from [...] improper data feed to the Madgwick fusion filter [...]
I would say that I can vouch for accelerometer and gyro data feed. Without magnetometer data, the output of fusion filter seems to be great and very precise. With magnetometer - I've tested all 48 possible combinations of order and signs, without success.
Poor accuracy usually results from [...] crappy sensors.
Im not in position to judge, however usage of ICM-20948 (with AK09916 as magnetometer) doesnt seem to be that bad of a choice. Also, I'm positive that chip damage is not an issue (few devices acting the same). It's not an issue of slight deviation, but total chaos (further described).
Are you sending the properly scaled and calibrated sensor data to the Madgwick filter in NED or ENU order?
I believe so. Orientation of axes is described by documentation:
I reverse Y and Z axes value at the level of ICM-20948 driver (yes, they are little-endian):
x = (i2cReadBuffer[1] << 8) + i2cReadBuffer[0];
y = -((i2cReadBuffer[3] << 8) + i2cReadBuffer[2]);
z = -((i2cReadBuffer[5] << 8) + i2cReadBuffer[4]);
After this operation, all axes should be aligned (acc/gyro/mag). Then I've tried feeding data in NED (Y, X, -Z), ENU (-X, Y, Z) and NWU (Y, X, Z). None of mentioned fixed issues.
What does "totally wild" mean?
When I start feeding data to filter, the quaternion rotates crazily from the original position. E. g. almost instant 90 rotation in "x" axis (depends on the way of feeding magnetometer data). After this initial rotation, it stays steadily in such position. Any movement of device results on temporary deviation from this rotation, which is soon restored to the "original" state. During experiments with data feed order/signs I've recieved different results - sometimes one axis was stable, but others were making this weird rotations. Results were 100% repetitive. If device decided to head to some "determined" orientation, then it would reach it. E. g. if I rotate device, then it would deviate, and then eventually come back to this "origin" orientation. Just as if magnetometer readings overriden acc/gyro readings.
If you choose Ax pointing North, select the other 8 sensor data in NED order and point the device such that Ax aligns with North and leave it flat and motionless, do you see: 0, 0, 0, 1 for the quaternion and 0, 0, 1 for the accel, 0, 0, 0 for the gyro and Mx, My, Mz that correspond to what the USGS says you should expect for your location? Now if you rotate the device 90 degrees and leave it flat and motionless...what do you see?
It's really difficult to conduct such tests, because of previously mentioned issues. If I turn on the device (already calibrated, and motionless on flat surface) it would go from 0,0,0,1 to some crazy "predefined" orientation. If I rotate it 90 degrees (slow or fast, doesnt matter) it would rotate, then eventually come back to origin "predefined" orientation. Magnetometer readings vector magnitude correlates to values expected for my location, however it's due to calibration (it scales data, so it matches input norm value).
If the heading swings rapidly to a fixed point no matter the orientation of the device this sounds to me like the mag data is way wrong despite the good look of your calibration surface. The mag offset biases are huge. So if you have such a locally distorted mag field due to unavoidable mechanical constraints then you might not be able to use 9 DoF fusion here. Or you will have to find a way to correct for the ginormous local field. I would think the usual hard iron correction would work but if the local field is much larger than the Earth's field components you might never see any change...i.e., always points to the same heading... This is not wild, it is more or less what one would expect by putting a gigantic magnet next to an IMU.
I believe this is not the case, since the measurements (virtualized on image from my first message) are not "drawn" to a single point. When I rotate device, the vector changes the direction uniformly around XY, XZ, YZ and planes are creating proper circle. This makes me think that I am missing something in the rest of my solution. Raw and scaled data (further passed to madgwick/mahony fusion filter) seems to be just right, but somehow magnetometer data seems to have higher weight than the rest of measurements. The only scenario that cames to my mind is that the magnetometer reading vector is somehow distorted, and the "drift-preventive" part of the filter seems to draw it into invalid direction. However im more a tinkerer, than mathematician that's why I'm asking for help, since I've been struggling with this issue for many days already :(
Not sure there is anything I can do here. There are lots of examples showing how to get more or less accurate quats from the ICM20948 and a magnetometer. Nothing magic about it. I would verify that this is the case on a breadboard away from your specific application device to be sure of the method.
After temporarily removing the source of magnetic field interference I've conducted same test. While the calibration wasnt "that extreme", the final results were very same.
For tests I was using Arduino's implementation of Madgwick filter which can be found here:
https://github.com/arduino-libraries/MadgwickAHRS/blob/master/src/MadgwickAHRS.cpp
Test procedure:
- Turned off module, standing motionless on flat surface
- Turning on, init device.
- Device starts reading IMU and executes mentioned implementation of madgwick filter (data feed in X, Y, Z order - NWU).
- Quaternion rotates untill it achieves some "predetermined" orientation, then holds it. It happens everytime quaternion values are reset (1,0,0,0 w,x,y,z). Increasing "beta" parameter of filter just shortens the time the process occurs.
Is it normal for quaternion to initially reach some "predetermined" orientation (its like 210 degrees rotation)?
If the sensors are properly calibrated it is normal for the quaternion to show 0 0 0 1 when pointed at magnetic north. The heading should show whatever heading Ax (assuming this is what you chose as North) is pointed at.
I've chosen X axis as north (in NWU). If I rotate the device in a way, that Mag x/y/z are 1/0/0 (normalized values) then my Acc reads -1/0/0. Madgwick's filter quickly rotates from there (to some other angle). If Acc values are statically overriden with 0/0/1 (typical readings when device is flat) then the result quaternion is indeed 0/0/0/1 (and stable).
After many many hours of experimenting I've reached somehow acceptable results in terms of drift elimination, however when rotating device around "Z" axis, rotation seems to be "offsetted" by an angle. Occurs only while using magnetometer data. After further investigation, I've noticed that XZ plane is somehow "twisted" by an angle disturbingly similiar to the "offset" I was noticing.
This tilt occurs only on this specific axis and I believe it is the cause of "offsetted" angle. However I have little-to-none ideas how could I fix such issue... Magneto calibration doesnt seems to be covering it.
So when heading is 0 degrees the accel x-axis will point to magnetic North. Where I am there is a -13.3 degree offset (called magnetic declination) that has to be included such that 0 degrees means True North. In your area it will be different. Check the USGS and include the declination in your heading estimation. Not sure this will cure your offset issue, but this is the right thing to do for accurate absolute orientation estimation.
Im not operating on yaw/pitch/roll values. I'm using Unity engine to visualize orientation of my device with calculated quaternions. I believe there is no need for compensation of magnetic declination, since my goal is not to get "absolute" orientation, but "relative" to the lab environment. However, if my case requires such compensation, please correct me.
What's interesting - I've noticed that one of headings seems to be not affected by this "angle offset". If device lays on flat surface, and is observed from top, the observations are:
Device is rotated by 90 degrees every "step" and in three of four cases it "overshoots", however one "side" is always correct (the same, no matter how many rotations and directions I've made during tests).
Looks like hard iron error to me.
But could it "hide" in the calibration results visualisation I've attached to the first message? I would expect some visible offset from the center, however it is not there...