Masterprojekt_V3/Parameterschaetzung.py

from Stochastisches_Modell import StochastischesModell
from Netzqualität_Genauigkeit import Genauigkeitsmaße
from Datumsfestlegung import Datumsfestlegung
import numpy as np
import Export


def ausgleichung_global(A, dl, Q_ext, P):
    A=np.asarray(A, float)
    dl = np.asarray(dl, float).reshape(-1, 1)
    Q_ext = np.asarray(Q_ext, float)
    P = np.asarray(P, float)

    # 1) Gewichtsmatrix P
    #P = Q_ext

    # 2) Normalgleichungsmatrix N und Absolutgliedvektor n
    N = A.T @ P @ A
    n = A.T @ P @ dl

    # 3) Zuschlagsvektor dx
    dx = np.linalg.solve(N, n)

    # 4) Residuenvektor v
    v = A @ dx - dl

    # 5) Kofaktormatrix der Unbekannten Q_xx
    Q_xx = StochastischesModell.berechne_Q_xx(N)

    # 6) Kofaktormatrix der Beobachtungen Q_ll_dach
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 7) Kofaktormatrix der Verbesserungen Q_vv
    Q_vv = StochastischesModell.berechne_Qvv(Q_ext, Q_ll_dach)

    # 8) Ausgabe
    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ext": Q_ext,
    }
    return dict_ausgleichung, dx



def ausgleichung_lokal1(A, dl, Q_ll):
    A = np.asarray(A, dtype=float)
    dl = np.asarray(dl, dtype=float).reshape(-1, 1)
    Q_ll = np.asarray(Q_ll, dtype=float)

    # 1) Gewichtsmatrix
    P = np.linalg.inv(Q_ll)

    # 2) Normalgleichungen
    N = A.T @ P @ A
    n = A.T @ P @ dl

    # 3) Datumsfestlegung
    G = Datumsfestlegung.build_G_from_names(x0, Jacobimatrix_symbolisch_liste_unbekannte, liste_punktnummern, mit_massstab=True)
    u = A.shape[1]
    aktive = Datumsfestlegung.aktive_indices_from_selection(auswahl, Jacobimatrix_symbolisch_liste_unbekannte)
    E = Datumsfestlegung.auswahlmatrix_E(u, aktive)
    Gi = E @ G

    # 3) Zuschlagsvektor dx
    dx, k = Datumsfestlegung.berechne_dx_geraendert(N, n, Gi)

    # 5) Residuen
    v = dl - A @ dx

    # 5) Kofaktormatrix der Unbekannten Q_xx
    Q_xx = StochastischesModell.berechne_Q_xx(N)

    # 6) Kofaktormatrix der Beobachtungen Q_ll_dach
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 7) Kofaktormatrix der Verbesserungen Q_vv
    Q_vv = StochastischesModell.berechne_Qvv(Q_ll, Q_ll_dach)

    # 8) Ausgabe
    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ll": Q_ll,
    }
    return dict_ausgleichung, dx



def ausgleichung_lokal(
    A, dl, Q_ll,
    *,
    datumfestlegung="hart",
    x0=None,
    unbekannten_liste=None,
    liste_punktnummern=None,
    datenbank=None,
    mit_massstab=True
):
    A = np.asarray(A, float)
    dl = np.asarray(dl, float).reshape(-1, 1)
    Q_ll = np.asarray(Q_ll, float)

    # 1) Gewichtsmatrix
    P = np.linalg.inv(Q_ll)

    # 2) Normalgleichungen
    N = A.T @ P @ A
    n = A.T @ P @ dl
    u = N.shape[0]

    # 3) Datumsfestlegung
    if datumfestlegung == "weiche Lagerung":
        # hier wurde Q_ll bereits extern erweitert → ganz normal lösen
        dx = np.linalg.solve(N, n)

    elif datumfestlegung == "gesamtspur":
        if x0 is None or unbekannten_liste is None or liste_punktnummern is None:
            raise ValueError("gesamtspur benötigt x0, unbekannten_liste, liste_punktnummern")

        G = Datumsfestlegung.build_G_from_names(
            x0,
            unbekannten_liste,
            liste_punktnummern,
            mit_massstab=mit_massstab
        )
        dx, k = Datumsfestlegung.berechne_dx_geraendert(N, n, G)

    elif datumfestlegung == "teilspur":
        if x0 is None or unbekannten_liste is None or liste_punktnummern is None or datenbank is None:
            raise ValueError("teilspur benötigt x0, unbekannten_liste, liste_punktnummern, datenbank")

        # G über alle Punkte
        G = Datumsfestlegung.build_G_from_names(
            x0,
            unbekannten_liste,
            liste_punktnummern,
            mit_massstab=mit_massstab
        )

        # Auswahl aus DB
        liste_datumskoordinaten = datenbank.get_datumskoordinate()
        if not liste_datumskoordinaten:
            raise ValueError("Teilspur gewählt, aber keine Datumskoordinaten in der DB gesetzt.")

        aktive = Datumsfestlegung.aktive_indices_from_selection(
            [(s[1:], s[0]) for s in liste_datumskoordinaten],
            unbekannten_liste
        )

        E = Datumsfestlegung.auswahlmatrix_E(u, aktive)
        Gi = E @ G

        dx, k = Datumsfestlegung.berechne_dx_geraendert(N, n, Gi)

    else:
        raise ValueError(f"Unbekannte Datumsfestlegung: {datumfestlegung}")

    # 4) Residuen
    v = dl - A @ dx

    # 5) Kofaktormatrix der Unbekannten
    Q_xx = StochastischesModell.berechne_Q_xx(N)

    # 6) Kofaktormatrix der Beobachtungen
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 7) Kofaktormatrix der Verbesserungen
    Q_vv = StochastischesModell.berechne_Qvv(Q_ll, Q_ll_dach)

    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ll": Q_ll,
    }

    return dict_ausgleichung, dx


