Tilt compensation using accelerometer and magnetometer for ecompass problem

[si0ly]

Hi,
So I use accelerometer and magnetometer data for use in a complementary filter to determine pitch, roll, yaw. I have noticed that during specific yaw angles when pitch or roll occurs, the yaw changes by about 10 - 20 degrees.

I have checked the calibration of the magnetometer using a simple code from matlab which plots the raw data collected from the magnetometer and the calibrated data, the graph of which shows the 3D sfere so this is not a magnetometer problem I think.

My tilt compensation and complementary filter code looks like this:

void applyAHRSFilter(float gyroX, float gyroY, float gyroZ, float accelX, float accelY, float accelZ, float magX, float magY, float magZ, float dt) {

    float accelRoll = atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * RAD_TO_DEG;
    float accelPitch = atan2(accelX, sqrt(accelY * accelY + accelZ * accelZ)) * RAD_TO_DEG;

    float sinRoll = sin(accelRoll * DEG_TO_RAD);
    float cosRoll = cos(accelRoll * DEG_TO_RAD);
    float sinPitch = sin(accelPitch * DEG_TO_RAD);
    float cosPitch = cos(accelPitch * DEG_TO_RAD);
     //------------------------------------

     float mx = magX * cosPitch + magZ * sinPitch;
     float my = magX * sinRoll * sinPitch + magY * cosRoll - magZ * sinRoll * cosPitch;

    float MagYaw = atan2(-my, mx) * RAD_TO_DEG;

    MagYaw = fmod(MagYaw + 360.0f, 360.0f);

    float gyroRoll = roll + gyroY * dt;
    float gyroPitch = pitch + gyroX * dt;
    float gyroYaw = yaw + gyroZ * dt;

   
    const float alpha = 0.98;
    const float alphaM = 0.96;
    // roll = alpha * gyroRoll + (1 - alpha) * accelRoll;
    // pitch = alpha * gyroPitch + (1 - alpha) * accelPitch;
    // yaw = gyroYaw *alphaM + (1 - alphaM) * MagYaw;
    roll = accelRoll;
    pitch = accelPitch;
    yaw = MagYaw;
    // yaw = fmod(yaw + 360.0f, 360.0f);

}

I had to adjust the axes of the accelerometer, gyroscope and magnetometer to make them suitable for my reference system. Does the Complementary Filter with tilt compensation for the accelerometer and magnetometer have any limitations in this case (apart from the gimbal lock) or have I not implemented it correctly?

[si0ly]

