final
This commit is contained in:
@@ -117,26 +117,27 @@ def azimuth_at_ESpoint(P_prev: NDArray, P_curr: NDArray, E_hat_curr: NDArray, N_
|
||||
return wrap_to_pi(float(np.arctan2(sE, sN)))
|
||||
|
||||
|
||||
def optimize_next_point(beta_i: float, omega_i: float, alpha_target: float, ds: float, gamma0: float,
|
||||
def optimize_next_point(beta_i: float, omega_i: float, alpha_i: float, ds: float, gamma0: float,
|
||||
ell: EllipsoidTriaxial, maxSegLen: float = 1000.0, sigma0: float = None) -> Tuple[float, float, NDArray, float]:
|
||||
"""
|
||||
|
||||
:param beta_i:
|
||||
:param omega_i:
|
||||
:param alpha_target:
|
||||
:param ds:
|
||||
:param gamma0:
|
||||
:param ell:
|
||||
:param maxSegLen:
|
||||
Berechnung der 1. GHA mithilfe der CMA-ES.
|
||||
Die CMA-ES optimiert sukzessive einen Punkt, der maxSegLen vom vorherigen Punkt entfernt und zusätzlich auf der
|
||||
geodätischen Linien liegt. Somit entsteht ein Geodäten ähnlicher Polygonzug auf der Oberfläche des dreiachsigen Ellipsoids.
|
||||
:param beta_i: Beta Koordinate am Punkt i
|
||||
:param omega_i: Omega Koordinate am Punkt i
|
||||
:param alpha_i: Azimut am Punkt i
|
||||
:param ds: Gesamtlänge
|
||||
:param gamma0: Jacobi-Konstante am Startpunkt
|
||||
:param ell: Ellipsoid
|
||||
:param maxSegLen: maximale Segmentlänge
|
||||
:param sigma0:
|
||||
:return:
|
||||
"""
|
||||
# Startbasis (für Predictor + optionales alpha_start)
|
||||
# Startbasis
|
||||
E_i, N_i, U_i, En_i, Nn_i, P_i = ENU_beta_omega(beta_i, omega_i, ell)
|
||||
# Predictor: dβ ≈ ds cosα / |N|, dω ≈ ds sinα / |E|
|
||||
d_beta = ds * float(np.cos(alpha_target)) / Nn_i
|
||||
d_omega = ds * float(np.sin(alpha_target)) / En_i
|
||||
|
||||
# Prediktor: dβ ≈ ds cosα / |N|, dω ≈ ds sinα / |E|
|
||||
d_beta = ds * float(np.cos(alpha_i)) / Nn_i
|
||||
d_omega = ds * float(np.sin(alpha_i)) / En_i
|
||||
beta_pred = beta_i + d_beta
|
||||
omega_pred = wrap_to_pi(omega_i + d_omega)
|
||||
|
||||
@@ -144,36 +145,44 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_target: float, ds:
|
||||
|
||||
if sigma0 is None:
|
||||
R0 = (ell.ax + ell.ay + ell.b) / 3
|
||||
sigma0 = 1e-3 * (ds / R0)
|
||||
sigma0 = 1e-5 * (ds / R0)
|
||||
|
||||
def fitness(x: NDArray) -> float:
|
||||
"""
|
||||
Fitnessfunktion: Fitnesscheck erfolgt anhand der Segmentlänge und der Jacobi-Konstante.
|
||||
Die Segmentlänge muss möglichst gut zum Sollwert passen. Die Jacobi-Konstante am Punkt x muss zur
|
||||
Jacobi-Konstanten am Startpunkt passen, damit der Polygonzug auf derselben geodätischen Linie bleibt.
|
||||
:param x: Koordinate in beta, lambda aus der CMA-ES
|
||||
:return: Fitnesswert (f)
|
||||
"""
|
||||
beta = x[0]
|
||||
omega = wrap_to_pi(x[1])
|
||||
|
||||
P = ell.ell2cart(beta, omega)
|
||||
d = float(np.linalg.norm(P - P_i))
|
||||
P = ell.ell2cart(beta, omega) # in kartesischer Koordinaten
|
||||
d = float(np.linalg.norm(P - P_i)) # Distanz zwischen
|
||||
|
||||
# length penalty
|
||||
# maxSegLen einhalten
|
||||
J_len = ((d - ds) / ds) ** 2
|
||||
if d > maxSegLen * 1.02:
|
||||
J_len += 1e3 * ((d / maxSegLen) - 1.02) ** 2
|
||||
w_len = 1.0
|
||||
|
||||
# alpha at end, computed using previous point (for Jacobi gamma)
|
||||
# Azimut für Jacobi-Konstante
|
||||
E_j, N_j, U_j, _, _, _ = ENU_beta_omega(beta, omega, ell)
|
||||
alpha_end = azimuth_at_ESpoint(P_i, P, E_j, N_j, U_j)
|
||||
|
||||
# Jacobi gamma at candidate/end
|
||||
# Jacobi-Konstante
|
||||
g_end = jacobi_konstante(beta, omega, alpha_end, ell)
|
||||
J_gamma = (g_end - gamma0) ** 2
|
||||
w_gamma = 10
|
||||
|
||||
return float(w_len * J_len + w_gamma * J_gamma)
|
||||
f = float(w_len * J_len + w_gamma * J_gamma)
|
||||
|
||||
return f
|
||||
|
||||
|
||||
xb = escma(fitness, N=2, xmean=xmean, sigma=sigma0) # Aufruf CMA-ES
|
||||
|
||||
beta_best = float(np.clip(float(xb[0]), -0.499999 * np.pi, 0.499999 * np.pi))
|
||||
omega_best = wrap_to_pi(float(xb[1]))
|
||||
beta_best = xb[0]
|
||||
omega_best = wrap_to_pi(xb[1])
|
||||
P_best = ell.ell2cart(beta_best, omega_best)
|
||||
E_j, N_j, U_j, _, _, _ = ENU_beta_omega(beta_best, omega_best, ell)
|
||||
alpha_end = azimuth_at_ESpoint(P_i, P_best, E_j, N_j, U_j)
|
||||
@@ -181,17 +190,16 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_target: float, ds:
|
||||
return beta_best, omega_best, P_best, alpha_end
|
||||
|
||||
|
||||
|
||||
def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float, s_total: float, maxSegLen: float = 1000):
|
||||
"""
|
||||
|
||||
:param ell:
|
||||
:param beta0:
|
||||
:param omega0:
|
||||
:param alpha0:
|
||||
:param s_total:
|
||||
:param maxSegLen:
|
||||
:return:
|
||||
Aufruf der 1. GHA mittels CMA-ES
|
||||
:param ell: Ellipsoid
|
||||
:param beta0: Beta Startkoordinate
|
||||
:param omega0: Omega Startkoordinate
|
||||
:param alpha0: Azimut Startkoordinate
|
||||
:param s_total: Gesamtstrecke
|
||||
:param maxSegLen: maximale Segmentlänge
|
||||
:return: Zielpunkt Pk und Azimut am Zielpunkt
|
||||
"""
|
||||
beta = float(beta0)
|
||||
omega = wrap_to_pi(float(omega0))
|
||||
@@ -205,14 +213,12 @@ def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float,
|
||||
s_acc = 0.0
|
||||
step = 0
|
||||
nsteps_est = int(np.ceil(s_total / maxSegLen))
|
||||
|
||||
while s_acc < s_total - 1e-9:
|
||||
step += 1
|
||||
ds = min(maxSegLen, s_total - s_acc)
|
||||
|
||||
print(f"[GHA1-ES] Step {step}/{nsteps_est} ds={ds:.3f} m s_acc={s_acc:.3f} m beta={beta:.6f} omega={omega:.6f} alpha={alpha:.6f}")
|
||||
|
||||
beta, omega, P, alpha = optimize_next_point(beta_i=beta, omega_i=omega, alpha_target=alpha, ds=ds, gamma0=gamma0,
|
||||
beta, omega, P, alpha = optimize_next_point(beta_i=beta, omega_i=omega, alpha_i=alpha, ds=ds, gamma0=gamma0,
|
||||
ell=ell, maxSegLen=maxSegLen)
|
||||
s_acc += ds
|
||||
points.append(P)
|
||||
@@ -225,7 +231,6 @@ def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float,
|
||||
return Pk, alpha1
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ell = EllipsoidTriaxial.init_name("BursaSima1980round")
|
||||
s = 188891.650873
|
||||
|
||||
Reference in New Issue
Block a user