fa2338/Masterprojekt-Campusnetz
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Transformationen

Browse Source
This commit is contained in:
fa2338
2025-12-03 18:49:34 +01:00
parent fd4dca5818
commit f677987632
4 changed files with 475 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
Vorbereitungen_Fabian/Tests.ipynb Normal file
View File

@@ -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
}

48
Vorbereitungen_Fabian/Transformation_Helmert.py → Vorbereitungen_Fabian/Transformation_Helmert_V1.py
View File

@@ -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))
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!")

155
Vorbereitungen_Fabian/Transformation_Helmert_V2.py Normal file
View File

@@ -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!")

246
Vorbereitungen_Fabian/Transformation_Helmert_V3.py Normal file
View File

@@ -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)}")