75 lines
2.7 KiB
Python
75 lines
2.7 KiB
Python
import sympy as sp
|
||
from dataclasses import dataclass, field
|
||
from typing import Dict, Tuple, Iterable
|
||
|
||
@dataclass
|
||
class StochastischesModell:
|
||
n_beob: int
|
||
sigma_beob: Iterable[float] =None #σ a priori der einzelnen Beobachtung
|
||
gruppe_beob: Iterable[int] =None #Gruppenzugehörigkeit jeder Beobachtung (Distanz, Richtung, GNSS, Nivellement,...,)
|
||
sigma0_gruppe: Dict[int, float] = field(default_factory=dict) #σ0² für jede Gruppe
|
||
|
||
|
||
def __post_init__(self):
|
||
# Defaults setzen
|
||
if self.sigma_beob is None:
|
||
self.sigma_beob = [1.0] * int(self.n_beob)
|
||
|
||
if self.gruppe_beob is None:
|
||
self.gruppe_beob = [1] * int(self.n_beob)
|
||
|
||
# In SymPy-Spaltenvektoren umwandeln
|
||
self.sigma_beob = sp.Matrix(list(self.sigma_beob))
|
||
self.gruppe_beob = sp.Matrix(list(self.gruppe_beob))
|
||
|
||
# Dimension prüfen
|
||
if self.sigma_beob.rows != self.gruppe_beob.rows:
|
||
raise ValueError("sigma_beob und gruppe_beob müssen gleich viele Einträge haben.")
|
||
|
||
if self.sigma_beob.rows != int(self.n_beob):
|
||
raise ValueError("n_beob passt nicht zur Länge von sigma_beob / gruppe_beob.")
|
||
|
||
# Fehlende Gruppen mit sigma0_sq = 1.0 ergänzen
|
||
unique_groups = sorted({int(g) for g in self.gruppe_beob})
|
||
for g in unique_groups:
|
||
if g not in self.sigma0_gruppe:
|
||
self.sigma0_gruppe[g] = 1.0
|
||
|
||
|
||
def berechne_Qll(self) -> Tuple[sp.Matrix, sp.Matrix]:
|
||
n = self.n_beob
|
||
Q_ll = sp.zeros(n, n)
|
||
P = sp.zeros(n, n)
|
||
for i in range(self.n_beob):
|
||
sigma_i = self.sigma_beob[i, 0] #σ-Wert der i-ten Beobachtung holen
|
||
g = int(self.gruppe_beob[i, 0]) #Gruppenzugehörigkeit der Beobachtung bestimmen
|
||
sigma0_sq = self.sigma0_gruppe[g] #Den Varianzfaktor der Gruppe holen
|
||
q_ii = sigma_i**2 #σ² berechnen
|
||
Q_ll[i, i] = q_ii #Diagonale
|
||
return Q_ll
|
||
|
||
def Qll_aus_db(self):
|
||
pass
|
||
|
||
|
||
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
|
||
|
||
|
||
def berechne_R(self, Q_vv: sp.Matrix, P: sp.Matrix) -> sp.Matrix:
|
||
R = Q_vv * P
|
||
return R #Redundanzmatrix
|
||
|
||
|
||
def berechne_r(self, R: sp.Matrix) -> sp.Matrix:
|
||
n = R.rows
|
||
r = sp.zeros(n, 1)
|
||
for i in range(n):
|
||
r[i, 0] = R[i, i]
|
||
return r #Redundanzanteile |