How to implement a tilt compensated compass?

Question

So i was trying to implement a tilt compensated compass, using HMC5883L and MPU6050. The problem i am facing is, there are 2 equations for tilt compensation

Tilt Compensated X: X,Y,Z are compass output (raw output)

new_X = X*cos(pitch) + Y*sin(roll)*sin(pitch) – Z*cos(roll)*sin(pitch)

Tilt Compensated Y:

new Y = Y*cos(roll) + Z*sin(roll)

Final Heading : Azimuth = atan2(Y, X) * RAD_TO_DEG

The angles which i am getting from the mpu are gyro-drift corrected angles which are coming through a kalman filter. Those angles are fed to these above equations and after atan2 i get the tilt compensated heading. But the issue/observation here is, when i rotate the body(on which the compass and mpu are) along the Z axis (YAW axis) the heading doesn't change(sort of locked on), but changes when i tilt the body in X/Y. I searched for thorough references, but wasn't able to find any!
Here is my code snippet (uses compass part form adafruit library):

double HEADING(double r, double p){
 sensors_event_t event; 
 mag.getEvent(&event);
 r = r * (PI/180);
 p = p * (PI/180);
 double x = (event.magnetic.x)*cos(p) + (event.magnetic.y)*sin(r)*sin(p) - (event.magnetic.z)*cos(r)*sin(p);
 double y = (event.magnetic.y)*cos(r) + (event.magnetic.z)*sin(r);
 
 float heading = atan2(y,x);
 
 float declinationAngle = 0.366667 * (PI/180);// (0°22')positve declination
 heading += declinationAngle;
 
 if(heading < 0)
 heading += 2*PI;
 
 if(heading > 2*PI)
 heading -= 2*PI;
 
 heading = heading * 180/PI; 
 
 return heading;
}

The HMC5883L data acquisition I wrote :

// i just need a direction reference not actual heading, just to keep my drone 
// steady on yaw axis with reference to that direction reference but it shall be Tilt compensated
#include<Wire.h>
#define MAG 0x1E
#define R 0
#define P 1
#define Y 2
float mag_raw[3],h;
float CMP_DEC = 0.36667 * (PI/180);
void setup(){
 Serial.begin(115200);
 Wire.begin();
 Wire.setClock(400000);
 
 Wire.beginTransmission(MAG);
 Wire.write(0x02);
 Wire.write(0x00);
 Wire.endTransmission(true);
 
}
void loop(){
 Wire.beginTransmission(MAG);
 Wire.write(0x03);
 Wire.endTransmission(false);
 Wire.requestFrom(MAG,6,true);
 mag_raw[R] = Wire.read() << 8 | Wire.read();
 mag_raw[P] = Wire.read() << 8 | Wire.read();
 mag_raw[Y] = Wire.read() << 8 | Wire.read();
 h = atan2(mag_raw[P],mag_raw[R]);
// h += CMP_DEC;
 if(h < 0) h+= (2*PI);
 if(h > (2*PI)) h -= (2*PI);
 h *= (180/PI);
 
// Serial.print("X : ");
// Serial.print(mag_raw[R]);
// Serial.print(" Y : ");
// Serial.print(mag_raw[P]);
// Serial.print(" Z : ");
// Serial.print(mag_raw[Y]);
 Serial.print(" H : ");
 Serial.println(h);
 
}

Some guidance will be very helpful!

• st2000: Are you writing all the code? (Not using libraries?) I ask as you did not mention how you were compensating the magnetometer's output for hard and soft iron errors. Creating a tilt compass is hard enough. But I find it better, if writing most or all of the code from scratch, to create a normal (use it flat on a table) compass 1st.
• Sajil: @st2000 I tried writing my own code (the general approach for i2C Wire lib). Sometimes i got a solid output (no change in data even if i move in different axis), sometimes data came but the range remained almost around 170 for X , 65000 for Y and 56 for Z axes. I didn't understand what was going on so i started using libraries :( can you give me some relevant references ?
• Edgar Bonet: Are the pitch and roll angles small when the body sits flat on a table? Do the raw values of the magnetic field components change when you rotate it?
• Sajil: @EdgarBonet i'll answer according to what i understood; So the R&P angles coming from the MPU are 0±0.1° when kept on the flat surface. Now when I rotate the compass along Z axis, there is no change in the values(actually it is there but it changed about 3 points when i rotated it 90°!). For X,Y axes, the values change a lot !almost ±120 units. ill just add my from scratch written code in the question. Have a look to it, maybe something wrong can be seen...
• Sajil: @st2000 yeah let me work more on magnetometer thoroughly. Can you send me some references for it ? Possibly not using the libraries (I find it easy without libraries 😅).

Answer 1

Inspecting your code, it does not appear you are compensating the raw magnetometer values for soft and hard iron effects. These terms roughly refer to adjusting the range and magnitude of each of the individual magnetometers (X, Y and Z). Some magnetometer manufactures allow writing these compensation values into the chip. Even then the developer and user will still need to perform some form of calibration as the magnetometer can not predict under what conditions the magnetometer will be used. Here is another explanation using matrices of hard and soft iron compensation.

Once the compass produces expected results on a flat surface parallel to the surface of the earth (in a place on earth where the magnetic lines roughly align with true north). Consider adding tilt compensation. Accelerometers usually do not need to be compensated as extensively as magnetometers.

This can be done using trigonometric math and Euler angles. However, as with physical gambles, this can lead to a situation similar to gimbal lock. Where the trigonometric math goes to infinity.

To avoid a gimbal lock like situation, most tilt compensated compasses use quaternions. Quaternions use imaginary numbers and matrix math to describe 3 dimensional rotation.