diff --git a/GHA_triaxial/gha1_ES.py b/GHA_triaxial/gha1_ES.py index 06b11af..fbdb159 100644 --- a/GHA_triaxial/gha1_ES.py +++ b/GHA_triaxial/gha1_ES.py @@ -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, - ell: EllipsoidTriaxial, maxSegLen: float = 1000.0, sigma0: float = None) -> Tuple[float, float, NDArray, 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,15 +213,13 @@ 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, - ell=ell, maxSegLen=maxSegLen) + 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) alpha_end.append(alpha) @@ -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