diff --git a/Vorbereitungen_Fabian/Tests.ipynb b/Vorbereitungen_Fabian/Tests.ipynb new file mode 100644 index 0000000..1477723 --- /dev/null +++ b/Vorbereitungen_Fabian/Tests.ipynb @@ -0,0 +1,33 @@ +{ + "cells": [ + { + "metadata": {}, + "cell_type": "code", + "outputs": [], + "execution_count": null, + "source": "", + "id": "32199e6b9a0ba953" + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 2 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython2", + "version": "2.7.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/Vorbereitungen_Fabian/Transformation_Helmert.py b/Vorbereitungen_Fabian/Transformation_Helmert_V1.py similarity index 53% rename from Vorbereitungen_Fabian/Transformation_Helmert.py rename to Vorbereitungen_Fabian/Transformation_Helmert_V1.py index b38e72b..0bb62c2 100644 --- a/Vorbereitungen_Fabian/Transformation_Helmert.py +++ b/Vorbereitungen_Fabian/Transformation_Helmert_V1.py @@ -78,18 +78,32 @@ for punkt in liste_Punkte: -dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1') +dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1, xp2, yp2, zp2, xp3, yp3, zp3, xp4, yp4, zp4, xp5, yp5, zp5 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1 xp2 yp2 zp2 xp3 yp3 zp3 xp4 yp4 zp4 xp5 yp5 zp5') #print(Translation[0]) -zahlen = {dX: Translation[0], dY: Translation[1], dZ: Translation[2], m: m0, q0: q0_wert, q1: q1_wert, q2: q2_wert, q3: q3_wert, xp1: p1[0], yp1: p1[1], zp1: p1[2]} +zahlen = {dX: Translation[0], dY: Translation[1], dZ: Translation[2], m: m0, q0: q0_wert, q1: q1_wert, q2: q2_wert, q3: q3_wert, xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} #print(zahlen[zp1]) f = sp.Matrix( - [[dX + m * (xp1 * (1 - 2 * (q2**2 + q3**2)) + yp1 * (2 * (q1 * q2 - q0 * q3)) + zp1 * (2 * (q0 * q2 - q1 * q3)))], + [[dX + m * (xp1 * (1 - 2 * (q2**2 + q3**2)) + yp1 * (2 * (q1 * q2 - q0 * q3)) + zp1 * (2 * (q0 * q2 + q1 * q3)))], [dY + m * (xp1 * (2 * (q1 * q2 + q0 * q3)) + yp1 * (1 - 2 * (q1**2 + q3**2)) + zp1 * (2 * (q2 * q3 - q0 * q1)))], - [dZ + m * (xp1 * (2 * (q1 * q3 - q0 * q2)) + yp1 * (2 * (q0 * q1 + q2 * q3)) + zp1 * (1 - 2 * (q1**2 + q2**2)))] + [dZ + m * (xp1 * (2 * (q1 * q3 - q0 * q2)) + yp1 * (2 * (q0 * q1 + q2 * q3)) + zp1 * (1 - 2 * (q1**2 + q2**2)))], + [dX + m * (xp2 * (1 - 2 * (q2**2 + q3**2)) + yp2 * (2 * (q1 * q2 - q0 * q3)) + zp2 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp2 * (2 * (q1 * q2 + q0 * q3)) + yp2 * (1 - 2 * (q1**2 + q3**2)) + zp2 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp2 * (2 * (q1 * q3 - q0 * q2)) + yp2 * (2 * (q0 * q1 + q2 * q3)) + zp2 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp3 * (1 - 2 * (q2**2 + q3**2)) + yp3 * (2 * (q1 * q2 - q0 * q3)) + zp3 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp3 * (2 * (q1 * q2 + q0 * q3)) + yp3 * (1 - 2 * (q1**2 + q3**2)) + zp3 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp3 * (2 * (q1 * q3 - q0 * q2)) + yp3 * (2 * (q0 * q1 + q2 * q3)) + zp3 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp4 * (1 - 2 * (q2**2 + q3**2)) + yp4 * (2 * (q1 * q2 - q0 * q3)) + zp4 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp4 * (2 * (q1 * q2 + q0 * q3)) + yp4 * (1 - 2 * (q1**2 + q3**2)) + zp4 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp4 * (2 * (q1 * q3 - q0 * q2)) + yp4 * (2 * (q0 * q1 + q2 * q3)) + zp4 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp5 * (1 - 2 * (q2**2 + q3**2)) + yp5 * (2 * (q1 * q2 - q0 * q3)) + zp5 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp5 * (2 * (q1 * q2 + q0 * q3)) + yp5 * (1 - 2 * (q1**2 + q3**2)) + zp5 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp5 * (2 * (q1 * q3 - q0 * q2)) + yp5 * (2 * (q0 * q1 + q2 * q3)) + zp5 * (1 - 2 * (q1**2 + q2**2)))], + + ] ) @@ -100,8 +114,28 @@ A = A_ohne_zahlen.subs(zahlen) #print(J_zahlen.evalf(n=3)) # Parameterschätzung -P = sp.eye(3) +schwellenwert = 1e-3 +alle_kleiner = True + +x0 = sp.Matrix([zahlen[dX], zahlen[dY], zahlen[dZ], zahlen[m], zahlen[q0], zahlen[q1], zahlen[q2], zahlen[q3]]) +P = sp.eye(15) N = A.T * P * A -l = sp.Matrix([p1[0], p1[1], p1[2]]) +l = sp.Matrix([p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[1], p3[2], p4[0], p4[1], p4[2], p5[0], p5[1], p5[2]]) +# ToDo: Prüfen, ob n mit l oder mit dl! n = A.T * P * l -print(n.evalf(n=3)) \ No newline at end of file +Qxx = N.evalf(n=30).inv() +dx = Qxx * n +x = x0 + dx + + +print(x0.evalf(n=3)) +print(dx.evalf(n=3)) +print(x.evalf(n=3)) + +for i in range (dx.rows): + wert = float(dx[i]) + if abs(wert) >= schwellenwert: + print("Warnung") + alle_kleiner = False + +if alle_kleiner: print("Ausgleichung fertig!") \ No newline at end of file diff --git a/Vorbereitungen_Fabian/Transformation_Helmert_V2.py b/Vorbereitungen_Fabian/Transformation_Helmert_V2.py new file mode 100644 index 0000000..cf0ac93 --- /dev/null +++ b/Vorbereitungen_Fabian/Transformation_Helmert_V2.py @@ -0,0 +1,155 @@ +import sympy as sp +from sympy.algebras.quaternion import Quaternion + +#ToDo: Achtung: Die Ergebnisse sind leicht anders, als in den Beispielrechnung von Luhmann (Rundungsfehler bei Luhmann?) +#ToDo: Automatische Ermittlung der Anzahl Nachkommastellen für Test auf Orthonormalität integrieren! +#Beipsiel aus Luhmann S. 76 +# Ausgangssystem +p1 = sp.Matrix([110, 100, 110]) +p2 = sp.Matrix([150, 280, 100]) +p3 = sp.Matrix([300, 300, 120]) +p4 = sp.Matrix([170, 100, 100]) +p5 = sp.Matrix([200, 200, 140]) + +# Zielsystem +P1 = sp.Matrix([153.559, 170.747, 150.768]) +P2 = sp.Matrix([99.026, 350.313, 354.912]) +P3 = sp.Matrix([215.054, 544.420, 319.003]) +P4 = sp.Matrix([179.413, 251.030, 115.601]) +P5 = sp.Matrix([213.431, 340.349, 253.036]) + +#1) Näherungswertberechnung +m0 = (P2 - P1).norm() / (p2 - p1).norm() + +U = (P2 - P1) / (P2 - P1).norm() +W = (U.cross(P3 - P1)) / (U.cross(P3 - P1)).norm() +V = W.cross(U) + +u = (p2 - p1) / (p2 - p1).norm() +w = (u.cross(p3 - p1)) / (u.cross(p3 - p1)).norm() +v = w.cross(u) + +R = sp.Matrix.hstack(U, V, W) * sp.Matrix.hstack(u, v, w).T + +XS = (P1 + P2 + P3) / 3 +xS = (p1 + p2 + p3) / 3 + +Translation = XS - m0 * R * xS + + +#print(m0.evalf()) +#print(R.evalf()) +#print(Translation.evalf()) + +# 2) Test auf orthonormale Drehmatrix bei 3 Nachkommastellen! +if R.T.applyfunc(lambda x: round(float(x), 3)) == R.inv().applyfunc(lambda x: round(float(x), 3)) and (R.T * R).applyfunc(lambda x: round(float(x), 3)) == sp.eye(3).applyfunc(lambda x: round(float(x), 3)) and ((round(R.det(), 3) == 1.000 or round(R.det(), 3) == -1.000)): + print("R ist Orthonormal!") +else: + print("R ist nicht Orthonormal!") + +# Testmatrix R aus Luhmann S. 66 +#R = sp.Matrix([ +# [0.996911, -0.013541, -0.077361], +# [0.030706, 0.973820, 0.225238], +# [0.072285, -0.226918, 0.971228] +#]) + +# 3) Quaternionen berechnen +# ToDo: Prüfen, ob Vorzeichen bei q0 richtig ist! +#ToDo: q0 stimmt nicht mit Luhmann überein! + +q = Quaternion.from_rotation_matrix(R) +q0_wert = q.a +q1_wert = q.b +q2_wert = q.c +q3_wert = q.d + + + +# 4) Funktionales Modell +liste_Punkte = ["P1", "P2", "P3", "P4", "P5"] +liste_unbekannte = ["dX", "dY", "dZ", "dm", "dq0", "dq1", "dq2", "dq3"] +liste_beobachtungen =[] +for punkt in liste_Punkte: + liste_beobachtungen.append(f"X_{punkt}") + liste_beobachtungen.append(f"Y_{punkt}") + liste_beobachtungen.append(f"Z_{punkt}") + + + + +dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1, xp2, yp2, zp2, xp3, yp3, zp3, xp4, yp4, zp4, xp5, yp5, zp5 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1 xp2 yp2 zp2 xp3 yp3 zp3 xp4 yp4 zp4 xp5 yp5 zp5') + +#print(Translation[0]) + +zahlen = {dX: Translation[0], dY: Translation[1], dZ: Translation[2], m: m0, q0: q0_wert, q1: q1_wert, q2: q2_wert, q3: q3_wert, xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} + +#print(zahlen[zp1]) + +f = sp.Matrix( + [[dX + m * (xp1 * (1 - 2 * (q2**2 + q3**2)) + yp1 * (2 * (q1 * q2 - q0 * q3)) + zp1 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp1 * (2 * (q1 * q2 + q0 * q3)) + yp1 * (1 - 2 * (q1**2 + q3**2)) + zp1 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp1 * (2 * (q1 * q3 - q0 * q2)) + yp1 * (2 * (q0 * q1 + q2 * q3)) + zp1 * (1 - 2 * (q1**2 + q2**2)))], + [dX + m * (xp2 * (1 - 2 * (q2**2 + q3**2)) + yp2 * (2 * (q1 * q2 - q0 * q3)) + zp2 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp2 * (2 * (q1 * q2 + q0 * q3)) + yp2 * (1 - 2 * (q1**2 + q3**2)) + zp2 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp2 * (2 * (q1 * q3 - q0 * q2)) + yp2 * (2 * (q0 * q1 + q2 * q3)) + zp2 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp3 * (1 - 2 * (q2**2 + q3**2)) + yp3 * (2 * (q1 * q2 - q0 * q3)) + zp3 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp3 * (2 * (q1 * q2 + q0 * q3)) + yp3 * (1 - 2 * (q1**2 + q3**2)) + zp3 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp3 * (2 * (q1 * q3 - q0 * q2)) + yp3 * (2 * (q0 * q1 + q2 * q3)) + zp3 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp4 * (1 - 2 * (q2**2 + q3**2)) + yp4 * (2 * (q1 * q2 - q0 * q3)) + zp4 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp4 * (2 * (q1 * q2 + q0 * q3)) + yp4 * (1 - 2 * (q1**2 + q3**2)) + zp4 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp4 * (2 * (q1 * q3 - q0 * q2)) + yp4 * (2 * (q0 * q1 + q2 * q3)) + zp4 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp5 * (1 - 2 * (q2**2 + q3**2)) + yp5 * (2 * (q1 * q2 - q0 * q3)) + zp5 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp5 * (2 * (q1 * q2 + q0 * q3)) + yp5 * (1 - 2 * (q1**2 + q3**2)) + zp5 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp5 * (2 * (q1 * q3 - q0 * q2)) + yp5 * (2 * (q0 * q1 + q2 * q3)) + zp5 * (1 - 2 * (q1**2 + q2**2)))], + + + ] +) + +A_ohne_zahlen = f.jacobian([dX, dY, dZ, m, q0, q1, q2, q3]) +A = A_ohne_zahlen.subs(zahlen) + +#print(J) +#print(J_zahlen.evalf(n=3)) + +# Parameterschätzung +schwellenwert = 1e-3 +alle_kleiner = True + +x0 = sp.Matrix([zahlen[dX], zahlen[dY], zahlen[dZ], zahlen[m], zahlen[q0], zahlen[q1], zahlen[q2], zahlen[q3]]) +P = sp.eye(15) +N = A.T * P * A +#l = sp.Matrix([p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[1], p3[2], p4[0], p4[1], p4[2], p5[0], p5[1], p5[2]]) + +R_matrix = sp.Matrix([[1 - 2 * (q2_wert**2 + q3_wert**2), 2 * (q1_wert * q2_wert - q0_wert * q3_wert), 2 * (q0_wert * q2_wert + q1_wert * q3_wert)], + [2 * (q1_wert * q2_wert + q0_wert * q3_wert), 1 - 2 * (q1_wert**2 + q3_wert**2), 2 * (q2_wert * q3_wert - q0_wert * q1_wert)], + [2 * (q1_wert * q3_wert - q0_wert * q2_wert), 2 * (q0_wert * q1_wert + q2_wert * q3_wert), 1 - 2 * (q1_wert**2 + q2_wert**2)]]) + +liste_punkte_ausgangssystem = [p1, p2, p3, p4, p5] +liste_l_berechnet_0 = [Translation + m0 * R_matrix * p for p in liste_punkte_ausgangssystem] +l_berechnet_0 = sp.Matrix.vstack(*liste_l_berechnet_0) + +l = sp.Matrix([P1[0] - p1[0], P1[1] - p1[1], P1[2] - p1[2], P2[0] - p2[0], P2[1] - p2[1], P2[2] - p2[2], P3[0] - p3[0], P3[1] - p3[1], P3[2] - p3[2], P4[0] - p4[0], P4[1] - p4[1], P4[2] - p4[2], P5[0] - p5[0], P5[1] - p5[1], P5[2] - p5[2]]) + +# ToDo: Prüfen, ob n mit l oder mit dl! +n = A.T * P * l +Qxx = N.evalf(n=30).inv() +dx = Qxx * n +x = x0 + dx + + + +print(l.evalf(n=3)) +print(l_berechnet_0.evalf(n=3)) +#print(x0.evalf(n=3)) +#print(dx.evalf(n=3)) +#print(x.evalf(n=3)) + +for i in range (dx.rows): + wert = float(dx[i]) + if abs(wert) >= schwellenwert: + #print("Warnung") + alle_kleiner = False + +if alle_kleiner: print("Ausgleichung fertig!") \ No newline at end of file diff --git a/Vorbereitungen_Fabian/Transformation_Helmert_V3.py b/Vorbereitungen_Fabian/Transformation_Helmert_V3.py new file mode 100644 index 0000000..f903d7d --- /dev/null +++ b/Vorbereitungen_Fabian/Transformation_Helmert_V3.py @@ -0,0 +1,246 @@ +import sympy as sp +from sympy.algebras.quaternion import Quaternion + +#ToDo: Achtung: Die Ergebnisse sind leicht anders, als in den Beispielrechnung von Luhmann (Rundungsfehler bei Luhmann?) +#ToDo: Automatische Ermittlung der Anzahl Nachkommastellen für Test auf Orthonormalität integrieren! +#Beipsiel aus Luhmann S. 76 +# Ausgangssystem +p1 = sp.Matrix([110, 100, 110]) +p2 = sp.Matrix([150, 280, 100]) +p3 = sp.Matrix([300, 300, 120]) +p4 = sp.Matrix([170, 100, 100]) +p5 = sp.Matrix([200, 200, 140]) + +# Zielsystem +P1 = sp.Matrix([153.559, 170.747, 150.768]) +P2 = sp.Matrix([99.026, 350.313, 354.912]) +P3 = sp.Matrix([215.054, 544.420, 319.003]) +P4 = sp.Matrix([179.413, 251.030, 115.601]) +P5 = sp.Matrix([213.431, 340.349, 253.036]) + +#1) Näherungswertberechnung +m0 = (P2 - P1).norm() / (p2 - p1).norm() + +U = (P2 - P1) / (P2 - P1).norm() +W = (U.cross(P3 - P1)) / (U.cross(P3 - P1)).norm() +V = W.cross(U) + +u = (p2 - p1) / (p2 - p1).norm() +w = (u.cross(p3 - p1)) / (u.cross(p3 - p1)).norm() +v = w.cross(u) + +R = sp.Matrix.hstack(U, V, W) * sp.Matrix.hstack(u, v, w).T + +XS = (P1 + P2 + P3) / 3 +xS = (p1 + p2 + p3) / 3 + +Translation = XS - m0 * R * xS + + +#print(m0.evalf()) +#print(R.evalf()) +#print(Translation.evalf()) + +# 2) Test auf orthonormale Drehmatrix bei 3 Nachkommastellen! +if R.T.applyfunc(lambda x: round(float(x), 3)) == R.inv().applyfunc(lambda x: round(float(x), 3)) and (R.T * R).applyfunc(lambda x: round(float(x), 3)) == sp.eye(3).applyfunc(lambda x: round(float(x), 3)) and ((round(R.det(), 3) == 1.000 or round(R.det(), 3) == -1.000)): + print("R ist Orthonormal!") +else: + print("R ist nicht Orthonormal!") + +# Testmatrix R aus Luhmann S. 66 +#R = sp.Matrix([ +# [0.996911, -0.013541, -0.077361], +# [0.030706, 0.973820, 0.225238], +# [0.072285, -0.226918, 0.971228] +#]) + +# 3) Quaternionen berechnen +# ToDo: Prüfen, ob Vorzeichen bei q0 richtig ist! +#ToDo: q0 stimmt nicht mit Luhmann überein! + +q = Quaternion.from_rotation_matrix(R) +q0_wert = q.a +q1_wert = q.b +q2_wert = q.c +q3_wert = q.d + + + +# 4) Funktionales Modell +liste_Punkte = ["P1", "P2", "P3", "P4", "P5"] +liste_unbekannte = ["dX", "dY", "dZ", "dm", "dq0", "dq1", "dq2", "dq3"] +liste_beobachtungen =[] +for punkt in liste_Punkte: + liste_beobachtungen.append(f"X_{punkt}") + liste_beobachtungen.append(f"Y_{punkt}") + liste_beobachtungen.append(f"Z_{punkt}") + + + + +dX, dY, dZ, m, q0, q1, q2, q3, xp1, yp1, zp1, xp2, yp2, zp2, xp3, yp3, zp3, xp4, yp4, zp4, xp5, yp5, zp5 = sp.symbols('dX dY dZ m q0 q1 q2 q3 xp1 yp1 zp1 xp2 yp2 zp2 xp3 yp3 zp3 xp4 yp4 zp4 xp5 yp5 zp5') + +#print(Translation[0]) + + +#print(zahlen[zp1]) + +f = sp.Matrix( + [[dX + m * (xp1 * (1 - 2 * (q2**2 + q3**2)) + yp1 * (2 * (q1 * q2 - q0 * q3)) + zp1 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp1 * (2 * (q1 * q2 + q0 * q3)) + yp1 * (1 - 2 * (q1**2 + q3**2)) + zp1 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp1 * (2 * (q1 * q3 - q0 * q2)) + yp1 * (2 * (q0 * q1 + q2 * q3)) + zp1 * (1 - 2 * (q1**2 + q2**2)))], + [dX + m * (xp2 * (1 - 2 * (q2**2 + q3**2)) + yp2 * (2 * (q1 * q2 - q0 * q3)) + zp2 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp2 * (2 * (q1 * q2 + q0 * q3)) + yp2 * (1 - 2 * (q1**2 + q3**2)) + zp2 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp2 * (2 * (q1 * q3 - q0 * q2)) + yp2 * (2 * (q0 * q1 + q2 * q3)) + zp2 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp3 * (1 - 2 * (q2**2 + q3**2)) + yp3 * (2 * (q1 * q2 - q0 * q3)) + zp3 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp3 * (2 * (q1 * q2 + q0 * q3)) + yp3 * (1 - 2 * (q1**2 + q3**2)) + zp3 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp3 * (2 * (q1 * q3 - q0 * q2)) + yp3 * (2 * (q0 * q1 + q2 * q3)) + zp3 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp4 * (1 - 2 * (q2**2 + q3**2)) + yp4 * (2 * (q1 * q2 - q0 * q3)) + zp4 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp4 * (2 * (q1 * q2 + q0 * q3)) + yp4 * (1 - 2 * (q1**2 + q3**2)) + zp4 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp4 * (2 * (q1 * q3 - q0 * q2)) + yp4 * (2 * (q0 * q1 + q2 * q3)) + zp4 * (1 - 2 * (q1**2 + q2**2)))], +[dX + m * (xp5 * (1 - 2 * (q2**2 + q3**2)) + yp5 * (2 * (q1 * q2 - q0 * q3)) + zp5 * (2 * (q0 * q2 + q1 * q3)))], + [dY + m * (xp5 * (2 * (q1 * q2 + q0 * q3)) + yp5 * (1 - 2 * (q1**2 + q3**2)) + zp5 * (2 * (q2 * q3 - q0 * q1)))], + [dZ + m * (xp5 * (2 * (q1 * q3 - q0 * q2)) + yp5 * (2 * (q0 * q1 + q2 * q3)) + zp5 * (1 - 2 * (q1**2 + q2**2)))], + + + ] +) + +A_ohne_zahlen = f.jacobian([dX, dY, dZ, m, q0, q1, q2, q3]) + + +#print(J) +#print(J_zahlen.evalf(n=3)) + +# Parameterschätzung +schwellenwert = 1e-3 +anzahl_iterationen = 0 +alle_kleiner_vorherige_iteration = False + + +P = sp.eye(15) + +#l = sp.Matrix([p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[1], p3[2], p4[0], p4[1], p4[2], p5[0], p5[1], p5[2]]) + + +liste_punkte_ausgangssystem = [p1, p2, p3, p4, p5] + + + +#l = sp.Matrix([P1[0] - p1[0], P1[1] - p1[1], P1[2] - p1[2], P2[0] - p2[0], P2[1] - p2[1], P2[2] - p2[2], P3[0] - p3[0], P3[1] - p3[1], P3[2] - p3[2], P4[0] - p4[0], P4[1] - p4[1], P4[2] - p4[2], P5[0] - p5[0], P5[1] - p5[1], P5[2] - p5[2]]) +l = sp.Matrix([P1[0], P1[1], P1[2], P2[0], P2[1], P2[2], P3[0], P3[1], P3[2], P4[0], P4[1], P4[2], P5[0], P5[1], P5[2]]) +# ToDo: Prüfen, ob n mit l oder mit dl! + +while True: + if anzahl_iterationen == 0: + zahlen_0 = {dX: Translation[0], dY: Translation[1], dZ: Translation[2], m: m0, q0: q0_wert, q1: q1_wert, + q2: q2_wert, + q3: q3_wert, xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], + yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} + x0 = sp.Matrix([zahlen_0[dX], zahlen_0[dY], zahlen_0[dZ], zahlen_0[m], zahlen_0[q0], zahlen_0[q1], zahlen_0[q2], zahlen_0[q3]]) + R_matrix_0 = sp.Matrix([[1 - 2 * (q2_wert ** 2 + q3_wert ** 2), 2 * (q1_wert * q2_wert - q0_wert * q3_wert), + 2 * (q0_wert * q2_wert + q1_wert * q3_wert)], + [2 * (q1_wert * q2_wert + q0_wert * q3_wert), 1 - 2 * (q1_wert ** 2 + q3_wert ** 2), + 2 * (q2_wert * q3_wert - q0_wert * q1_wert)], + [2 * (q1_wert * q3_wert - q0_wert * q2_wert), 2 * (q0_wert * q1_wert + q2_wert * q3_wert), + 1 - 2 * (q1_wert ** 2 + q2_wert ** 2)]]) + liste_l_berechnet_0 = [Translation + m0 * R_matrix_0 * p for p in liste_punkte_ausgangssystem] + l_berechnet_0 = sp.Matrix.vstack(*liste_l_berechnet_0) + dl_0 = l - l_berechnet_0 + + A_0 = A_ohne_zahlen.subs(zahlen_0) + N = A_0.T * P * A_0 + n_0 = A_0.T * P * dl_0 + Qxx_0 = N.evalf(n=30).inv() + dx = Qxx_0 * n_0 + x = x0 + dx + q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2) + x[4] /= q_norm + x[5] /= q_norm + x[6] /= q_norm + x[7] /= q_norm + anzahl_iterationen += 1 + print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen") + print(dx.evalf(n=3)) + + else: + #print("Im else-Block") + zahlen_i = {dX: float(x[0]), dY: float(x[1]), dZ: float(x[2]), m: float(x[3]), q0: float(x[4]), q1: float(x[5]), + q2: float(x[6]), + q3: float(x[7]), xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], + yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} + #print("zahlen_i") + R_matrix_i = sp.Matrix([[1 - 2 * (zahlen_i[q2] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q1] * zahlen_i[q2] - zahlen_i[q0] * zahlen_i[q3]), + 2 * (zahlen_i[q0] * zahlen_i[q2] + zahlen_i[q1] * zahlen_i[q3])], + [2 * (zahlen_i[q1] * zahlen_i[q2] + zahlen_i[q0] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q3] ** 2), + 2 * (zahlen_i[q2] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q1])], + [2 * (zahlen_i[q1] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q2]), + 2 * (zahlen_i[q0] * zahlen_i[q1] + zahlen_i[q2] * zahlen_i[q3]), + 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q2] ** 2)]]) + #print("R_matrix_i") + liste_l_berechnet_i = [sp.Matrix([zahlen_i[dX], zahlen_i[dY], zahlen_i[dZ]]) + zahlen_i[m] * R_matrix_i * p for p in liste_punkte_ausgangssystem] + #print("liste_l_berechnet_i") + l_berechnet_i = sp.Matrix.vstack(*liste_l_berechnet_i) + #print("l_berechnet_i") + dl_i = l - l_berechnet_i + #print("dl_i") + A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=3) + #print("A_i") + N_i = A_i.T * P * A_i + #print("N_i") + n_i = A_i.T * P * dl_i + # print("n_i") + Qxx_i = N_i.evalf(n=30).inv() + #print("Qxx_i") + n_i = A_i.T * P * dl_i + #print("n_i") + dx = Qxx_i * n_i + #print("dx") + x += dx + q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2) + x[4] /= q_norm + x[5] /= q_norm + x[6] /= q_norm + x[7] /= q_norm + # print("x") + anzahl_iterationen += 1 + print(f"Iteration Nr.{anzahl_iterationen} abgeschlossen") + print(dx.evalf(n=3)) + + alle_kleiner = True + for i in range(dx.rows): + wert = float(dx[i]) + if abs(wert) > schwellenwert: + alle_kleiner = False + + + if alle_kleiner and alle_kleiner_vorherige_iteration: + break + + alle_kleiner_vorherige_iteration = alle_kleiner + +print(l.evalf(n=3)) +print(l_berechnet_0.evalf(n=3)) +print(f"x = {x.evalf(n=3)}") + +#Neuberechnung Zielsystem +zahlen_i = {dX: float(x[0]), dY: float(x[1]), dZ: float(x[2]), m: float(x[3]), q0: float(x[4]), q1: float(x[5]), + q2: float(x[6]), + q3: float(x[7]), xp1: p1[0], yp1: p1[1], zp1: p1[2], xp2: p2[0], yp2: p2[1], zp2: p2[2], xp3: p3[0], + yp3: p3[1], zp3: p3[2], xp4: p4[0], yp4: p4[1], zp4: p4[2], xp5: p5[0], yp5: p5[1], zp5: p5[2]} +# print("zahlen_i") +R_matrix_i = sp.Matrix( + [[1 - 2 * (zahlen_i[q2] ** 2 + zahlen_i[q3] ** 2), 2 * (zahlen_i[q1] * zahlen_i[q2] - zahlen_i[q0] * zahlen_i[q3]), + 2 * (zahlen_i[q0] * zahlen_i[q2] + zahlen_i[q1] * zahlen_i[q3])], + [2 * (zahlen_i[q1] * zahlen_i[q2] + zahlen_i[q0] * zahlen_i[q3]), 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q3] ** 2), + 2 * (zahlen_i[q2] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q1])], + [2 * (zahlen_i[q1] * zahlen_i[q3] - zahlen_i[q0] * zahlen_i[q2]), + 2 * (zahlen_i[q0] * zahlen_i[q1] + zahlen_i[q2] * zahlen_i[q3]), + 1 - 2 * (zahlen_i[q1] ** 2 + zahlen_i[q2] ** 2)]]) +# print("R_matrix_i") +liste_l_berechnet_i = [sp.Matrix([zahlen_i[dX], zahlen_i[dY], zahlen_i[dZ]]) + zahlen_i[m] * R_matrix_i * p for p in + liste_punkte_ausgangssystem] +# print("liste_l_berechnet_i") +l_berechnet_i = sp.Matrix.vstack(*liste_l_berechnet_i) +print("") +print(f"l_berechnet_final: {l_berechnet_i.evalf(n=3)}")