Neigungskompensierter Kompass (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 wie erwartet und den fehler kann ich
nicht entdecken...

----------------------------------------
zum beschleunigungssensor adxl335:

es ist ein analoger sensor, weswegen ich zuerst die volt-werte lese.

code: [select]

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:

code: [select]

float xacc = (xread-327);
float yacc = (yread-333);
float zacc = (zread-334);

nun noch die werte skalieren, sodass sie in g (erdbeschleunigung)
ausgegeben werden:

code: [select]

  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:

code: [select]

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)

code: [select]

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.

code: [select]

magnetometerscaled scaled = compass.readscaledaxis();

code: [select]

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#

code: [select]

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 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:

code: [select]

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...

quote from: redonearduino on may 18, 2014, 01:26 pm

daher frage ich euch: wo liegt mein fehler?

der fehler liegt darin, dass du den sensor nicht individuell kalibriert hast, bevor du zu rechnen anfängst.

das, du da mit deiner library machst:

quote from: redonearduino on may 18, 2014, 01:26 pm

diesen steuere ich durch eine bibliothek an, die ich jetzt nicht genauer
erläutere. ich bekomme dann die "skalierten" achsen ausgegeben.

ist die reinste augenwischerei, wenn es die library ist, die ich mal kurz angetestet hatte und die ich vermute, dass du sie verwendest.

diese library macht irgendeinen scheißdreck mit einer statischen kalibrierung, wobei die statistische wahrscheinlichkeit, dass die "calibrated" werte besser zutreffen als die "raw" werte ungefähr bei 50% liegen. das ist völlig unbrauchbarer quatsch, diese library als readscaledaxis() anbietet. da kannst du genau gut würfeln oder eine münze werfen und nach dem ergebnis deinen sensor kalibrieren.

ich habe den hmc5883l selbst mal ein wenig angetestet im rahmen der von rabenauge hier angestoßenen diskussion:
http://forum.arduino.cc/index.php?topic=201453.0

selbst wenn du dich nur auf die waagerechte ebene beschränkst, und nach besten kräften magnetfelder in der nähe vermeidest: schon wenn der sensor auf einer kleinen platine aufgelötet ist und du noch eine kleine headerleiste mit stahlstifen zum anlöten von kabeln dranlötest, ist das magnetfeld in sensornähe bereits verzerrt, dass ohne eine individuelle kalibrierung des sensors die daten kaum irgendwelchen hohen genauigkeitsanforderungen entsprechen. erst recht, wenn du das sensormodul mit den angelöteten pin-header dann noch auf ein breadboard steckst, das im innern gespickt ist mit einem skelett aus stahl-klammern.

wenn du den sensor nicht nur in der ebene kalibrieren möchtest, sondern auch eine neigungskompensation brauchst, wirst du eine dynamische kalibrierung des sensors in allen drei raumachsen vornehmen müssen. d.h. du mußt zunächst konkreten sensor eine messreihe fahren und
- während einer vollen dreihung um die x-achse viele werte ermitteln
- während einer vollen dreihung um die y-achse viele werte ermitteln
- während einer vollen dreihung um die z-achse viele werte ermitteln
aus diesen rohdaten muss dann eine korrektur-matrix berechnet werden, die verknüpft mit dem messwert für jeden messwert denselben magnetvektor ermittelt, egal wie die lage des sensors im raum ist.

mathematische grundlagen siehe z.b.:
http://www.freescale.com/files/sensors/doc/app_note/an4246.pdf

für den arduino ist z.b. im multiwii fluglagecontroller eine kompass-kalibrierung enthalten:
http://www.multiwii.com/wiki/index.php?title=compass_calibration
hinweis: der kompass muss während der 30-sekündigen kalibrierungsphase fleißig um alle drei achsen gedreht werden, dass werte aus möglichst viele verschiedenen raumlagen eingesammelt werden können, oder die kalibrierung wird vermurkst.

hier ist auch eine anleitung, wie du die werte der kalibrierungsmatrix ermitteln kannst, indem du die rohdaten-messwerte erst aufzeichnest und dann mit einem pc-programm verarbeiten läßt:
http://sailboatinstruments.blogspot.de/2011/08/improved-magnetometer-calibration.html
die ermittelte matrix muß dann noch invertiert werden. ich habe mir das aber noch nicht genau angesehen, mit welchen formeln dann aus der invertierten kalibrierungsmatrix die kalibrierten werte des magnetsensors errechnet werden.

und erst wenn du die korrekten(!) und kalibrierten sensorwerte hast, kannst du dann deine formeln verwenden, um daraus eine neigungskompensierte himmelsrichtung zu ermitteln.

Arduino Forum > International > Deutsch (Moderator: uwefed) > Neigungskompensierter Kompass (tilt compensated magnetometer/compass)

arduino