I recently tried to implement a magnetometer into my EKF algorithm by using an MPU6050 (Gyro and Accelerometer measurements) and a QMC5883l (Magnetometer measurements.) Without the magnetometer, the EKF works wonderful, and the state estimation flows like butter. However, when adding the magnetometer, things get messed up. When in real life yaw = pitch = roll = 0 degrees, the state estimation shows significant non-zero values.
I have calibrated the magnetometer precisely by using magneto 1.2, here some plots that show the configuration:
Magnetometer readings before calibration Magnetometer readings after calibration
The sensitivity of the chip is chosen to be +/-2G, and here a picture of what magneto shows me: Magneto's correction values
When further investigating this issue, I figured out that there must be a problem with the magnetometer readings itself. The reason that is if you imagine the initial quaternion ([1 0 0 0]) (yaw, pitch, roll are zero), the estimated magnetometer readings should be [cos(INCLINE), 0, sin(INCLINE)], where incline is the local magnetic declination (+1.53 degrees locally), therefore, if you always norm the estimate and the measurements, the magnetometer z component should be almost 0, while the x component should be almost 1.
However, when having the magnetometer flat on the table, this is the measurements I get: Format of readings is: x, y, z
This is a huge contrast to the estimation, therefore, the Kalman Filter tries to compensate for this difference, and starts showing wrong values. I have tried and looked through almost everything and haven't found people with a similar problem. I have tested several chips now, and it is always the same problem. I don't know where such a strong vertical magnetic field could be coming from, my table is made out of wood and I placed other ferromagnetic materials reasonably far away. Please someone help in figuring out why I get these values and how to fix them, here is a snippet of the function I use to read the calibrated values:
I will mention that I am converting the axis according to NED frame and I am using a NucleoF103RB:
void gy271_MagRead(gy271* mag)
{
uint8_t data[6];
int16_t x_mag;
int16_t y_mag;
int16_t z_mag;
HAL_I2C_Mem_Read(&hi2c1, (DEVICE_ADDRESS_MAG << 1), REG_MAG_DATA, 1, data, 6, 100);
x_mag = (data[1] << 8) | data[0];
y_mag = (data[3] << 8) | data[2];
z_mag = (data[5] << 8) | data[4];
x_mag = x_mag - 2603.789389f;
y_mag = y_mag - 68.478941f;
z_mag = z_mag + 2499.499613f;
mag->MagM[0] = x_mag*4.332538f - y_mag*0.139199f + z_mag*0.131290f;
mag->MagM[1] = -(-x_mag*0.139199f + y_mag*4.321255f - z_mag*0.532433f);
mag->MagM[2] = -( x_mag*0.131290f - y_mag*0.532433f + z_mag*4.436754f);
float norm = sqrtf(mag->MagM[0]*mag->MagM[0] + mag->MagM[1]*mag->MagM[1] + mag->MagM[2]*mag->MagM[2]);
mag->MagM[0] = mag->MagM[0]/norm;
mag->MagM[1] = mag->MagM[1]/norm;
mag->MagM[2] = mag->MagM[2]/norm;
printf("%f ", mag->MagM[0]);
printf("%f ", mag->MagM[1]);
printf("%f \n", mag->MagM[2]);
/* float yaw = atan2f(mag->MagM[1], mag->MagM[0]);
printf("yaw is: %f \n", yaw*RAD_TO_DEGREES); */
}
Any help is welcomed, thank you so much!
In the end, I figured out that the problem is that instead of inclination I used declination. It is true that the z-axis value is that high. The measurements are correct, however, the local inclination is 51.2 degrees where I am. The inclination tells you the inclination of the earth magnetic field at your position to predict the initial magnetometer values. Don't mix up declination with inclination!