Tilt Heading Berechnung arbeitet nicht korrekt

[Ritchie]

Hallo Zusammen,

hatte eine Lange Pause an meinem Robby (beruflich/private Ursache), bin aber hoffentlich jetzt wieder dran, mein letztes Problem zu lösen.

Ich arbeite derzeit an meiner Heading Berechnung für eine verbesserte Navigation.

Aus diesem Grund habe ich derzeit nur einen kleinen Testaufbau auf dem Schreibtisch mittels eines "Atomic IMU6".

Hierbei habe ich festgestellt, das sich meine Heading Werte auch ändern, wenn ich die Platine nur im Roll oder Pitch ändere.

Ich verwende hierbei die folgenden Tilt- Kompensation:



temp=sqrt(Atomic_accel[ROLL] * Atomic_accel[ROLL]+ Atomic_accel[PITCH] * Atomic_accel[PITCH] + Atomic_accel[HEADING] * Atomic_accel[HEADING]);
Atomic_Xnorm = Atomic_accel[ROLL]/temp;
Atomic_Ynorm = Atomic_accel[PITCH]/temp;
Angle[PITCH] = asin(Atomic_Xnorm);
Angle[ROLL]= -asin(Atomic_Ynorm/cos(Angle[PITCH])); float magXcomp = LM303_magnet[ROLL]*cos(asin(Atomic_Xnorm))+LM303_magnet[HEADING]*sin(Angle[PITCH]);
float magYcomp = LM303_magnet[ROLL]*sin(asin(Atomic_Ynorm/cos(Angle[PITCH])))*sin(asin(Atomic_Xnorm)) +
LM303_magnet[PITCH]*cos(asin(Atomic_Ynorm/cos(Angle[PITCH])))-
LM303_magnet[HEADING]*sin(asin(Atomic_Ynorm/cos(Angle[PITCH]))) * cos(asin(Atomic_Xnorm));

Angle[HEADING] = atan2(magYcomp,magXcomp);

Quelle der Tilt Infos:
Teilweise habe ich die Infos von dieser Seite: http://ozzmaker.com/compass2/
Ebenso
http://www.dfrobot.com/wiki/index.php/LSM303_Tilt_Compensated_Compass%28SEN0079%29

Ist den die Annahme richtig, das diese Kompensation eine Änderung von Pitch/Roll auf die
Berechnung von Heading entsprechend anpasst. (Wenn Pitch oder Roll < 45°).

Für Tips zur Beseitigung des Fehlers bin ich dankbar.

Edit: oder muss ich diese Fallbetrachtung noch einbringen:

31402

Edit 1: So Ich habe die Ausrichtung beider Boards jetzt auch angepasst und hier mal als Bild dargestellt.
31409

Edit2:
Hier einige Datenpakete

Datenpaket beider Sensoren für die horizontale Lage


Atomic_accel[ROLL] Atomic_accel[PITCH] Atomic_accel[HEADING] Angle[ROLL] Angle[PITCH] Angle[HEADING] LM303_magnet[ROLL] LM303_magnet[PITCH] LM303_magnet[HEADING]
0.0000 -0.0108 -2.5806 0.0042 0.0000 0.5994 411.2640 279.9200 265.1440
-0.0054 -0.0108 -2.5806 0.0042 -0.0021 0.5964 412.4160 278.4800 263.4720
-0.0108 0.0000 -2.5753 -0.0000 -0.0042 0.5950 410.5440 277.1200 263.5600
-0.0054 0.0000 -2.5699 -0.0000 -0.0021 0.5956 411.3360 278.4000 262.1520
-0.0054 0.0000 -2.5699 -0.0000 -0.0021 0.5920 411.3360 276.2400 257.0480
-0.0054 -0.0054 -2.5753 0.0021 -0.0021 0.5996 411.4800 280.3200 264.3520
-0.0054 -0.0054 -2.5753 0.0021 -0.0021 0.5954 411.8400 278.0800 264.3520
-0.0161 0.0000 -2.5699 -0.0000 -0.0063 0.5965 411.1920 278.0800 264.8800
0.0108 -0.0054 -2.5699 0.0021 0.0042 0.5954 411.6240 279.0400 263.1200