Here is an example of measurement data when the IMU is tilted in 180 degree yaw orientation :
Yaw: 180.01 Pitch: 0.24 Roll:0.21
Yaw: 179.99 Pitch: 0.22 Roll:0.20
Yaw: 180.72 Pitch: 0.22 Roll:0.20
Yaw: 180.75 Pitch: 0.17 Roll:0.22
Yaw: 179.43 Pitch: 0.17 Roll:0.22
Yaw: 179.42 Pitch: -0.12 Roll:0.22
Yaw: 179.42 Pitch: -0.71 Roll:0.22
Yaw: 178.68 Pitch: -0.71 Roll:0.22
Yaw: 178.71 Pitch: -1.75 Roll:0.26
Yaw: 178.13 Pitch: -1.75 Roll:0.26
Yaw: 178.09 Pitch: -3.04 Roll:0.27
Yaw: 178.02 Pitch: -4.43 Roll:0.26
Yaw: 178.22 Pitch: -4.43 Roll:0.26
Yaw: 178.14 Pitch: -5.97 Roll:0.25
Yaw: 179.24 Pitch: -5.97 Roll:0.25
Yaw: 179.24 Pitch: -7.74 Roll:0.26
Yaw: 179.39 Pitch: -9.77 Roll:0.38
Yaw: 178.18 Pitch: -9.77 Roll:0.38
Yaw: 178.19 Pitch: -11.79 Roll:0.44
Yaw: 178.16 Pitch: -11.79 Roll:0.44
Yaw: 177.93 Pitch: -13.52 Roll:0.34
Yaw: 177.92 Pitch: -15.15 Roll:0.39
Yaw: 177.23 Pitch: -15.15 Roll:0.39
Yaw: 177.19 Pitch: -16.39 Roll:0.41
Yaw: 177.18 Pitch: -16.39 Roll:0.41
Yaw: 176.87 Pitch: -17.37 Roll:0.28
Yaw: 176.65 Pitch: -18.72 Roll:0.21
Yaw: 175.81 Pitch: -18.72 Roll:0.21
Yaw: 175.45 Pitch: -20.36 Roll:0.14
Yaw: 176.20 Pitch: -20.36 Roll:0.14
Yaw: 175.84 Pitch: -22.22 Roll:0.09
Yaw: 175.55 Pitch: -23.90 Roll:0.06
Yaw: 175.55 Pitch: -23.90 Roll:0.06
Yaw: 175.56 Pitch: -25.32 Roll:0.18
Yaw: 175.29 Pitch: -25.32 Roll:0.18
Yaw: 175.08 Pitch: -26.52 Roll:0.17
Yaw: 174.46 Pitch: -27.69 Roll:0.15
Yaw: 174.46 Pitch: -27.69 Roll:0.15
Yaw: 174.06 Pitch: -28.96 Roll:0.09
Yaw: 173.63 Pitch: -28.96 Roll:0.09
Yaw: 173.19 Pitch: -30.25 Roll:0.04
Yaw: 173.27 Pitch: -31.49 Roll:-0.00
Yaw: 173.27 Pitch: -31.49 Roll:-0.00
Yaw: 172.79 Pitch: -32.81 Roll:-0.03
Yaw: 173.02 Pitch: -32.81 Roll:-0.03
Yaw: 172.39 Pitch: -34.31 Roll:-0.07
Yaw: 173.14 Pitch: -35.65 Roll:-0.09
Yaw: 173.14 Pitch: -35.65 Roll:-0.09
Yaw: 172.65 Pitch: -36.86 Roll:-0.10
Yaw: 172.80 Pitch: -36.86 Roll:-0.10
Yaw: 172.22 Pitch: -37.89 Roll:-0.16
Yaw: 171.32 Pitch: -41.03 Roll:-0.23
Yaw: 172.38 Pitch: -38.76 Roll:-0.23
Yaw: 172.22 Pitch: -39.35 Roll:-0.20
Yaw: 172.22 Pitch: -39.35 Roll:-0.20
Yaw: 172.04 Pitch: -39.88 Roll:-0.19
Yaw: 172.70 Pitch: -39.88 Roll:-0.19
Yaw: 172.35 Pitch: -40.72 Roll:-0.19
Yaw: 173.05 Pitch: -41.53 Roll:-0.16
Yaw: 173.05 Pitch: -41.53 Roll:-0.16
Yaw: 172.87 Pitch: -42.09 Roll:-0.14
Yaw: 171.90 Pitch: -42.09 Roll:-0.14
Yaw: 171.43 Pitch: -43.03 Roll:-0.14
Yaw: 171.32 Pitch: -44.20 Roll:-0.14
Yaw: 171.32 Pitch: -44.20 Roll:-0.14
Yaw: 170.56 Pitch: -45.43 Roll:-0.16
Yaw: 170.52 Pitch: -45.43 Roll:-0.16
Yaw: 169.57 Pitch: -47.06 Roll:-0.13
Yaw: 168.58 Pitch: -48.52 Roll:-0.07
Yaw: 168.58 Pitch: -48.52 Roll:-0.07
Yaw: 167.68 Pitch: -49.66 Roll:-0.03
Yaw: 167.68 Pitch: -49.66 Roll:-0.03
Yaw: 166.73 Pitch: -50.61 Roll:-0.01
Yaw: 165.90 Pitch: -51.33 Roll:-0.03
Yaw: 165.90 Pitch: -51.33 Roll:-0.03
Yaw: 165.30 Pitch: -51.63 Roll:-0.07
Yaw: 166.46 Pitch: -51.63 Roll:-0.07
Yaw: 165.86 Pitch: -51.75 Roll:-0.16
Yaw: 166.38 Pitch: -52.02 Roll:-0.21
Yaw: 166.38 Pitch: -52.02 Roll:-0.21
Yaw: 165.68 Pitch: -52.44 Roll:-0.23
Yaw: 165.74 Pitch: -52.44 Roll:-0.23
Yaw: 164.76 Pitch: -53.16 Roll:-0.21
Yaw: 163.21 Pitch: -53.77 Roll:-0.21
Yaw: 163.21 Pitch: -53.77 Roll:-0.21
Yaw: 162.63 Pitch: -54.08 Roll:-0.20
Yaw: 160.95 Pitch: -54.08 Roll:-0.20
Yaw: 160.44 Pitch: -54.32 Roll:-0.20
Yaw: 164.26 Pitch: -54.69 Roll:-0.20
Yaw: 164.26 Pitch: -54.69 Roll:-0.20
Yaw: 158.27 Pitch: -55.19 Roll:-0.23
Yaw: 158.27 Pitch: -55.19 Roll:-0.23
Yaw: 156.96 Pitch: -55.55 Roll:-0.24
Yaw: 154.74 Pitch: -55.71 Roll:-0.23
Yaw: 154.74 Pitch: -55.71 Roll:-0.23
Yaw: 150.85 Pitch: -55.94 Roll:-0.17
Yaw: 150.85 Pitch: -55.94 Roll:-0.17
Yaw: 150.92 Pitch: -56.18 Roll:-0.03
Yaw: 160.13 Pitch: -55.86 Roll:0.45
Yaw: 160.13 Pitch: -55.86 Roll:0.45
Yaw: 149.69 Pitch: -56.31 Roll:0.54
Yaw: 149.69 Pitch: -56.31 Roll:0.54
Yaw: 157.47 Pitch: -56.00 Roll:1.20
Yaw: 140.38 Pitch: -56.85 Roll:0.52
Yaw: 140.38 Pitch: -56.85 Roll:0.52
Yaw: 128.63 Pitch: -56.22 Roll:-0.04
Yaw: 128.63 Pitch: -56.22 Roll:-0.04
Yaw: 136.21 Pitch: -55.21 Roll:-0.17
Yaw: 119.22 Pitch: -55.21 Roll:-0.17
Yaw: 130.58 Pitch: -54.00 Roll:-0.19
Yaw: 134.98 Pitch: -52.82 Roll:-0.17
Yaw: 134.98 Pitch: -52.82 Roll:-0.17
Yaw: 137.28 Pitch: -52.47 Roll:-0.20
Yaw: 125.77 Pitch: -52.47 Roll:-0.20
Yaw: 124.92 Pitch: -52.64 Roll:-0.09
Yaw: 126.24 Pitch: -51.83 Roll:-0.16
Yaw: 126.24 Pitch: -51.83 Roll:-0.16
Yaw: 130.93 Pitch: -50.69 Roll:-0.09
Yaw: 128.30 Pitch: -50.69 Roll:-0.09
Yaw: 134.90 Pitch: -49.86 Roll:-0.18
Yaw: 128.13 Pitch: -48.43 Roll:-0.38
Yaw: 128.13 Pitch: -48.43 Roll:-0.38
Yaw: 136.85 Pitch: -46.72 Roll:-0.32
Yaw: 136.85 Pitch: -46.72 Roll:-0.32
Yaw: 147.06 Pitch: -44.96 Roll:-0.23
Yaw: 142.44 Pitch: -44.18 Roll:-0.38
Yaw: 142.44 Pitch: -44.18 Roll:-0.38
Yaw: 139.44 Pitch: -43.49 Roll:-0.51
Yaw: 139.44 Pitch: -43.49 Roll:-0.51
Yaw: 147.15 Pitch: -41.69 Roll:-0.69
Yaw: 145.60 Pitch: -40.46 Roll:-0.59
Yaw: 145.60 Pitch: -40.46 Roll:-0.59
Yaw: 139.16 Pitch: -38.76 Roll:-1.41
Yaw: 139.16 Pitch: -38.76 Roll:-1.41
Yaw: 145.95 Pitch: -37.03 Roll:-1.54
Yaw: 149.52 Pitch: -34.43 Roll:-1.13
Yaw: 149.52 Pitch: -34.43 Roll:-1.13
Yaw: 151.00 Pitch: -32.47 Roll:-0.93
Yaw: 151.00 Pitch: -32.47 Roll:-0.93
Yaw: 153.62 Pitch: -31.28 Roll:-1.00
Yaw: 153.67 Pitch: -29.91 Roll:-1.14
Yaw: 153.67 Pitch: -29.91 Roll:-1.14
Yaw: 157.12 Pitch: -27.48 Roll:-0.53
Yaw: 157.12 Pitch: -27.48 Roll:-0.53
Yaw: 164.14 Pitch: -23.88 Roll:-0.03
Yaw: 167.68 Pitch: -20.16 Roll:0.55
Yaw: 167.68 Pitch: -20.16 Roll:0.55
Yaw: 171.82 Pitch: -16.84 Roll:1.69
Yaw: 171.82 Pitch: -16.84 Roll:1.69
Yaw: 169.78 Pitch: -12.34 Roll:0.70
Yaw: 171.13 Pitch: -5.09 Roll:0.13
Yaw: 171.13 Pitch: -5.09 Roll:0.13
Yaw: 171.33 Pitch: -4.25 Roll:0.06
Yaw: 171.33 Pitch: -4.25 Roll:0.06
Yaw: 171.87 Pitch: -3.80 Roll:0.10
Yaw: 172.03 Pitch: -3.36 Roll:0.13
Yaw: 172.03 Pitch: -3.36 Roll:0.13
Yaw: 172.84 Pitch: -2.94 Roll:0.14
Yaw: 172.84 Pitch: -2.94 Roll:0.14
Yaw: 173.37 Pitch: -2.50 Roll:0.10
Yaw: 173.36 Pitch: -2.52 Roll:0.10
Yaw: 173.21 Pitch: -1.96 Roll:-0.05
Yaw: 173.04 Pitch: -1.76 Roll:0.04
Yaw: 173.04 Pitch: -1.76 Roll:0.04
Yaw: 172.87 Pitch: -1.63 Roll:0.15
Yaw: 172.87 Pitch: -1.63 Roll:0.15
Yaw: 173.01 Pitch: -1.47 Roll:0.21
Yaw: 172.43 Pitch: -1.37 Roll:0.32
Yaw: 172.43 Pitch: -1.37 Roll:0.32
Yaw: 173.35 Pitch: -1.26 Roll:0.39
Yaw: 173.35 Pitch: -1.26 Roll:0.39
Yaw: 173.50 Pitch: -1.09 Roll:0.46
Yaw: 173.46 Pitch: -0.97 Roll:0.41

[jremington]

To make a tilt-compensated compass, the vector approach used by Pololu takes around a half-dozen lines of code and is by far the simplest and most accurate.

With the algebra/trig approach you have taken, it is all too easy to make a typo or simple sign error, and is extremely difficult to debug.

Here are versions of Pololu's approach (using a much better method of calibration) for the ICM-20948 and for the LSM9DS1

[si0ly]

In what units are the values in the hard iron matrix in this example? I would like to use my own calibration data for the magnetometer and accelerometer.

[jremington]

The correction matrix is dimensionless.

[xfpd]

Wrap your data in a code block blanket...