86 lines
3.0 KiB
Python
86 lines
3.0 KiB
Python
import sympy as sp
|
||
from dataclasses import dataclass, field
|
||
from typing import Dict, Tuple
|
||
|
||
@dataclass
|
||
class StochastischesModell:
|
||
sigma_beob: Iterable[float] #σ der einzelnen Beobachtung
|
||
group_beob: Iterable[int] #Gruppenzugehörigkeit jeder Beobachtung (Distanz, Richtung, GNSS, Nivellement,...,)
|
||
sigma0_groups: Dict[int, float] = field(default_factory=dict) #σ0² für jede Gruppe
|
||
|
||
|
||
def __post_init__(self):
|
||
self.sigma_beob = sp.Matrix(list(self.sigma_beob)) #Spaltenvektor
|
||
self.group_beob = sp.Matrix(list(self.group_beob)) #Spaltenvektor
|
||
|
||
if self.sigma_beob.rows != self.group_beob.rows:
|
||
raise ValueError("sigma_obs und group_ids müssen gleich viele Einträge haben.")
|
||
|
||
unique_groups = sorted({int(g) for g in self.group_beob}) #jede Beobachtungsgruppe wird genau einmal berücksichtigt
|
||
for g in unique_groups:
|
||
if g not in self.sigma0_groups: #Fehlende Gruppen mit σ_0j^2 = 1.0
|
||
self.sigma0_groups[g] = 1.0
|
||
|
||
|
||
@property
|
||
def n_beob(self) -> int:
|
||
return int(self.sigma_beob.rows)
|
||
|
||
|
||
def aufstellen_Qll_P(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):
|
||
sigma_i = self.sigma_beob[i, 0] #σ-Wert der i-ten Beobachtung holen
|
||
g = int(self.group_beob[i, 0]) #Gruppenzugehörigkeit der Beobachtung bestimmen
|
||
sigma0_sq = self.sigma0_groups[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, P
|
||
|
||
|
||
@staticmethod
|
||
def redundanz_pro_beobachtung(A, P):
|
||
n = P.rows
|
||
sqrtP = sp.zeros(n, n)
|
||
for i in range(n):
|
||
sqrtP[i, i] = sp.sqrt(P[i, i])
|
||
|
||
A_tilde = sqrtP * A
|
||
M = (A_tilde.T * A_tilde).inv()
|
||
|
||
r = sp.zeros(n, 1)
|
||
for i in range(n):
|
||
a_i = sp.Matrix([A_tilde.row(i)])
|
||
r[i] = 1 - (a_i * M * a_i.T)[0]
|
||
return r
|
||
|
||
|
||
def varianzkomponenten(self, v, A) -> Dict[int, float]:
|
||
_, P = self.aufstellen_Qll_P()
|
||
r_obs = self.redundanz_pro_beobachtung(A, P)
|
||
gruppen = sorted(set(int(g) for g in self.group_beob))
|
||
|
||
sigma_hat = {}
|
||
|
||
for g in gruppen:
|
||
idx = [i for i in range(self.n) if int(self.group_beob[i]) == g]
|
||
|
||
v_i = sp.Matrix([v[i] for i in idx])
|
||
|
||
P_i = sp.zeros(len(idx))
|
||
for k, j in enumerate(idx):
|
||
P_i[k, k] = P[j, j]
|
||
|
||
r_g = sum(r_obs[j] for j in idx)
|
||
|
||
sigma_hat[g] = float((v_i.T * P_i * v_i)[0] / r_g)
|
||
return sigma_hat
|
||
|
||
|
||
def update_sigma(self, sigma_hat_dict):
|
||
for g, val in sigma_hat_dict.items():
|
||
self.sigma0_groups[g] = float(val) |