85 lines
2.5 KiB
Python
85 lines
2.5 KiB
Python
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!
|
|
q0 = 1 / 2 * sp.sqrt(R[0, 0] + R[1, 1] + R[2, 2])
|
|
q1 = (R[2, 1] - R[1, 2]) / (4 * q0)
|
|
q2 = (R[0, 2] - R[2, 0]) / (4 * q0)
|
|
q3 = (R[1, 0] - R[0, 1]) / (4 * q0)
|
|
|
|
q = Quaternion.from_rotation_matrix(R)
|
|
q0 = q.a
|
|
q1 = q.b
|
|
q2 = q.c
|
|
q3 = 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}")
|
|
|
|
print(liste_beobachtungen)
|
|
|
|
# ToDo: Sympy Funktion jacobian nutzen!
|