Compass Heading from Magnetometer on other axis

2.8k views Asked by At

I am building a small device that also uses magnetometer data in order to calculate the compass heading. The LSM9DS0 IMU sensor works great if the heading is calculated as a yaw (if the sensor is on a flat surface).

I have 3D printed a shell in which i am going to assemble all the electronics. My problem is that it is poorly designed and the IMU sensor is not on a flat surface, but it has to stay on 90 degrees. So by this, the Z axis is no more my way to calculate the yaw (or heading), but it changed to Y.

In order to calculate the heading on Z, i was using this formula:

heading.value = atan2((float)dof.my, (float)dof.mx);
        if(heading.value < 0) heading.value += 2*PI;
        if(heading.value > 2*PI) heading.value -= 2*PI;
 heading.value *= 180/PI; 

...where my is the magnetometer Y and mx the magnetometer X

Now, I don't know how to calculate the heading based on other axis.

1

There are 1 answers

2
Navid On

I know this thread hasn't been active for a while, but during my search I came across this publication by NXP which explains the solution really nicely.

In brief:

  1. Align the accelerometer readings (G) and the magnetometer readings (B) so they follow the NED coordinate system with the x-axis pointing forward and the y and z-axis to the right and down, respectively.
  2. Calculate the roll and pitch

    // Using atan2 to restrict +/- PI const roll = Math.atan2(Gy, Gz)

    // Using atan to restrict to +/- PI/2 const pitch = Math.atan(-Gx / (Gy * Math.sin(roll) + Gz * Math.cos(roll)))

  3. Calculate the yaw / compass heading (here Vx, Vy, Vz correspond to the Hard-Iron effects that can be calculated separately as discussed in this publication):

    // Using atan2 to restring to +/- PI let yaw = Math.atan2( (Bz-Vz)*Math.sin(roll) - (By-Vy)*Math.cos(roll), (Bx-Vx)*Math.cos(pitch) + (By-Vy)*Math.sin(pitch)*Math.sin(roll) + (Bz-Vz)*Math.sin(pitch)*Math.cos(roll))

  4. Correct the heading to [0, 2*PI)

    if( yaw < 0 ) { yaw += 2*Math.PI }