How to tilt compensate my magnetometer ? Tried a lot

Question

I try to tilt compensate a magnetometer (BMX055) reading and tried various approaches I have found online, not a single one works.

I atually tried almost any result I found on Google.

I run this on an AVR, it would be extra awesome to find something that works without complex functions (trigonometry etc) for angles up to 50 degree.

I have a fused gravity vector (int16 signed in a float) from gyro+acc (1g gravity=16k). attitude.vect_mag.x/y/z is a float but contains a 16bit integer ranging from around -250 to +250 per axis.

Currently I try this code:

float rollRadians = attitude.roll * DEG_TO_RAD / 10;
float pitchRadians = attitude.pitch * DEG_TO_RAD / 10;
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
float Xh = attitude.vect_mag.x * cosPitch + attitude.vect_mag.z * sinPitch;
float Yh = attitude.vect_mag.x * sinRoll * sinPitch + attitude.vect_mag.y * cosRoll - attitude.vect_mag.z *sinRoll * cosPitch;

float heading = atan2(Yh, Xh);
attitude.yaw = heading*RAD_TO_DEG;

The result is meaningless, but the values without tilt compensation are correct.

The uncompensated formula:

atan2(attitude.vect_mag.y,attitude.vect_mag.x); 

works fine (when not tilted)

I am sort of clueless what is going wrong, the normal atan2 returns a good result (when balanced) but using the wide spread formulas for tilt compensation completely fails.
Do I have to keep the mag vector values within a specific range for the trigonometry to work ? Any way to do the compensation without trig functions ?

I'd be glad for some help.

Update: I found that the BMX055 magnetometer has X and Y inverted as well as Y axis is *-1 The sin/cos functions now seem to lead to a better result. I am trying to implement the suggested vector algorithms, struggling so far :)

Answer 1

Let us see.

(First, forgive me a bit of style nagging. The keyword volatile means that the variable may change even if we do not change it ourselves in our code. This may happen with a memory position that is written by another process (interrupt request in AVR context). For the compiler volatile means that the variable has to be always loaded and stored into memory when used. See:

http://en.wikipedia.org/wiki/Volatile_variable

So, most likely you do not want to have any attributes to your floats.)

Your input:

• three 12-bit (11 bits + sign) integers representing accelerometer data
• three approximately 9-bit (8 bits + sign) integers representing the magnetic field

Good news (well...) is that your resolution is not that big, so you can use integer arithmetics, which is much faster. Bad news is that there is no simple magical one-liner which would solve your problem.

First of all, what would you like to have as the compass bearing when the device is tilted? Should the device act as if it was not tilted, or should it actually show the correct projection of the magnetic field lines on the screen? The latter is how an ordinary compass acts (if the needle moves at all when tilted). In that case you should not compensate for anything, and the device can show the fancy vertical tilt of the magnetic lines when rolled sideways.

In any case, try to avoid trigonometry, it takes a lot of code space and time. Vector arithmetics is much simpler, and most of the time you can make do with multiplys and adds.

Let us try to define your problem in vector terms. Actually you have two space vectors to start with, m pointing to the direction of the magnetic field, g to the direction of gravity. If I have understood your intention correctly, you need to have vector d which points along some fixed direction in the device. (If I think of a mobile phone, d would be a vector parallel to the screen left or right edges.)

With vector mathematics this looks rather simple:

• g is a normal to a horizontal (truly horizontal) plane
• the projection of m on this plane defines the direction a horizontal compass would show
• the projection of d on the plane defines the "north" on the compass face
• the angle between m and d gives the compass bearing

Now that we are not interested in the magnitude of the magnetic field, we can scale everything as we want. This reduces the need to use unity vectors which are expensive to calculate.

So, the maths will be something along these lines:

# projection of m on g (. represents dot product)
mp := m - g (m.g) / (g.g)

# projection of d on g
dp := d - g (d.g) / (g.g)

# angle between mp and dp
cos2 := (mp.dp)^2 / (mp.mp * dp.dp)
sgn1 := sign(mp.dp)

# create a vector 90 rotated from d on the plane defined by g (x is cross product)
drot := dp x g
sin2 := (mp.drot)^2 / (mp.mp * drot.drot)
sgn2 := sign(mp.drot)

After this you will have a sin^2 and cos^2 of the compass directions. You need to create a resolving function for one quadrant and then determine the correct quadrant by using the signs. The resolving function may sound difficult, but actually you just need to create a table lookup function for sin2/cos2 or cos2/sin2 (whichever is smaller). It is relatively fast, and only a few points are required in the lookup (with bilinear approximation even fewer).

So, as you can see, there are no trig functions around, and even no square roots around. Vector dots and crosses are just multiplys. The only slightly challenging trick is to scale the fixed point arithmetics to the correct scale in each calculation.

You might notice that there is a lot of room for optimization, as the same values are used several times. The first step is to get the algorithm run on a PC with floating point with the correct results. The optimizations come later.

(Sorry, I am not going to write the actual code here, but if there is something that needs clarifying, I'll be glad to help.)