Tilted magnetometer output -pitch and roll tilt compensation - i am lost

Question

I have wasted 2 days (and nights) on this specific issue and completely failed in tilt compensating my magnetometer output.
I tried about everything I could read on Google and in open source examples, nothing guided me to a proper tilt compensation for both (roll and pitch). .

The Setup: I use calibrated values but not unity values.
I have a fused gravity vector which is working properly and exact.
The sensor is a 9dof BMX055 ( http://www.mouser.com/ds/2/621/BST-BMX055-DS000-01-274824.pdf ) The magnetometer min/max are +- 512 on each axis (with small differences but all axes are zero'd out). The hardware is Sam3X8e (Cortex M3), all written in C.
Floating point and trigonometry can be done quite fast, so that's no problem. The BMX055 chip is aligned so that the pins 20,19,18 point to the front.
Datasheet page 159-161 show the orientation. Pitch rises when I rise the front.
Roll rises when I rise the left side.

Example values:
Pointing at a direction my algorithm calls 305deg when leveled horizontal

Pitch: 0  , Roll 0   : MAG cal:  x 132   y  93  z   -364  
Pitch: +24, Roll 0   : MAG cal:  x -109   y  93  z   -397  
Pitch: +46, Roll 0   : MAG cal:  x -303   y  89  z   -351  
Pitch: 0  , Roll -44 : MAG cal:  x 151   y  352  z   -235  
Pitch: 0  , Roll +36 : MAG cal:  x 130   y  -140  z   -328  
Pitch: 78 , Roll -2  : MAG cal:  x -503   y  93  z   -199  
Pitch: 7  , Roll -53 : MAG cal:  x 135   y  424  z   -180

The alignment should always have been around 305 degree (as good as I could do it), maybe +- 5 degree.

The formula: (same one used about everywhere)

uint16_t compass_tilt_compensation(float roll_radians, float pitch_radians,float mag_y, float mag_x, float mag_z) 
{

  float tilt_compensated_heading;
  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  int16_t heading;
  //pitch_radians =roll_radians;
  //roll_radians *= -1;
  //mag_x *= -1;
  //roll_radians=0.0f;
  //pitch_radians=0;v
  //pitch_radians *=-1;

  cos_roll = cos(roll_radians);
  sin_roll = sin(roll_radians);
  cos_pitch = cos(pitch_radians);
  sin_pitch = sin(pitch_radians);


#if 0
  MAG_X = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
  MAG_Y = mag_y*cos_roll-mag_z*sin_roll;
  tilt_compensated_heading = atan2f(-MAG_Y,MAG_X); 
#else
    MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
    MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
    tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
    //tilt_compensated_heading = atan2f(-mag_y,mag_x); // works fine when leveled
#endif

//convert to degree from 0-3599
    heading = tilt_compensated_heading * RAD_TO_DEG * 10;
     if ( heading < 0 ) 
     {
         heading += 3600;
     }

  return heading;
}

I tried various combinations, I tried to only fix one axis and leave one always at 0, exchanging X/Y pitch/roll, *-1 on any of the inputs.

The results are always completely wrong. Sometimes (depending on where I try to invert or not invert a value by trial/error) the values seem to be off nearly linear. Sometimes one axis is compensated in positive areas.
However rolling and pitching always causes 'random' jumps and changes of the heading.
Math has never been my favourite, now I regret it.

My personal guess is that the formula is right in principe, the mag is working in principe (after all I get proper degrees when leveled) but I am somehow feeding something wrong. (Like Y and X need to be exchanged, z*-1, pitch needs to be *-1)

Could someone who is good in this subject maybe take a look and guide me how to get a proper heading ?
Would be great to have a few hours sleep this night without having to dream about a dysfunctional algorithm again :)

Update: The tilt compensation here works for negative roll when pointing at the 305deg heading. It is also used here: http://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf

Answer 1

First, verify that it is indeed a software problem. Your equations seem to be correct. Just in case, generate a table populated with test data to pass to your procedure and compare the output of your function with the expected values if the code was correct.

You are using a magnetometer and these are very sensitive devices. The first thing I would check is whether there are any large metal structures near the sensor. If you have access to an oscilloscope, probe the chip's power rails and see if power going into it is stable. Adding a 1uF decoupling cap could solve power issues. I would also suggest getting a graph while having a sampling frequency larger than 100Hz and see if the jumps are periodic. If the signal is periodic with a 50Hz frequency, assuming the sensor is not being moved, that would indicate interference from your mains supply. Performing FFT analysis over the data can't hurt. We had similar problems caused by power cables running underneath our lab floor. If the sensor keeps jumping around randomly, the chip is probably dead. Did you use proper ESD protection while handling it?

Hope this helps.

Answer 2

3 days of work, finally I found the issues I had and tilt compensation is working !
I read quite a few people had such issues in various forums, so here is what I did:

I explain what I did and why I did, step by step. Maybe someone with a similar issue will find a solution that way.

1. Playing around with the values can help (maybe just pitch or roll has to be inverted, X or Y has to be exchanged).
   However, there are quite a lot of values and the amount of combinations is too high if your problem is more than a tiny one.

2. The formula posted works fine (the one which is in the active #if branch.
   If you have a magnetometer and the atan2(-y,x) gives you a proper heading when leveled flat, then this formula will work for you too.

3. What I did was to completely go through all my sensors and vectors beginning from the I2C binary logic (and the datasheet).
   Important in this secial case (BMX055), the Datasheet orientation page is WRONG!
   There are multiple bugs regarding orientation of axes (x,y) as well as when a rotation is positive or negative. Sometimes right hand rule applies, sometimes not and the drawings are misleading (wrong). Bosch did a bad job documenting this chip (and the previous one). They do not seem to want people to understand it properly, they write about a fusion API several times with optimized fixed point arithmetiks and advanced calibration but it's not available for the public.

What I needed to do was to make a proper body reference system. Decide yourself where X is and then make it so all your sensors properly change the X axis when pitched/rolled into the same direction (positive/negative). Do this for pitch,roll and gravity/magnetic field.

Once all of them play together nicely I started all over again.
The heading formula was still dysfunctional but now I trusted the vextors the first time.
I added a vector rotation matrix function and rotated the magnetic vector back using roll and pitch and yaw=0. Surprise: the magnetic vector was tilt compensated.

Now I knew it CAN be done :)

I went back to the original formula, replaced X with Y (because I had exchanged them to match the body reference system (X and Y of gyro/mag).
Now tilt compensation worked for pitch but not for roll.
So I inverted roll_radians and suddenly it's perfectly tilt compensated.

I have two solutions now. One is a rotation matrix solution and the other one the standard solution everyone is using. I will see if one of them performs better and maybe give a last update here if the results are worth it.