Datenpaket beider Sensoren für die Schraglage Roll 90 Grad


Atomic_accel[ROLL] Atomic_accel[PITCH] Atomic_accel[HEADING] Angle[ROLL] Angle[PITCH] Angle[HEADING] LM303_magnet[ROLL] LM303_magnet[PITCH] LM303_magnet[HEADING]
0.9731 0.0108 -2.5591 -0.0042 0.3634 1.7547 -47.3040 511.1200 -143.3520
0.9624 0.0108 -2.5591 -0.0042 0.3597 1.7557 -47.6640 509.2800 -144.1440
0.9677 0.0108 -2.5591 -0.0042 0.3615 1.7569 -48.0240 509.7600 -144.5840
0.9409 0.0161 -2.5591 -0.0063 0.3523 1.7536 -47.4480 508.4000 -143.7040
0.9731 0.0000 -2.5699 -0.0000 0.3620 1.7575 -48.1680 509.1200 -144.3200
0.9677 0.0108 -2.5645 -0.0042 0.3608 1.7562 -48.6000 509.2000 -142.0320
0.9677 0.0108 -2.5591 -0.0042 0.3615 1.7554 -48.8880 509.0400 -139.7440
0.9731 0.0000 -2.5645 -0.0000 0.3627 1.7584 -47.5200 508.3200 -146.6960
0.9731 0.0108 -2.5591 -0.0042 0.3634 1.7563 -47.8080 509.6800 -143.7040

Datenpaket beider Sensoren für die Schraglage Pitch 90 Grad


Atomic_accel[ROLL] Atomic_accel[PITCH] Atomic_accel[HEADING] Angle[ROLL] Angle[PITCH] Angle[HEADING] LM303_magnet[ROLL] LM303_magnet[PITCH] LM303_magnet[HEADING]
0.0269 1.0161 -1.5699 -0.5744 0.0144 0.1066 380.6640 132.2400 134.4640
0.0161 1.0108 -1.5699 -0.5720 0.0086 0.1084 380.6640 132.9600 133.0560
0.0215 1.0054 -1.5645 -0.5712 0.0116 0.1132 381.0240 134.3200 132.9680
0.0108 1.0054 -1.5699 -0.5696 0.0058 0.1205 380.7360 135.4400 128.0400
0.0215 1.0054 -1.5753 -0.5681 0.0115 0.1145 380.6640 134.2400 133.0560
0.0484 1.0269 -1.5591 -0.5824 0.0259 0.1196 381.2400 137.3600 134.4640
0.0269 1.0161 -1.5591 -0.5776 0.0144 0.1194 380.3760 135.1200 128.8320
0.0269 1.0215 -1.5538 -0.5816 0.0145 0.1177 381.3840 134.6400 127.8640
0.0161 1.0161 -1.5591 -0.5776 0.0087 0.1054 381.4560 133.8400 134.5520
0.0269 1.0108 -1.5591 -0.5752 0.0145 0.1140 380.9520 135.3600 133.7600
0.0323 1.0215 -1.5538 -0.5816 0.0173 0.1113 381.5280 135.4400 134.5520
0.0323 1.0215 -1.5591 -0.5800 0.0173 0.1067 381.1680 133.6000 135.6080
0.0323 1.0215 -1.5591 -0.5800 0.0173 0.1107 381.4560 134.0800 133.4080


Die Datenpaket für 90 Grad sind nicht korrekt, da die Tilt Routinen nur bis 45 Grad arbeiten. Hier kommen am Wochenende nochmals neue Versuchsdaten hinzu.

Für weitere Infos wäre ich dankbar.

Viele Grüße

R.

[Ritchie]

Hallo Zusammen,

hat den keiner eine Idee, woran es liegen kann ?

Ich habe jetzt auch mal diese Routinen verwendet, welche mir den gleichen Effekt gezeigt haben:




