Neigungskompensierter Kompass & Arduino ( tilt compensated magnetometer/compass)

Hallo Liebe Community,

ich versuche einen neigungskompensierten Kompass mit Hilfe eines 
Arduino's, eines Hmc5883l Magnetometers und eines ADXL335 zu bauen. 
Leider funktioniert es nicht so wie erwartet und den Fehler kann ich 
nicht entdecken...

----------------------------------------
Zum Beschleunigungssensor ADXL335:

Es ist ein analoger Sensor, weswegen ich zuerst die Volt-Werte lese.

 float xAcc = xRead;
 float yAcc = yRead;
 float zAcc = zRead;

Dann wird ein Offset subtrahiert, damit die X- und Y-Achse auf 0 Volt 
liegt, wenn der Sensor HORIZONTAL liegt. Und ein Offset bei zAcc 
abgezogen, damit dieser 0 liegt, wenn die Z-Achse horizontal liegt:

 float xAcc = (xRead-327);
 float yAcc = (yRead-333);
 float zAcc = (zRead-334);

Nun noch die Werte skalieren, sodass sie in g (Erdbeschleunigung) 
ausgegeben werden:

  float xAcc = (xRead-327)/69.00;
  float yAcc = (yRead-333)/69.00;
  float zAcc = (zRead-334)/69.00;

Liegt der Sensor gerade kommt: X: 0g Y: 0g Z:1g
Liegt die X-Achse oben, kommt: X:1 Y:0 Z:0 usw.

Diese werden nun in Radians umgewandelt:

 float pitch = atan(xAcc/sqrt(pow(yAcc,2) + pow(zAcc,2)));
 float roll = atan(yAcc/sqrt(pow(xAcc,2) + pow(zAcc,2)));

Wobei diese Formel auch funktioniert: (ich weiß nicht warum)

 float rollRadians = asin(yAcc);
 float pitchRadians = asin(xAcc);

Eigentlich müsste das alles stimmen.

---------------------------------------
Zum Magnetometer HMC5883l:

Diesen steuere ich durch eine Bibliothek an, die ich jetzt nicht genauer 
erläutere. Ich bekomme dann die "skalierten" Achsen ausgegeben.

MagnetometerScaled scaled = compass.ReadScaledAxis();

float xMag = scaled.XAxis;
float yMag = scaled.YAxis;
float zMag = scaled.ZAxis;

Hier gibt es noch eine Methode, um die "raw" (Rohdaten) der Achsen 
auszugeben. Ich denke aber, dass die nicht zu gebrauchen sind. Oder 
kennt sich dort jemand aus?

---------------------------------------
Zur Neigungskompensation:

Nun benutze ich Formeln, die ich aus dem Internet habe.
Von: http://de.scribd.com/doc/89773850/6DOF-Arduino#

float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);

float Xh = scaled.XAxis * cosPitch + scaled.ZAxis * sinPitch;
float Yh = scaled.XAxis  sinRoll  sinPitch + scaled.YAxis * cosRoll - 
scaled.ZAxis sinRoll  cosPitch;

float heading = atan2(Yh, Xh);

Diese Formel funktioniert komischerweise nicht. Hebe ich beide Sensoren 
auf dem Breadboard an der Seite an, bleiben die "Heading"-Werte nicht 
gleich, sondern ändern sich.
Ich habe schon pitch und roll getauscht, die Achsen umgedreht, nichts 
hat geholfen.

Auch diese Formel, der Webseite http://www.timzaman.com/?p=1010 , 
funktioniert nicht:

xh=cx*cos(ayf)+cy*sin(ayf)*sin(axf)-cz*cos(axf)*sin(ayf);
yh=cy*cos(axf)+cz*sin(axf);

Daher frage ich euch: Wo liegt mein Fehler?

Vielen Dank im Voraus! Hoffentlich konnte jemand Nutzen aus meinen 
Beitrag ziehen, da es doch leider wenig Dokumentation zur 
Neigungskompensation im Internet gibt...