diff --git a/Vorbereitungen_Fabian/Transformation_Helmert_V3.py b/Vorbereitungen_Fabian/Transformation_Helmert_V3.py index f903d7d..0e63921 100644 --- a/Vorbereitungen_Fabian/Transformation_Helmert_V3.py +++ b/Vorbereitungen_Fabian/Transformation_Helmert_V3.py @@ -113,7 +113,7 @@ A_ohne_zahlen = f.jacobian([dX, dY, dZ, m, q0, q1, q2, q3]) #print(J_zahlen.evalf(n=3)) # Parameterschätzung -schwellenwert = 1e-3 +schwellenwert = 1e-4 anzahl_iterationen = 0 alle_kleiner_vorherige_iteration = False @@ -154,7 +154,9 @@ while True: Qxx_0 = N.evalf(n=30).inv() dx = Qxx_0 * n_0 x = x0 + dx + x = sp.N(x, 10) # 10 Nachkommastellen q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2) + x = sp.Matrix(x) x[4] /= q_norm x[5] /= q_norm x[6] /= q_norm @@ -164,12 +166,12 @@ while True: print(dx.evalf(n=3)) else: - #print("Im else-Block") + 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") + 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), @@ -182,9 +184,11 @@ while True: #print("liste_l_berechnet_i") l_berechnet_i = sp.Matrix.vstack(*liste_l_berechnet_i) #print("l_berechnet_i") + print("Vor dl_i") dl_i = l - l_berechnet_i + print("NAch dl_i") #print("dl_i") - A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=3) + A_i = A_ohne_zahlen.subs(zahlen_i).evalf(n=30) #print("A_i") N_i = A_i.T * P * A_i #print("N_i") @@ -197,6 +201,7 @@ while True: dx = Qxx_i * n_i #print("dx") x += dx + x = sp.Matrix(x) q_norm = sp.sqrt(x[4] ** 2 + x[5] ** 2 + x[6] ** 2 + x[7] ** 2) x[4] /= q_norm x[5] /= q_norm @@ -214,7 +219,7 @@ while True: alle_kleiner = False - if alle_kleiner and alle_kleiner_vorherige_iteration: + if alle_kleiner and alle_kleiner_vorherige_iteration or anzahl_iterationen == 200: break alle_kleiner_vorherige_iteration = alle_kleiner @@ -243,4 +248,7 @@ liste_l_berechnet_i = [sp.Matrix([zahlen_i[dX], zahlen_i[dY], zahlen_i[dZ]]) + z # 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)}") +print("l_berechnet_final:") +for v in l_berechnet_i: + print(f"{float(v):.3f}") +