void getAtomicTiltHeading(void)
{
Angle[PITCH] = asin(-Atomic_accel[X]);
Angle[ROLL]= asin(Atomic_accel[Y]/cos(Angle[PITCH]));

float magXcomp = (LM303_magnet[X]*cos(Angle[PITCH]))+(LM303_magnet[Z]*sin(Angle[PITCH]));
float magYcomp = (LM303_magnet[X]*sin(Angle[ROLL])*sin(Angle[PITCH]))+(LM303_magnet[Y]*cos(Angle[ROLL]))-(LM303_magnet[Z]*sin(Angle[ROLL]) * cos(Angle[PITCH]));

Angle[HEADING] = atan2(magYcomp,magXcomp) + M_PI_2;
}

//================================================== ================
// calculate the actual heading of LM300 Chip
//================================================== ================

void getLM303TiltHeading(void)
{
LM303[PITCH] = asin(-LM303_accel[X]);
LM303[ROLL]= asin(LM303_accel[Y]/cos(LM303[PITCH]));

float magXcomp = (LM303_magnet[X]*cos(LM303[PITCH]))+(LM303_magnet[Z]*sin(LM303[PITCH]));
float magYcomp = (LM303_magnet[X]*sin(LM303[ROLL])*sin(LM303[PITCH]))+(LM303_magnet[Y]*cos(LM303[ROLL]))-(LM303_magnet[Z]*sin(LM303[ROLL]) * cos(LM303[PITCH]));

LM303[HEADING] = atan2(magYcomp,magXcomp) + M_PI_2;
}


Ausserdem dachte ich mal, ich verwendet die Standard Routine LSM303-Klasse von Pololu zu Testzwecken. Hier habe ich aber Probleme mit der Klasse Vector. Dachte AVR Studio 7 wäre eine Lösung, musste aber auch hier passen.

Viele Grüße

Richard

[Ritchie]

Hallo Zusammen,

ich habe mir jetzt die ganze Formel nochmals angesehen und wenn ich das richtig sehe stimmt die Umsetzung der Formel
laut Application note.

Quellcode:


//================================================== ================
// Get the Pitch / Row and heading
//================================================== ================

void getAtomicTiltHeading(void)
{
float Atomic_Xnorm;
float Atomic_Ynorm;
float temp;

temp=sqrt(Atomic_accel[X] * Atomic_accel[X]+ Atomic_accel[Y] * Atomic_accel[Y] + Atomic_accel[Z] * Atomic_accel[Z]);

Atomic_Xnorm = Atomic_accel[X]/temp;
Atomic_Ynorm = Atomic_accel[Y]/temp;

Angle[PITCH] = asin(-Atomic_Xnorm);
Angle[ROLL]= asin(Atomic_Ynorm/cos(Angle[PITCH]));

float AccXcomp = LM303_magnet[X]*cos(Angle[PITCH])+LM303_magnet[Z]*sin(Angle[PITCH]);
float AccYcomp = LM303_magnet[Y]*sin(Angle[ROLL])*sin(Angle[PITCH])+LM303_magnet[Y]*cos(Angle[ROLL])-LM303_magnet[Z]*sin(Angle[ROLL])*cos(Angle[PITCH]);

Angle[HEADING] = atan2(AccYcomp,AccXcomp);
}


Formel:
31429

Eigentlich kann es jetzt nur noch an den Werten der Sensoren und deren Ausrichtung liegen (oder ?) .

Keine eine Idee wo mein Fehler sein könnte ?

Viele Grüße

R.

[Rabenauge]

Nur raten, da ich das Board nicht kenne: Hard-Iron-Kalibrierung gemacht?
Der bekannte HMC 5883l beispielsweise ist ohne diese nahezu nutzlos. Schon aufm Steckbrett funktioniert er nur halbwegs brauchbar, wenn man ihn schön in die Mitte steckt.

[Ritchie]

Hallo Zusammen,

ich habe das mit folgender Routine bis jetzt immer versucht, bin mir aber nicht sicher ob das so richtig ist.



void getLSM303_CalibrationValues(void)
{
int16_t MinValues[3],MaxValues[3];
int16_t Average[3],i,y,iValue;
float Average_rad;

for (i=0;i<3;i++) // Setup the Min/max values
{
MaxValues[i]= -32760;
MinValues[i] = 32760;
}

Serial_string_printf("\n\nStart of calibration Move around all directions\n\r");
Serial_string_printf("Press any key to abort the calibration\n\r");

read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMEN T + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magne t_raw));

