diff --git a/GHA_triaxial/ES_gha2.py b/GHA_triaxial/ES_gha2.py new file mode 100644 index 0000000..71d8844 --- /dev/null +++ b/GHA_triaxial/ES_gha2.py @@ -0,0 +1,217 @@ +import numpy as np +from numpy import arccos +from Hansen_ES_CMA import escma +from ellipsoide import EllipsoidTriaxial +from numpy.typing import NDArray +import plotly.graph_objects as go +from GHA_triaxial.panou_2013_2GHA_num import gha2_num +from utils import sigma2alpha + +ell_ES: EllipsoidTriaxial = None +P_start: NDArray = None +P_prev: NDArray = None +P_next: NDArray = None +P_end: NDArray = None +stepLen: float = None + +def Bogenlaenge(P1, P2): + """ + Berechnung der mittleren Bogenlänge zwischen zwei kartesischen Punkten + :param P1: kartesische Koordinate Punkt 1 + :param P2: kartesische Koordinate Punkt 2 + :return: Bogenlänge s + """ + R1 = np.linalg.norm(P1) + R2 = np.linalg.norm(P2) + R = 0.5*(R1 + R2) + theta = arccos(P1 @ P2 / (R1 * R2)) + s = R * theta + return s + + +def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, stepLenTarget: float = None, sigmaStep: float = 1e-7, stopeval: int = 1000, maxSteps: int = 10000, all_points: bool = False): + """ + Berechnen der 2. GHA mithilfe der CMA-ES. + Die CMA-ES optimiert sukzessive einzelne Punkte, die einen definierten Abstand (stepLenTarget) zum vorherigen und den kürzesten + Abstand zum Zielpunkt aufweisen. Der Abbruch der Optimierung erfolgt, wenn die Restdistanz zwischen vorherigen und Zielpunkt die + 'stepLenTarget' unterschreitet. Die Distanzen zwischen den einzelnen Punkten werden als mittlere Bogenlänge berechnet und aufaddiert. + :param ell: Parameter des triaxialen Ellipsoids + :param P0: Startpunkt + :param Pk: Zielpunkt + :param stepLenTarget: Abstand zwischen vorherigen und optimierten Punkt + :param sigmaStep: Sigma Startwert für die CMA-ES (Suchraum um den zu optimierenden Punkt) + :param stopeval: maximale Iterationen + :param maxSteps: maximale Anzahl der zu optimierenden Punkte + :param all_points: Ergebnisliste mit allen Punkte, die wahlweise mit ausgegeben werden kann + :return: Richtungswinkel des Start- und Zielpunktes, totalLen + """ + global ell_ES, P_start, P_prev, P_next, P_end, stepLen + ell_ES = ell + P_start = P0 + P_end = Pk + if stepLenTarget == None: + R0 = (ell.ax + ell.ay + ell.b) / 3 + stepLenTarget = R0 * 1 / 600 + stepLen = stepLenTarget + + P_all = [P_start] + totalLen = 0 + + P_prev = P_start + + for i in range(1, maxSteps): + + d_remain = Bogenlaenge(P_prev, P_end) + + # Abbruch: letzter "Rest-Schritt" ist < 10 km -> Ziel anhängen + if d_remain <= stepLen: + P_all.append(P_end) + totalLen += d_remain + print(f'[Punkt {i}] Stop: Restdistanz {round(d_remain, 3)} m <= {round(stepLen, 3)} m, Ziel angehängt.') + break + + # Globals für Fitness: aktueller Start(P0) und Ziel(Pk) + # P0 = P_prev; + # Pk = P_end; + + # Näherung für die ES ermitteln + # % d = P_end - P_prev; + # % L = Bogenlaenge(P_prev, P_end); + # % t = min(stepLen / L, 1.0); + # % Q = P_prev + t * d; + # % q = [Q(1) / a; + # Q(2) / b; + # Q(3) / c]; + # % nq = norm(q); + # % q = q / nq; + + # %q + # entspricht[cos(u) + # cos(v); + # cos(u) + # sin(v); + # sin(u)] auf + # Einheitskugel + # % arg_u = max(-1, min(1, q(3))); + # + # %Quadrantenabfrage + # % u0 = mod(asin(arg_u) + pi / 2, pi) - pi / 2; + # % v0 = atan2(q(2), q(1)); + # % xmean_init = [u0; + # v0]; + xmean_init = ell.cartonell(P_prev + stepLen * (P_end - P_prev) / np.linalg.norm(P_end - P_prev)) + + # [~, ~, aux] = geoLength(xmean_init); + # print('Startguess: d_step=%.3f (soll %.3f), d_to_target=%.3f\n', aux(1), stepLen, aux(2)); + + print(f'[Punkt {i}] Optimiere nächsten Punkt: Restdistanz = {round(d_remain, 3)} m') + xmean_init = np.array(ell_ES.cart2para(xmean_init)) + + u, v = escma(geoLength,2, xmean_init, sigmaStep, -np.inf, stopeval) + + P_next = ell.para2cart(u, v) + + d_step = Bogenlaenge(P_prev, P_next) + d_new = Bogenlaenge(P_next, P_end) + d_old = Bogenlaenge(P_prev, P_end) + + print(f'[Punkt {i}] Ergebnis: Schritt = {round(d_step, 3)} m (soll {round(stepLen, 3)}), Rest neu = {round(d_new, 3)} m') + + # Sicherheitscheck: wenn wir nicht näher kommen, abbrechen + if d_new >= d_old: + print(f'Punkt {i}: Neuer Punkt ist nicht naeher am Ziel ({d_old} -> {d_new}). Abbruch.') + P_all.append(P_end) + totalLen += Bogenlaenge(P_prev, P_end) + break + + P_all.append(P_next) + totalLen += d_step + P_prev = P_next + + + print('Maximale Schrittanzahl erreicht.') + P_all.append(P_end) + totalLen += Bogenlaenge(P_prev, P_end) + + p0i = ell.cartonell(P0 + 10 * (P_all[1] - P0) / np.linalg.norm(P_all[1] - P0)) + sigma0 = (p0i - P0) / np.linalg.norm(p0i - P0) + alpha0 = sigma2alpha(ell_ES, sigma0, P0) + + p1i = ell.cartonell(Pk - 10 * (Pk - P_all[-2]) / np.linalg.norm(Pk - P_all[-2])) + sigma1 = (Pk - p1i) / np.linalg.norm(Pk - p1i) + alpha1 = sigma2alpha(ell_ES, sigma1, Pk) + + if all_points: + return alpha0, alpha1, totalLen, np.array(P_all) + else: + return alpha0, alpha1, totalLen + + +def geoLength(P_candidate): + # P_candidate = [u;v] des naechsten Punktes. + # Ziel: Distanz zum Ziel minimieren, aber Schrittlaenge ~ stepLenTarget erzwingen. + u, v = P_candidate + + global ell_ES, P_start, P_prev, P_next, P_end, stepLen + # Punkt auf Ellipsoid + P_next = ell_ES.para2cart(u, v) + + # Distanzen + d_step = Bogenlaenge(P_prev, P_next) + d_to_target = Bogenlaenge(P_end, P_next) + d_prev_to_target = Bogenlaenge(P_end, P_prev) + + # Penalties(dimensionslos) + pen_step = ((d_step - stepLen) / stepLen)**2 + + # falls Punkt "weg" vom Ziel geht, extra bestrafen + pen_away = max(0, (d_to_target - d_prev_to_target) / stepLen)**2 + + # Gewichtungen + alpha = 1e2 + # Schrittlaenge sehr hart + gamma = 1e2 # "nicht weg vom Ziel" hart + + f = d_to_target * (1 + alpha * pen_step + gamma * pen_away) + + # Für Debug / Extraktion + aux = [d_step, d_to_target] + return f # , P_candidate, aux + +def show_points(points: NDArray, pointsNum:NDArray, p0: NDArray, p1: NDArray): + fig = go.Figure() + + fig.add_scatter3d(x=pointsNum[:, 0], y=pointsNum[:, 1], z=pointsNum[:, 2], + mode='lines', line=dict(color="green", width=3), name="Numerisch") + fig.add_scatter3d(x=points[:, 0], y=points[:, 1], z=points[:, 2], + mode='lines', line=dict(color="red", width=3), name="ES") + fig.add_scatter3d(x=[p0[0]], y=[p0[1]], z=[p0[2]], + mode='markers', marker=dict(color="black"), name="P0") + fig.add_scatter3d(x=[p1[0]], y=[p1[1]], z=[p1[2]], + mode='markers', marker=dict(color="black"), name="P1") + + fig.update_layout( + scene=dict(xaxis_title='X [km]', + yaxis_title='Y [km]', + zaxis_title='Z [km]', + aspectmode='data')) + + fig.show() + + +if __name__ == '__main__': + ell = EllipsoidTriaxial.init_name("Fiction") + + beta0, lamb0 = (0.2, 0.1) + P0 = ell.ell2cart(beta0, lamb0) + beta1, lamb1 = (0.7, 0.3) + P1 = ell.ell2cart(beta1, lamb1) + + alpha0, alpha1, s, betas, lambs = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=5000, all_points=True) + points_num = [] + for beta, lamb in zip(betas, lambs): + points_num.append(ell.ell2cart(beta, lamb)) + points_num = np.array(points_num) + + alpha0, alpha1, s, points = gha2_ES(ell, P0, P1) + show_points(points, points_num, P0, P1) diff --git a/GHA_triaxial/approx_gha2.py b/GHA_triaxial/approx_gha2.py index 1fe8b40..b47ac5d 100644 --- a/GHA_triaxial/approx_gha2.py +++ b/GHA_triaxial/approx_gha2.py @@ -1,27 +1,13 @@ import numpy as np -from numpy import arctan2 from ellipsoide import EllipsoidTriaxial -from panou import pq_ell -from panou_2013_2GHA_num import gha2_num +from GHA_triaxial.panou_2013_2GHA_num import gha2_num import plotly.graph_objects as go import winkelumrechnungen as wu from numpy.typing import NDArray from typing import Tuple -def sigma2alpha(sigma: NDArray, point: NDArray) -> float: - """ - Berechnung des Richtungswinkels an einem Punkt anhand der Ableitung zu den kartesischen Koordinaten - :param sigma: Ableitungsvektor ver kartesischen Koordinaten - :param point: Punkt - :return: Richtungswinkel - """ - "" - p, q = pq_ell(ell, point) - P = float(p @ sigma) - Q = float(q @ sigma) +from utils import sigma2alpha - alpha = arctan2(P, Q) - return alpha def gha2(ell: EllipsoidTriaxial, p0: NDArray, p1: NDArray, ds: float, all_points: bool = False) -> Tuple[float, float, float] | Tuple[float, float, float, NDArray]: """ @@ -55,11 +41,11 @@ def gha2(ell: EllipsoidTriaxial, p0: NDArray, p1: NDArray, ds: float, all_points p0i = ell.cartonell(p0 + ds/100 * (points[1] - p0) / np.linalg.norm(points[1] - p0)) sigma0 = (p0i - p0) / np.linalg.norm(p0i - p0) - alpha0 = sigma2alpha(sigma0, p0) + alpha0 = sigma2alpha(ell, sigma0, p0) p1i = ell.cartonell(p1 - ds/100 * (p1 - points[-2]) / np.linalg.norm(p1 - points[-2])) sigma1 = (p1 - p1i) / np.linalg.norm(p1 - p1i) - alpha1 = sigma2alpha(sigma1, p1) + alpha1 = sigma2alpha(ell, sigma1, p1) s = np.sum(np.array([np.linalg.norm(points[i] - points[i+1]) for i in range(len(points)-1)])) diff --git a/Hansen_ES_CMA.py b/Hansen_ES_CMA.py index 881ff93..e21585b 100644 --- a/Hansen_ES_CMA.py +++ b/Hansen_ES_CMA.py @@ -1,5 +1,5 @@ import numpy as np - +from typing import Callable def felli(x): N = x.shape[0] @@ -9,15 +9,16 @@ def felli(x): return float(np.sum((1e6 ** exponents) * (x ** 2))) -def escma(): +def escma(fitnessfct: Callable, N, xmean, sigma, stopfitness, stopeval, + bestEver = np.inf, noImproveGen = 0, absTolImprove = 1e-10, maxNoImproveGen = 100, sigmaImprove = 1e-12): #Initialization # User defined input parameters - N = 10 - xmean = np.random.rand(N) - sigma = 0.5 - stopfitness = 1e-10 - stopeval = int(1e3 * N**2) + # N = 10 + # xmean = np.random.rand(N) + # sigma = 0.5 + # stopfitness = 1e-10 + # stopeval = int(1e3 * N**2) # Strategy parameter setting: Selection lambda_ = 4 + int(np.floor(3 * np.log(N))) @@ -52,13 +53,18 @@ def escma(): arz = np.zeros((N, lambda_)) arfitness = np.zeros(lambda_) + gen = 0 + + print(f' [CMA-ES] Start: lambda = {lambda_}, sigma ={round(sigma, 6)}, stopeval = {stopeval}') + while counteval < stopeval: + gen += 1 # Generate and evaluate lambda offspring for k in range(lambda_): arz[:, k] = np.random.randn(N) arx[:, k] = xmean + sigma * (B @ D @ arz[:, k]) - arfitness[k] = felli(arx[:, k]) + arfitness[k] = fitnessfct(arx[:, k]) counteval += 1 # Sort by fitness and compute weighted mean into xmean @@ -70,6 +76,28 @@ def escma(): xmean = arx[:, arindex[:mu]] @ weights zmean = arz[:, arindex[:mu]] @ weights + # Stagnation check + fbest = arfitness[0] + if bestEver - fbest > absTolImprove: + bestEver = fbest + noImproveGen = 0 + else: + noImproveGen = noImproveGen + 1 + + + if gen == 1 or gen%50==0: + print(f' [CMA-ES] Gen {gen}, best = {round(fbest, 6)}, sigma = {sigma:.3g}') + + if noImproveGen >= maxNoImproveGen: + print(f' [CMA-ES] Abbruch: keine Verbesserung > {round(absTolImprove, 3)} in {maxNoImproveGen} Generationen.') + break + + if sigma < sigmaImprove: + print(f' [CMA-ES] Abbruch: sigma zu klein {sigma:.3g}') + break + + + # Cumulation: Update evolution paths ps = (1 - cs) * ps + np.sqrt(cs * (2 - cs) * mueff) * (B @ zmean) norm_ps = np.linalg.norm(ps) @@ -103,13 +131,16 @@ def escma(): # Escape flat fitness, or better terminate? if arfitness[0] == arfitness[int(np.ceil(0.7 * lambda_)) - 1]: sigma = sigma * np.exp(0.2 + cs / damps) - print("warning: flat fitness, consider reformulating the objective") + print(' [CMA-ES] stopfitness erreicht.') + #print("warning: flat fitness, consider reformulating the objective") - print(f"{counteval}: {arfitness[0]}") + #print(f"{counteval}: {arfitness[0]}") #Final Message - print(f"{counteval}: {arfitness[0]}") + #print(f"{counteval}: {arfitness[0]}") xmin = arx[:, arindex[0]] + bestValue = arfitness[0] + print(f' [CMA-ES] Ende: Gen = {gen}, best = {round(bestValue, 6)}') return xmin diff --git a/dashboard.py b/dashboard.py index 68a2999..41f3fbf 100644 --- a/dashboard.py +++ b/dashboard.py @@ -6,6 +6,7 @@ import numpy as np from GHA_triaxial.panou import gha1_ana from GHA_triaxial.panou import gha1_num from GHA_triaxial.panou_2013_2GHA_num import gha2_num +from GHA_triaxial.ES_gha2 import gha2_ES from ellipsoide import EllipsoidTriaxial import winkelumrechnungen as wu import ausgaben as aus @@ -500,16 +501,27 @@ def calc_and_plot(n1, n2, ) if "stochastisch" in method2: - # stoch - a_stoch = "noch nicht implementiert.." + beta0 = np.deg2rad(float(beta21)) + lamb0 = np.deg2rad(float(lamb21)) + beta1 = np.deg2rad(float(beta22)) + lamb1 = np.deg2rad(float(lamb22)) + alpha_1, alpha_2, s12, geo_line_es = gha2_ES( + ell, + ell.ell2cart(beta0, lamb0), + ell.ell2cart(beta1, lamb1), + all_points=True + ) + alpha_1 = 1 + alpha_2 = 2 out2.append( html.Div([ html.Strong("Stochastisch (ES): "), - html.Span(f"{a_stoch}") + html.Span(f"{aus.gms('α₁₂', alpha_1, 4)}, {aus.gms('α₂₁', alpha_2, 4)}, s = {s12:.4f} m"), ]) ) + if not method2: return html.Span("Bitte Berechnungsverfahren auswählen!", style={"color": "red"}), "", go.Figure() @@ -517,6 +529,8 @@ def calc_and_plot(n1, n2, fig = figure_constant_lines(fig, ell, "ell") if "numerisch" in method2: fig = figure_lines(fig, geo_line_num, "#ff8c00") + if "stochastisch" in method2: + fig = figure_lines(fig, geo_line_es, "#ff8c00") fig = figure_points(fig, [("P1", p1, "black"), ("P2", p2, "red")]) return "", out2, fig diff --git a/utils.py b/utils.py new file mode 100644 index 0000000..c8b6765 --- /dev/null +++ b/utils.py @@ -0,0 +1,22 @@ +from numpy import arctan2 +from numpy.typing import NDArray + +from GHA_triaxial.panou import pq_ell +from ellipsoide import EllipsoidTriaxial + + +def sigma2alpha(ell: EllipsoidTriaxial, sigma: NDArray, point: NDArray) -> float: + """ + Berechnung des Richtungswinkels an einem Punkt anhand der Ableitung zu den kartesischen Koordinaten + :param ell: Ellipsoid + :param sigma: Ableitungsvektor ver kartesischen Koordinaten + :param point: Punkt + :return: Richtungswinkel + """ + "" + p, q = pq_ell(ell, point) + P = float(p @ sigma) + Q = float(q @ sigma) + + alpha = arctan2(P, Q) + return alpha