2. GHA mit CMA-ES

2026-01-12 15:18:39 +01:00
parent 6cc7245b0f
commit bded05a231
5 changed files with 302 additions and 32 deletions
--- a/GHA_triaxial/ES_gha2.py
+++ 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)
--- 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)]))

--- 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


--- 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
--- a/utils.py
+++ 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