write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_L_M, 0); // Clear the existing Offset value of the chip
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_H_M, 0);
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_L_M, 0); // Clear the existing Offset value of the chip
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_H_M, 0);
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_L_M, 0); // Clear the existing Offset value of the chip
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_H_M, 0);

for(i=0;i<32000;i++)
{
read_LSM303();

// read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMEN T + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magne t_raw));

if( (i % 100) == 0 ) // show every 100 readings
STATUS_LED_TOOGLE; // Show Updating of values by toggle bit

if( (i % 1000) == 0 ) // show every 1000 readings
{
Serial_string_printf("Loop Value : ");
Serial_int16_printf(i);
Serial_string_printf("\r");
}

for (y=0;y<3;y++) // Setup the Min/max values
{
if (LM303_magnet[y] > MaxValues[y] ) // Find the max value of the Sensor
MaxValues[y] = LM303_magnet[y];

if (LM303_magnet[y] < MinValues[y] ) // Find the min value of the sensor
MinValues[y] = LM303_magnet[y];
}
}

for (i=0;i<3;i++) // Average distance from the center
{
iValue =((MinValues[i] + MaxValues[i]) / 2); // get the average distance from the center
MaxValues[i] = MaxValues[i] - iValue ; //
MinValues[i] = MinValues[i] - iValue ;

Serial_string_printf("\r\n Min: ");
Serial_float_printf(MinValues[i]);
Serial_string_printf(" Max: ");
Serial_float_printf(MaxValues[i]);
}

Average_rad =0.0;
for (i=0;i<3;i++) // Setup the Min/max values
{
Average[i] = (MaxValues[i] + (-1*MinValues[i]) / 2 );
LM303_magnet_offset[i] = (MaxValues[i] + MinValues[i]) / 2 ;
Average_rad += (float) Average[i];
}

Average_rad /= 3.0;

//Finally calculate the scale factor by dividing average radius by average value for that axis.

for (i=0;i<3;i++)
{
LM303_magnet_scaling[i] = Average_rad / Average[i];
}

// Store the final values in the chip

initLSM303_Offset();
cli();
eeprom_write_block((void*)&LM303_magnet_scaling, (void *) LSM303_MAG_GAIN, sizeof(LM303_magnet_scaling));
eeprom_write_block((void*)&LM303_magnet_offset, (void *) LSM303_MAG_OFFSET, sizeof(LM303_magnet_offset));
sei();

// Gain of the value

Serial_string_printf("\n\n\rCalibration is finished\n\n\r");
}


Init des Offset vom Chip direkt:



void initLSM303_Offset(void)
{
int8_t LowByte,HighByte;

LowByte = (int8_t) (LM303_magnet_offset[0] & 0xFF);
HighByte = (int8_t) (LM303_magnet_offset[0] >> 8);
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_L_M, LowByte); // Store the new value of the X offset
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_X_H_M, HighByte);
LowByte = (int8_t) (LM303_magnet_offset[1] & 0xFF);
HighByte = (int8_t) (LM303_magnet_offset[1] >> 8);
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_L_M, LowByte); // Store the new value of the Y offset
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Y_H_M, HighByte);
LowByte = (int8_t) (LM303_magnet_offset[2] & 0xFF);
HighByte = (int8_t) (LM303_magnet_offset[2] >> 8);
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_L_M, LowByte); // Store the new value of the Z offset
write_Byte_i2c(LM303_ADR, ADDR_OFFSET_Z_H_M, HighByte);
}



gelesen wird dann hiermit


void read_LSM303(void)
{
int8_t i;

read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMEN T + ADDR_OUT_X_L_M,LM303_magnet_raw,sizeof(LM303_magne t_raw));
read_Array_LH_I2c(LM303_ADR,ADDR_READ_AUTOINCREMEN T + ADDR_OUT_X_L_A,LM303_accel_raw,sizeof(LM303_accel_ raw));

