Dreidimensionale Ausgleichsgerade

13.07.2009, 16:29

mathenull123

Dreidimensionale Ausgleichsgerade

Hallo alle miteinander!

Ich bearbeite gerade ein Projekt und bin dabei auf folgende Problematik gestoßen:
Ich habe verrauschte Messdaten, das sind dreidimensionale Positionen im Raum. Das zu lokalisierende Flugzeug bewegt sich relativ kontinuierlich vorwärts (macht aber auch Kurven). Für eine Abschätzung, wo sich das Flugzeug gerade befindet, möchte ich eine Ausgleichsgerade durch die letzten x (sagen wir mal 4 bis 8) gemessenen Positionen legen und für die Positionsschätzung auf dieser den zwischen zwei Messungen zurückgelegten Weg auftragen. Bisher erzeuge ich dafür eine Gerade aus dem letzten Punkt und sagen wir einmal dem fünftletzten Punkt, was aber eine extrem schlechte Methode ist.

Ich habe mal in einem anderen Projekt ein bisschen mit Pseudoinversen herumgespielt. Da hat man ja eigentlich fast das gleiche Problem, bloß dass man da eine Ebene und keine Gerade durchlegt.

Hat vielleicht jemand von Euch (mein Guru Gott

) eine entsprechende Methode für mich, die der Berechnung einer Pseudoinversen ähnelt, bei der aber die dreidimensionale Ausgleichsgerade (die Abstände aller Punkte zur Gerade sollen minimiert werden) herauskommt?

Ich wäre sehr dankbar, wenn mir jemand weiterhelfen könnte!

Besten Dank

14.07.2009, 03:42

frank09

Auf diesen Beitrag antworten »

1. Errechne einen dreidimensionalen Mittelwert, durch den die Gerade laufen soll.
Wenn du von jedem der 4-8 Punkte den Mittelwert subtrahierst, liegt der Mittelwert im Ursprung, also M(0|0|0).
2. Statt die Abstände zu minimieren, minimiert man die Quadrate der Abstände.
So spart man sich übrigens auch die Wurzeln.
3. Stell dir vor, die X-Achse soll deine Ausgleichgerade werden, d.h. die Summe
aus quadrierter Y- und quadr. Z-Koord. soll minimal werden
4. Um die Y-Koordinaten zu minmieren drehst du mit geeigneter Matrix so lange
um die Z-Achse bis die Summe der Quadrate der Y-Koord. minimal wird (Z-Koord. bleibt dabei unverändert)
5. Um nun die Z-Koordinaten zu minimieren drehst du mit geeigneter Matrix so lange um die Y-Achse bis die Summe der Quadrate der Z-Koord. minimal wird (Y-Koord. bleibt dabei unverändert)
6. Nun musst du deine Ausgleichgerade X-Achse mir den Inversen der Drehmatrizen multipizieren und dann noch durch Addition des Mittelwertes deinen ursprüglichen Punkten anpassen.

Die Minimierungen errechnest durch durch Ableiten nach dem Drehwinkel.

14.07.2009, 07:46

mathenull123

Auf diesen Beitrag antworten »

Danke schonmal!
Hallo Frank,

Vielen Dank für den Vorschlag! Ich werde das gleich mal ausprobieren.

Zitat:

Original von frank09
Die Minimierungen errechnest durch durch Ableiten nach dem Drehwinkel.

Das Ableiten klingt mir nach einer analytischen Lösung, richtig?
Ich muss den Berechnungsaufwand so gering wie möglich halten, wäre also schön, wenn es so ist. Hast Du diese Methode denn schonmal ausprobiert? Wenn ja wäre ein kleiner Codefetzen der absolute Hammer smile

.

Ich versuche mal, das zu implementieren und melde mich bei Bedarf dann nochmal.

Schönen Gruß und herzlichen Dank!

15.07.2009, 01:22

frank09

Auf diesen Beitrag antworten »

