diff --git a/Datumsfestlegung.py b/Datumsfestlegung.py index 74b129c..8e703be 100644 --- a/Datumsfestlegung.py +++ b/Datumsfestlegung.py @@ -1,4 +1,59 @@ import sympy as sp +from typing import List, Iterable, Tuple + +def raenderungsmatrix_G( + x0: sp.Matrix, + idx_X: List[int], + idx_Y: List[int], + idx_Z: List[int], + mit_massstab: bool = True, + ) -> sp.Matrix: + + u = x0.rows + d = 7 if mit_massstab else 6 + G = sp.zeros(u, d) + + # --- Translationen --- + for i in idx_X: + G[i, 0] = 1 + for i in idx_Y: + G[i, 1] = 1 + for i in idx_Z: + G[i, 2] = 1 + + # --- Rotationen --- + # Rotation um X-Achse + for iy, iz in zip(idx_Y, idx_Z): + zi = x0[iz, 0] + yi = x0[iy, 0] + G[iy, 3] = -zi + G[iz, 3] = yi + + # Rotation um Y-Achse + for ix, iz in zip(idx_X, idx_Z): + zi = x0[iz, 0] + xi = x0[ix, 0] + G[ix, 4] = zi + G[iz, 4] = -xi + + # Rotation um Z-Achse + for ix, iy in zip(idx_X, idx_Y): + yi = x0[iy, 0] + xi = x0[ix, 0] + G[ix, 5] = -yi + G[iy, 5] = xi + + # --- Maßstab --- + if mit_massstab: + for ix, iy, iz in zip(idx_X, idx_Y, idx_Z): + xi = x0[ix, 0] + yi = x0[iy, 0] + zi = x0[iz, 0] + G[ix, 6] = xi + G[iy, 6] = yi + G[iz, 6] = zi + return G + def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matrix: E = sp.zeros(u, u) @@ -6,9 +61,21 @@ def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matr E[int(idx), int(idx)] = 1 return E -def raenderungsmatric_G(): - def teilspurminimierung_Gi(G: sp.Matrix, E: sp.Matrix) -> sp.Matrix: Gi = E * G return Gi + + +def berechne_dx_geraendert(N: sp.Matrix, n: sp.Matrix, Gi: sp.Matrix) -> sp.Matrix: + u = N.rows + d = Gi.shape[1] + + K = N.row_join(Gi) + K = K.col_join(Gi.T.row_join(sp.zeros(d, d))) + + rhs = n.col_join(sp.zeros(d, 1)) + + sol = K.LUsolve(rhs) + dx = sol[:u, :] + return dx \ No newline at end of file diff --git a/Parameterschaetzung.py b/Parameterschaetzung.py index c134e4a..e1d5165 100644 --- a/Parameterschaetzung.py +++ b/Parameterschaetzung.py @@ -1,9 +1,11 @@ +from Datumsfestlegung import * from Stochastisches_Modell import StochastischesModell import sympy as sp import Export import Netzqualität_Genauigkeit -def ausgleichung(A, dl, stoch_modell: StochastischesModell): + +def ausgleichung_global(A, dl, stoch_modell: StochastischesModell): #Q_ll, P = stoch_modell.berechne_Qll_P() #Kofaktormatrix und P-Matrix P = sp.eye(A.shape[0]) @@ -37,4 +39,79 @@ def ausgleichung(A, dl, stoch_modell: StochastischesModell): Export.Export.ausgleichung_to_datei(r"Zwischenergebnisse\Ausgleichung_Iteration0.csv", dict_ausgleichung) - return dict_ausgleichung, dx \ No newline at end of file + return dict_ausgleichung, dx + + + +def ausgleichung_lokal( + A: sp.Matrix, + dl: sp.Matrix, + stoch_modell: StochastischesModell, + x0: sp.Matrix, + idx_X, idx_Y, idx_Z, + aktive_unbekannte_indices, + mit_massstab: bool = True, +): + # 1) Gewichte + Q_ll, P = stoch_modell.berechne_Qll_P() + # Debug-Option: + # P = sp.eye(A.rows) + + # 2) Normalgleichungen + N = A.T * P * A + n = A.T * P * dl + + # 3) Datum (G, E, Gi) + G = raenderungsmatrix_G(x0, idx_X, idx_Y, idx_Z, mit_massstab=mit_massstab) + E = auswahlmatrix_E(u=A.cols, aktive_unbekannte_indices=aktive_unbekannte_indices) + Gi = E * G + + # 4) Geränderte Lösung (dx) + dx = berechne_dx_geraendert(N, n, Gi) + + # 5) Residuen + v = dl - A * dx + + # 6) KORREKTE Q_xx für gerändertes Problem: + # Q_xx = N^{-1} - N^{-1}Gi (Gi^T N^{-1} Gi)^{-1} Gi^T N^{-1} + # numerisch besser via LUsolve statt inv: + N_inv = N.inv() # wenn N groß ist, kann man das unten auch ohne inv machen (siehe Hinweis) + N_inv_G = N_inv * Gi + S = Gi.T * N_inv_G + S_inv = S.inv() + Q_xx = N_inv - N_inv_G * S_inv * N_inv_G.T + + # 7) Q_lhat_lhat und Q_vv + Q_lhat_lhat = A * Q_xx * A.T + Q_vv = P.inv() - Q_lhat_lhat + + # 8) Redundanzmatrix und -anteile + R = Q_vv * P + r_vec = sp.Matrix(R.diagonal()) + + # 9) Freiheitsgrade (Redundanz gesamt) + n_beob = A.rows + u = A.cols + d = Gi.shape[1] + r_gesamt = n_beob - u + d + + # 10) sigma0 a posteriori + omega = float((v.T * P * v)[0, 0]) + sigma0_hat = (omega / float(r_gesamt)) ** 0.5 + + return { + "dx": dx, + "v": v, + "Q_ll": Q_ll, + "P": P, + "N": N, + "Q_xx": Q_xx, + "Q_lhat_lhat": Q_lhat_lhat, + "Q_vv": Q_vv, + "R": R, + "r": r_vec, + "r_gesamt": r_gesamt, + "sigma0_hat": sigma0_hat, + "G": G, + "Gi": Gi, + } \ No newline at end of file diff --git a/Stochastisches_Modell.py b/Stochastisches_Modell.py index c6aca9e..e30cefb 100644 --- a/Stochastisches_Modell.py +++ b/Stochastisches_Modell.py @@ -46,13 +46,14 @@ class StochastischesModell: sigma0_sq = self.sigma0_gruppe[g] #Den Varianzfaktor der Gruppe holen q_ii = sigma_i**2 #σ² berechnen Q_ll[i, i] = q_ii #Diagonale - P[i, i] = 1 / (sigma0_sq * q_ii) #durch VKS nicht mehr P=Qll^-1 return Q_ll + def berechne_P(Q_ll): P = Q_ll.inv() return P + def berechne_Qvv(self, A: sp.Matrix, P: sp.Matrix, Q_xx: sp.Matrix) -> sp.Matrix: Q_vv = P.inv() - A * Q_xx * A.T return Q_vv #Kofaktormatrix der Beobachtungsresiduen