for(i=0;i<3;i++)
{
LM303_magnet[i] = (float)(LM303_magnet_raw[i]) * LM303_magnet_scaling[i] * LSM303D_M_GN_2MG_GAUSS;
LM303_accel[i] = LM303_accel_raw[i] * LSM303D_LA_SO_2G;
}
}


Wenn ich dann die grafische Kontrolle mache, sehe ich eigentlich, das es noch nicht sauber zentriert ist.
31452

Wo ist mein Fehler ?

Ich kann die Werte auch direkt eingeben und komme aber nicht zu einem "sauberen" Kreis" in der Grafik.

In welcher Einheit arbeitet Ihr eigentlich für die einzelnen Sensoren ?
mg und mgauss oder in g und gauss

Viele Grüße

R.

[Rabenauge]

Eine Hard-Iron-Kalibrierung kannst du nicht so einfach in Software machen, das funktioniert nicht.
Dabei nimmt man zuerst mal die Störungen auf, indem man das Ganze in allen drei Raumachsen ausmisst.
Aus diesen Daten können dann Korrekturwerte berechnet werden, und _die_ werden dann in der eigentlichen Software benutzt.
Alles andere ergibt keine brauchbaren Ergebnisse.
Schau dir _das_ mal an: http://diydrones.com/profiles/blogs/advanced-hard-and-soft-iron-magnetometer-calibration-for-dummies

[Ritchie]

Vielen Dank für den Link.

Hier werde ich mir das ganze mal ansehen und versuchen auf meinen Chip anzuwenden.

Gruss R.

[Ritchie]

Hallo Sly,


hast Du den "Magviewer" ans rennen bekommen. Bei mir läuft zwar das Berechnungsprogramm, aber nicht der MagViewer. Ich habe das in einer VM laufen unter Windows 7 Pro 64.

Leider konnte ich keinen Quellcode vom Magviewer finden.

Habe es auch mal mit der Methode von

http://www.germersogorb.de/html/kalibrierung_des_hcm5883l.html

probiert und habe hier das Problem, das die Korrekturwerte nicht den gewünschten Effekt haben. Ursache suche ich noch.

Viele Grüße

R.

[Rabenauge]

Der MagViewer _ging_ unter XP problemlos.
Da man den aber nun nicht unbedingt braucht, hab ich das nicht mehr weiter verfolgt, da ich komplett auf Linux umgestiegen bin- in der VirtualBox lief der auch nicht.
Muss ich mal testen, bei Gelegenheit. Ich starte Windows nur noch ungerne, ich komm da immer vor Lachen nich zu was vernünftigem. :D
Kann also bissel dauern...letztendlich aber ist der nicht wichtig, man kann sich ja die Kursdaten mal eben seriell ausgeben lassen und schauen, ob das hin kommt.
Hoch präzise wurden die Kompass-Ausgaben bei mir auch nie (+-3 Grad), aber wenn man zusätzlich nen GPS hat, braucht man die auch nicht genauer.

[Ritchie]

Hallo Sly,

ich selber verwende auch nur noch Linux als "Hauptbetriebssystem". Für die Windowsprogramme, wie z.b. den MagViewer habe ich dann VMWare im Einsatz. Ich bin derzeitig
mein Testaufbau nochmals neu am aufbauen (diesmal ohne jede Schraube/Metall) nur zu Testzwecken, um zu sehen, ob meine Kalibierung und der Sensor arbeitet.
Ich werden den MagViewer auch mal unter XP prüfen.

Da das ganze später in ein Kettenfahrzeug kommen soll, habe ich hier dann die Aufgabe, das ganze so zu positionieren, das es auch geht.
Da ich in der Wohnung mein Aufgabengebiet habe, fällt GPS leider weg. (Will auch keine Braken haben)

Derzeit sind halt meine Winkelfehler durch die Aufsummierung von Schlupf und Co zu gross. Hier kommt dann als nächster Schritt noch ein Kalman
ins Spiel. Aber zuerst müssen die Sensoren für sich "relativ" gut arbeiten.

Ich poste meine Ergebnis dann hier.

Viele Grüße
R.