Die Drehmatrix einer Drehung um die Z-Achse (Drehwinkel $\begin{eqnarray*} \alpha \end{eqnarray*}$ ) sieht ja so aus:
$\begin{eqnarray*} M_z=\begin{pmatrix} cos\alpha & sin\alpha & 0 \\ -sin\alpha & cos\alpha & 0 \\ 0 & 0 & 1 \end{pmatrix} \end{eqnarray*}$
Angewandt auf einen Punkt P(x|y|z) bzw. deren Ortsvektor ergibt sich:
$\begin{eqnarray*} M_z\cdot \begin{pmatrix} x \\ y \\ z \end{pmatrix}=\begin{pmatrix} xcos\alpha +ysin\alpha \\ -xsin\alpha +ycos\alpha \\ z \end{pmatrix} \end{eqnarray*}$
die "neue" Y-Koordinate y' soll im Quadrat minimiert werden:
$\begin{eqnarray*} y'=-xsin\alpha +ycos\alpha \end{eqnarray*}$
$\begin{eqnarray*} y'^2=x^2sin^2\alpha -2xysin\alpha cos\alpha+y^2cos^2\alpha \end{eqnarray*}$
der Einfachheit wegen werden die Koeffizienten wie folgt benannt:
$\begin{eqnarray*} a=x^2 \end{eqnarray*}$ bzw. $\begin{eqnarray*} x_1^2+x_2^2+x_3^2...+x_n^2 \end{eqnarray*}$ ,wenn man mehrere Punkte hat.
$\begin{eqnarray*} b=xy \end{eqnarray*}$ bzw. $\begin{eqnarray*} x_1y_1+...+x_ny_n \end{eqnarray*}$
$\begin{eqnarray*} c=y^2 \end{eqnarray*}$ bzw. $\begin{eqnarray*} y_1^2+y_2^2+y_3^2...+y_n^2 \end{eqnarray*}$

$\begin{eqnarray*} y'^2=asin^2\alpha -2bsin\alpha cos\alpha+ccos^2\alpha \end{eqnarray*}$
nun ableiten und gleich null setzen:
$\begin{eqnarray*} 2asin\alpha cos\alpha+4bsin^2\alpha-2b-2csin\alpha cos\alpha=0 \end{eqnarray*}$
$\begin{eqnarray*} (a-c)sin\alpha cos\alpha=b-2bsin^2\alpha \end{eqnarray*}$
quadrieren, um cos aus dem Produkt herauszubekommen:
$\begin{eqnarray*} (a-c)^2sin^2\alpha-(a-c)^2sin^4\alpha=b^2-4b^2sin^2\alpha+4b^2sin^4\alpha \end{eqnarray*}$
$\begin{eqnarray*} -[(a-c)^2+4b^2]sin^4\alpha+[(a-c)^2+4b^2]sin^2\alpha-b^2=0 \end{eqnarray*}$
bi-quadratische Gleichung mit 2 Lösungen:
$\begin{eqnarray*} sin^2\alpha=\frac{1}{2}\pm \sqrt{\frac{1}{4}-\frac{b^2}{(a-c)^2+4b^2}} \end{eqnarray*}$
Eine der Lösungen ist das Maximum, die andere das Minimum. Am besten jeweils $\begin{eqnarray*} y'^2 \end{eqnarray*}$ ausrechnen. Die Drehmatrix um die y-Achse ist diese:
$\begin{eqnarray*} M_y=\begin{pmatrix} cos\beta & 0 & sin\beta \\ 0 & 1 & 0 \\ -sin\beta &0 & cos\beta \end{pmatrix} \end{eqnarray*}$
Zu minimieren nun( $\begin{eqnarray*} x'=xcos\alpha+ysin\alpha \end{eqnarray*}$ ):
$\begin{eqnarray*} z'^2=x'^2sin^2\beta -2x'zsin\beta cos\beta+z^2cos^2\beta \end{eqnarray*}$
geht genauso.Die Matrix für beide Drehungen sieht dann so aus:
$\begin{eqnarray*} M_y\cdot M_z=\begin{pmatrix} cos\alpha cos\beta & sin\alpha cos\beta & sin\beta \\ -sin\alpha & cos\alpha & 0 \\ -cos\alpha sin\beta & -sin\alpha sin\beta & cos\beta \end{pmatrix} \end{eqnarray*}$
Für das Inverse nimmst du einfach (statt $\begin{eqnarray*} \alpha \end{eqnarray*}$ und $\begin{eqnarray*} \beta \end{eqnarray*}$ ) $\begin{eqnarray*} -\beta \end{eqnarray*}$ und $\begin{eqnarray*} -\alpha \end{eqnarray*}$
Somit ist der Richtungsvektor deiner Ausgleichsgeraden:
$\begin{eqnarray*} (M_y\cdot M_z)^{-1}\cdot\begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix}=\begin{pmatrix} cos(-\beta) cos(-\alpha) \\-sin(-\beta) \\ -cos(-\beta) sin(-\alpha) \end{pmatrix} \end{eqnarray*}$
Und vergiss, nicht zuerst die Punkte um ihren Mittelwert zu verschieben.
Sieht im Programmcode sicher sehr viel einfacher aus.