def ausgleichung_spurminimierung(A, dl, Q_ll, *, datumfestlegung, x0, unbekannten_liste, datenbank=None, mit_massstab=True):
    """
    Freie Ausgleichung mit Gesamtspur- oder Teilspurminimierung.
    Rückgabe-Layout wie ausgleichung_global.
    """
    A = np.asarray(A, float)
    dl = np.asarray(dl, float).reshape(-1, 1)
    Q_ll = np.asarray(Q_ll, float)

    # 1) Gewichtsmatrix P
    P = StochastischesModell.berechne_P(Q_ll)

    # 2) Normalgleichungen
    N = A.T @ P @ A
    n = A.T @ P @ dl

    # 3) G über ALLE Punkte aus unbekannten_liste (automatisch)
    G = Datumsfestlegung.build_G_from_names(
        x0=x0,
        unbekannten_liste=unbekannten_liste,
        liste_punktnummern=None,      # <- wichtig: auto
        mit_massstab=mit_massstab
    )

    # 4) Gesamtspur / Teilspur
    if datumfestlegung == "gesamtspur":
        Gi = G
        k = None  # wird unten überschrieben

    elif datumfestlegung == "teilspur":
        if datenbank is None:
            raise ValueError("teilspur benötigt datenbank mit get_datumskoordinate()")
        liste_datumskoordinaten = datenbank.get_datumskoordinate()
        if not liste_datumskoordinaten:
            raise ValueError("Teilspur gewählt, aber keine Datumskoordinaten in der DB gesetzt.")

        # ["X10034","Y10034"] -> [("10034","X"),("10034","Y")]
        auswahl = [(s[1:], s[0]) for s in liste_datumskoordinaten]
        aktive = Datumsfestlegung.aktive_indices_from_selection(auswahl, unbekannten_liste)

        E = Datumsfestlegung.auswahlmatrix_E(N.shape[0], aktive)
        Gi = E @ G

    else:
        raise ValueError("datumfestlegung muss 'gesamtspur' oder 'teilspur' sein")

    # 5) Lösung per Ränderung + korrektes Q_xx
    dx, k, Q_xx = Datumsfestlegung.loese_geraendert_mit_Qxx(N, n, Gi)

    # 6) Residuen (wie bei dir)
    v = A @ dx - dl

    # 7) Kofaktormatrix der Beobachtungen
    Q_ll_dach = StochastischesModell.berechne_Q_ll_dach(A, Q_xx)

    # 8) Kofaktormatrix der Verbesserungen
    Q_vv = StochastischesModell.berechne_Qvv(Q_ll, Q_ll_dach)

    dict_ausgleichung = {
        "dx": dx,
        "v": v,
        "P": P,
        "N": N,
        "n": n,
        "k": k,
        "Q_xx": Q_xx,
        "Q_ll_dach": Q_ll_dach,
        "Q_vv": Q_vv,
        "Q_ll": Q_ll,
        "datumfestlegung": datumfestlegung,
    }
    return dict_ausgleichung, dx