Transformationen

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