IMU - magnetometer - quadcopter

I am in the process of building a simple IMU for an autopilot. I am using Sparkfun, 9 Degrees of Freedom - Razor IMU ( It is surprisingly user friendly.

I was able to get accelerometer, gyro and magnetometer data right out of the box. After calibration of the sensors and a simple complementary filter i have reliable roll and pitch values.

Now I have a problem with the yaw...

I found here the equation to get the yaw out of the magnetometer value:

Xh= Xm cosPitch + Zm sinPitch Yh= Xm sinRoll sinPitch + Ym cosRoll - Zm sinRoll cosPitch with Xm, Ym, and Zm are magnetic sensor measurements

Yaw = Heading = arctan( Yh / Xh )

It sound be simple, but I can't do it. I hope that someone who already worked on IMUs can help me solve this problem.

The Excel file with IMU data and equations can be found here:

The graph shows results (first i rolled 90 degrees, then I pitched 90 degrees then I yawed 90 degrees). Roll and pitch are calculated by the IMU (programmed with Arduino IDE), they are good.

Yaw is calculated in Excel using gyro (good but drift over time) and I also calculated Yaw using the equation above (green line) and it is totally wrong (it should follow the purple line).

Any help will be appreciated.

Thank you