16.07.2009, 02:43

frank09

Auf diesen Beitrag antworten »

Geht noch einfacher.
Da das oben beschriebene Verfahren sehr rechenintensiv scheint, habe ich nach einer einfacheren Möglichkeit gesucht und bin auf diesen Link gestoßen:
http://www.stud.uni-karlsruhe.de/~uabzr/info4/tutorium3.pdf
Danach kannst du so verfahren:
Nachdem du von deinen Punkten den Mittelwert abgezogen hast, schreibst du diese spaltenweise in die Matrix P
z.B. P_1(1|2|3),P_2(3|5|4),P_3(2|4|6),P_4(2|5|7) Mittelwert m=(2|4|5)
$\begin{eqnarray*} P=\left((P_1-m)(P_2-m)(P_3-m)(P_4-m)\right)=\begin{pmatrix} -1& 1 & 0 & 0 \\ -2&1 &0 & 1 \\ -2&-1 &1 &2 \end{pmatrix} \end{eqnarray*}$
Nun mit sich selbst in transponierter Form multiplizieren ergibt Kovarianzmatrix:
$\begin{eqnarray*} PP^t=\begin{pmatrix} -1& 1 & 0 & 0 \\ -2&1 &0 & 1 \\ -2&-1 &1 &2 \end{pmatrix}\cdot \begin{pmatrix} -1& -2 & -2 \\1&1 &-1 \\ 0 &0 &1\\0&1&2 \end{pmatrix}=\begin{pmatrix} 2 & 3 & 1 \\ 3 & 6 & 5 \\ 1 & 5 & 10 \end{pmatrix} \end{eqnarray*}$
Und nun ist der Richtungsvektor deiner Ausgleichsgeraden "einfach nur" der Eigenvektor der Kovarianzmatrix mit dem größten Eigenwert. Der lässt sich numerisch leicht mit der Von-Mises-Iteration (http://goblor.de/wp/wp-content/uploads/2...ion-handout.pdf) bestimmen. Sie konvergiert sehr schnell und man kommt schon nach 5 Durchläufen auf zwei Nachkommastellen Genauigkeit:
$\begin{eqnarray*} \vec{RV} =\begin{pmatrix} 2.94\\ 8.04 \\10.98 \end{pmatrix} \end{eqnarray*}$

code:

1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:

double M [3][3]={{2,3,1},{3,6,5},{1,5,10}};//Kovarianzmatrix 
double vec1[3]={1,0,0};//Startwert
double vec2[3];
double betrag;
   for(int n = 0; n <10;n++)// Anzahl Iterationen
{vec2[0]=M[0][0]*vec1[0]+M[0][1]*vec1[1]+M[0][2]*vec1[2];
vec2[1]=M[1][0]*vec1[0]+M[1][1]*vec1[1]+M[1][2]*vec1[2];
vec2[2]=M[2][0]*vec1[0]+M[2][1]*vec1[1]+M[2][2]*vec1[2];
betrag=sqrt(vec2[0]*vec2[0]+vec2[1]*vec2[1]+vec2[2]*vec2[2]);
vec1[0]=vec2[0]/betrag;
vec1[1]=vec2[1]/betrag;
vec1[2]=vec2[2]/betrag;
}

Neue Frage »

Antworten »

Dreidimensionale Ausgleichsgerade

Verwandte Themen