Cart-Pole iLQR Online-MPC

Detaillierte Algorithmus-Beschreibung der Implementierung  |  2026

1   System: Cart-Pole-Dynamik

Zustandsraum und Parameter

Das System ist ein inverses Pendel auf einem Wagen (Cart-Pole). Der Zustandsvektor und der Steuereingang sind:

$$\mathbf{x} = \begin{pmatrix} x_c \\ \dot{x}_c \\ \theta \\ \dot{\theta} \end{pmatrix} \in \mathbb{R}^4, \qquad \mathbf{u} = \begin{pmatrix} F \end{pmatrix} \in \mathbb{R}^1$$
SymbolBedeutungWert
$x_c$Wagenposition—
$\dot{x}_c$Wagengeschwindigkeit—
$\theta$Pendelwinkel (0 = oben)—
$\dot{\theta}$Winkelgeschwindigkeit—
$F$horizontale Kraft auf Wagen$|F| \le 20\,\text{N}$
$m_c$Wagenmasse$1{,}0\,\text{kg}$
$m_p$Pendelmasse$0{,}2\,\text{kg}$
$l$Pendellänge$0{,}5\,\text{m}$
$g$Erdbeschleunigung$9{,}81\,\text{m/s}^2$
$\Delta t$Integrationsschritt$0{,}02\,\text{s}$

Nichtlineare Bewegungsgleichungen

Aus der Lagrange-Mechanik folgt die Nenner-Größe $D = m_c + m_p \sin^2\!\theta$ und die Beschleunigungen:

$$\ddot{x}_c = \frac{F + m_p\sin\theta\,\bigl(l\dot{\theta}^2 - g\cos\theta\bigr)}{D}$$ $$\ddot{\theta} = \frac{(m_c+m_p)\,g\sin\theta - F\cos\theta - m_p\,l\,\dot{\theta}^2\cos\theta\sin\theta}{l\,D}$$

Euler-Integration (Vorwärts-Euler)

Der nächste Zustand wird durch einen einfachen expliziten Euler-Schritt berechnet:

$$\mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \cdot \dot{\mathbf{x}}_k \;=\; f(\mathbf{x}_k, \mathbf{u}_k)$$
Code-Auszug — dynamics(), Zeile 219–227 
const dynamics = (x, u) => {
    const [xc, xcd, th, thd] = x;
    const F = u[0];
    const s = Math.sin(th), c = Math.cos(th);
    const D = PHYS.mc + PHYS.mp * s * s;
    const xdd  = (F + PHYS.mp * s * (PHYS.l * thd*thd - PHYS.g * c)) / D;
    const thdd = ((PHYS.mc+PHYS.mp)*PHYS.g*s - F*c - PHYS.mp*PHYS.l*thd*thd*c*s) / (PHYS.l*D);
    return [xc + DT*xcd, xcd + DT*xdd, th + DT*thd, thd + DT*thdd];
};
Hinweis: Der Winkel $\theta = \pi$ entspricht der hängenden Ruheposition (Startzustand). Das Regelziel ist $\mathbf{x}^* = \mathbf{0}$, d. h. Pendel senkrecht oben, Wagen zentriert.

2   Analytische Jacobians (Linearisierung)

iLQR linearisiert die nichtlineare Dynamik entlang der aktuellen nominellen Trajektorie. Dazu werden die partiellen Ableitungen von $f$ nach $\mathbf{x}$ und $\mathbf{u}$ an jedem Zeitschritt $k$ analytisch ausgewertet:

$$A_k = \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\mathbf{x}_k,\,\mathbf{u}_k}, \qquad B_k = \frac{\partial f}{\partial \mathbf{u}}\bigg|_{\mathbf{x}_k,\,\mathbf{u}_k}$$

Struktur der diskreten Matrizen

Mit dem Euler-Schritt $x_{k+1} = x_k + \Delta t \cdot \dot{x}(x_k, u_k)$ gilt für die Jacobians:

$$A_k = I + \Delta t \cdot \frac{\partial \dot{\mathbf{x}}}{\partial \mathbf{x}}, \qquad B_k = \Delta t \cdot \frac{\partial \dot{\mathbf{x}}}{\partial \mathbf{u}}$$

Explizit (nur Nicht-Null-Einträge abweichend von der Einheitsmatrix):

$$A_k = \begin{pmatrix} 1 & \Delta t & 0 & 0 \\ 0 & 1 & \Delta t\,\frac{\partial \ddot{x}_c}{\partial \theta} & \Delta t\,\frac{\partial \ddot{x}_c}{\partial \dot{\theta}} \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & \Delta t\,\frac{\partial \ddot{\theta}}{\partial \theta} & 1+\Delta t\,\frac{\partial \ddot{\theta}}{\partial \dot{\theta}} \end{pmatrix}, \qquad B_k = \begin{pmatrix} 0 \\ \Delta t / D \\ 0 \\ -\Delta t\cos\theta/(l\,D) \end{pmatrix}$$

Die Nicht-Null-Partialableitungen werden durch Quotientenregel aus den Ausdrücken für $\ddot{x}_c$ und $\ddot{\theta}$ bestimmt. Schlüsselvariablen dabei:

$$D = m_c + m_p\sin^2\!\theta, \qquad N_x = F + m_p\sin\theta\,(l\dot{\theta}^2 - g\cos\theta), \qquad N_\theta = (m_c+m_p)g\sin\theta - F\cos\theta - m_p l\dot{\theta}^2\cos\theta\sin\theta$$ $$\frac{\partial D}{\partial \theta} = m_p \cdot 2\sin\theta\cos\theta = m_p\sin 2\theta$$
Code-Auszug — jacobians(), Zeile 95–118 
function jacobians(x, u) {
    const P = PHYS;
    const [,, th, thd] = x, F = u[0];
    const s = Math.sin(th), c = Math.cos(th);
    const s2 = 2*s*c, c2 = c*c - s*s;            // sin(2θ), cos(2θ)
    const D = P.mc + P.mp*s*s, D2 = D*D;
    const Nx  = F + P.mp*s*(P.l*thd*thd - P.g*c);
    const Nth = (P.mc + P.mp)*P.g*s - F*c - P.mp*P.l*thd*thd*c*s;
    const dxdd_dth   = P.mp*(c*P.l*thd*thd - P.g*c2)/D - Nx*P.mp*s2/D2;
    const dxdd_dthd  = 2*P.mp*s*P.l*thd/D;
    const dxdd_dF    = 1/D;
    const dthdd_dth  = ((P.mc + P.mp)*P.g*c - P.mp*P.l*thd*thd*c2)/(P.l*D)
                       - Nth*P.mp*s2/(P.l*D2);
    const dthdd_dthd = -P.mp*thd*s2/D;
    const dthdd_dF   = -c/(P.l*D);
    const A = [
        [1, DT, 0,             0               ],
        [0, 1,  DT*dxdd_dth,   DT*dxdd_dthd   ],
        [0, 0,  1,             DT              ],
        [0, 0,  DT*dthdd_dth,  1+DT*dthdd_dthd]
    ];
    const B = [[0], [DT*dxdd_dF], [0], [DT*dthdd_dF]];
    return { A, B };
}
Warum analytische Jacobians? Numerische Differentiation (finite Differenzen) wäre langsamer und ungenauer. Da die Cart-Pole-Dynamik geschlossen differenzierbar ist, werden die Jacobians exakt in $O(1)$ ausgewertet — ein entscheidender Faktor für Echtzeit-Betrieb.

3   DARE — Diskrete Algebraische Riccati-Gleichung

Vor dem Start des Online-MPC wird einmalig die Diskrete Algebraische Riccati-Gleichung (DARE) am linearisierten Gleichgewicht $\mathbf{x}^* = \mathbf{0}$ gelöst. Das liefert zwei Dinge:

  1. Endkostenmatrix $P_\infty = Q_f$ — gibt dem iLQR-Algorithmus einen informierten Abschlusskostenterm, der den Wert nach dem Planungshorizont approximiert.
  2. LQR-Gain $K_\text{lqr}$ — dient als Fallback-Regler vor der ersten iLQR-Lösung und zum Auffüllen des Warm-Starts.

Linearisierung am Gleichgewicht

Für kleine Winkel ($\sin\theta \approx \theta$, $\cos\theta \approx 1$) vereinfacht sich das System zu einem linearen Modell mit:

$$A_\text{lin} = \begin{pmatrix} 1 & \Delta t & 0 & 0 \\ 0 & 1 & -\frac{\Delta t\, m_p g}{m_c} & 0 \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & \frac{\Delta t(m_c+m_p)g}{l\,m_c} & 1 \end{pmatrix}, \qquad B_\text{lin} = \begin{pmatrix} 0 \\ \Delta t / m_c \\ 0 \\ -\Delta t / (l\,m_c) \end{pmatrix}$$

DARE-Iteration

Das infinite-horizon LQR-Problem mit Kosten $\sum_{k=0}^\infty (\mathbf{x}_k^T Q\,\mathbf{x}_k + \mathbf{u}_k^T R\,\mathbf{u}_k)$ wird durch die Diskrete Riccati-Gleichung gelöst:

$$P_{i+1} = Q + A^T P_i A - A^T P_i B\,\underbrace{(R + B^T P_i B)^{-1}}_{\text{skalarer Quotient}}\,B^T P_i A$$

Die Iteration beginnt mit $P_0 = Q$ und konvergiert nach 500 Schritten gegen $P_\infty$. Der optimale LQR-Gain ergibt sich dann als:

$$K_\text{lqr} = (R + B^T P_\infty B)^{-1} B^T P_\infty A$$

Da $\mathbf{u} \in \mathbb{R}^1$, ist $(R + B^T P_\infty B)$ ein Skalar — die Matrix-Inverse reduziert sich auf eine Division.

Kostgewichte

MatrixDiagonaleGewichtet
$Q$ (Zustandskosten)$[10,\;1,\;80,\;5]$$x_c$, $\dot{x}_c$, $\theta$, $\dot{\theta}$
$Q_f$ (Endkosten)$[2,\;0.2,\;200,\;10]$stärkeres Gewicht auf $\theta$ am Horizont-Ende
$R$ (Steuerkosten)$[0.05]$Kraftaufwand
Code-Auszug — computeDARE(), Zeile 229–247 
const computeDARE = (maxIter = 500) => {
    const A = [
        [1, DT, 0,                                                       0  ],
        [0, 1,  -DT * PHYS.mp * PHYS.g / PHYS.mc,                       0  ],
        [0, 0,  1,                                                       DT ],
        [0, 0,  DT * (PHYS.mc + PHYS.mp) * PHYS.g / (PHYS.l * PHYS.mc), 1 ]
    ];
    const B = [[0], [DT / PHYS.mc], [0], [-DT / (PHYS.l * PHYS.mc)]];
    const AT = nn.transpose(A), BT = nn.transpose(B);
    const Q = diagMat(Qd);
    let Pm = Q;
    for (let i = 0; i < maxIter; i++) {
        const ATP  = nn.dot(AT, Pm);
        const ATPB = nn.dot(ATP, B);
        const S    = nn.dot(nn.dot(BT, Pm), B)[0][0] + Rd[0];   // Skalar
        Pm = nn.sub(nn.add(Q, nn.dot(ATP, A)),
                    nn.mul(1/S, nn.dot(ATPB, nn.dot(nn.dot(BT, Pm), A))));
    }
    return { Pm, A, B };
};

4   Warm-Start-Strategie

Online-MPC löst in jedem Simulationsschritt ($\Delta t = 20\,\text{ms}$) ein neues Optimierungsproblem. Da sich aufeinanderfolgende Probleme nur um einen Zeitschritt verschieben, kann die vorherige Lösung als Startpunkt (Warm-Start) genutzt werden, um die Konvergenz drastisch zu beschleunigen.

Algorithmus

Eingabe:  x₀  (aktueller Zustand),  prevUs  (alte Steuertrajektorie)
Ausgabe:  us  (initiale Steuertrajektorie der Länge N=50)

us ← prevUs[1..N]              // schiebe um 1 Schritt vorwärts (MPC-Shift)
x  ← rollout(x₀, us)          // simuliere bis zum Ende des alten Plans

WHILE |us| < N:
    F_lqr ← clip(−K_lqr · x, −F_MAX, F_MAX)   // LQR-Fallback
    us.append([F_lqr])
    x ← dynamics(x, [F_lqr])

RETURN us

Die Shift-Operation stellt sicher, dass die bereits optimierten Steuerwerte wiederverwendet werden. Die verbleibenden Schritte am Horizontende werden mit dem (suboptimalen, aber stabilen) LQR-Regler aufgefüllt.

Praktischer Effekt: Ohne Warm-Start benötigt iLQR typischerweise viele Iterationen bis zur Konvergenz. Mit Warm-Start reicht oft eine einzige Iteration, da die Startlösung bereits nahe am Optimum liegt — entscheidend für Echtzeit-Fähigkeit im Browser-Main-Thread.
Code-Auszug — buildWarmStart(), Zeile 134–146 
function buildWarmStart(x0, prevUs) {
    const us = prevUs ? prevUs.slice(0, N_TRAJ) : [];  // MPC-Shift
    let x = x0.slice();
    for (let i = 0; i < us.length; i++) x = dynamics(x, us[i]);
    while (us.length < N_TRAJ) {                       // LQR-Auffüllung
        let lqr = 0;
        for (let j = 0; j < K_lqr.length; j++) lqr -= K_lqr[j] * x[j];
        lqr = Math.max(-F_MAX, Math.min(F_MAX, lqr));
        us.push([lqr]);
        x = dynamics(x, [lqr]);
    }
    return us;
}

5   iLQR-Kernalgorithmus

iLQR (iterative Linear Quadratic Regulator) löst das nichtlineare Optimalsteuerungsproblem mit endlichem Horizont:

$$\min_{\mathbf{u}_0,\ldots,\mathbf{u}_{N-1}} \;\; \underbrace{\mathbf{x}_N^T Q_f \mathbf{x}_N}_{\text{Endkosten}} + \sum_{k=0}^{N-1} \Bigl( \underbrace{\mathbf{x}_k^T Q\,\mathbf{x}_k}_{\text{Zustandskosten}} + \underbrace{\mathbf{u}_k^T R\,\mathbf{u}_k}_{\text{Steuerkosten}} \Bigr) \quad \text{s.t.} \quad \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$$

Die Hauptschleife wiederholt Rückwärts-Pass → Liniensuche → Vorwärts-Pass bis zu $\text{MAX\_ITER} = 10$ Mal oder bis zur Konvergenz. 

GEGEBEN:  x₀, prevUs, MAX_ITER=10, N=50

us  ← buildWarmStart(x₀, prevUs)
xs  ← rollout(x₀, us)                    // Vorwärts-Rollout

FOR iter = 1 … MAX_ITER:
    ks, Ks ← backwardPass(xs, us)          // Rückwärts-Pass
    xs_new, us_new ← lineSearch(xs, us, ks, Ks, x₀)  // Liniensuche
    IF xs_new ≠ null:  xs, us ← xs_new, us_new
    IF max|ks| < 1e-4:  BREAK             // Konvergenz

RETURN xs, us

5a   Vorwärts-Rollout

Die nominelle Trajektorie $\{x_0, x_1, \ldots, x_N\}$ wird durch Simulation mit der aktuellen Steuertrajektorie $\{u_0, \ldots, u_{N-1}\}$ erzeugt:

$$x_{k+1} = f(x_k, u_k), \quad k = 0, 1, \ldots, N-1$$

Die Gesamtkosten werden von totalCostFast() berechnet:

$$J = \sum_{k=0}^{N-1} \bigl(x_k^T Q\, x_k + u_k^T R\, u_k\bigr) + x_N^T Q_f x_N$$

5b   Rückwärts-Pass — Riccati-Rekursion

Das Herzstück von iLQR. Die Action-Value-Funktion $Q(x_k, u_k)$ wird rückwärts in der Zeit durch quadratische Näherung approximiert. Ausgangspunkt ist die Wert-Funktion am Horizontende:

$$V_x^{(N)} = 2\,Q_f\,x_N, \qquad V_{xx}^{(N)} = 2\,Q_f$$

Dann für $k = N-1, N-2, \ldots, 0$:

Gradients der Kostenfunktion

$$l_x = \frac{\partial l_k}{\partial x} = 2Q\,x_k, \qquad l_u = \frac{\partial l_k}{\partial u} = 2R\,u_k$$ $$l_{xx} = 2Q, \qquad l_{uu} = 2R \qquad (\text{konstant})$$

Q-Funktion (quadratische Näherung)

\begin{align} Q_x &= l_x + A_k^T\,V_x^{(k+1)} \\ Q_u &= l_u + B_k^T\,V_x^{(k+1)} \\ Q_{xx}&= l_{xx}+ A_k^T\,V_{xx}^{(k+1)}\,A_k \\ Q_{ux}&= B_k^T\,V_{xx}^{(k+1)}\,A_k \\ Q_{uu}&= l_{uu}+ B_k^T\,V_{xx}^{(k+1)}\,B_k \end{align}

Tikhonov-Regularisierung

Um sicherzustellen, dass $Q_{uu}$ positiv definit und damit invertierbar ist, wird eine kleine Regularisierung addiert:

$$\tilde{Q}_{uu} = \max(Q_{uu},\; 0) + 0{,}01$$

Da $\mathbf{u} \in \mathbb{R}^1$ ist $Q_{uu}$ ein Skalar — kein Matrix-Eigenwertalgorithmus nötig.

Optimale Gains

$$k_k = -\tilde{Q}_{uu}^{-1}\,Q_u \quad \in \mathbb{R}^1 \qquad \text{(Feedforward-Korrektur)}$$ $$K_k = -\tilde{Q}_{uu}^{-1}\,Q_{ux} \quad \in \mathbb{R}^{1\times4} \qquad \text{(Feedback-Verstärkung)}$$

Value-Funktion Update

\begin{align} V_x^{(k)} &= Q_x + K_k^T\,Q_u \\ V_{xx}^{(k)}&= Q_{xx}+ K_k^T\,Q_{ux} \end{align}
Code-Auszug — Rückwärts-Pass, Zeile 156–183 
let V_x  = nn.dot(nn.mul(2, Qf), xs[N_TRAJ]);    // 2·Qf·x_N
let V_xx = nn.mul(2, Qf);                          // 2·Qf

for (let k = N_TRAJ - 1; k >= 0; k--) {
    const { A, B } = jacobians(xs[k], us[k]);
    const AT = nn.transpose(A), BT = nn.transpose(B);
    const x = xs[k], u = us[k];
    const lx = [2*Qd[0]*x[0], 2*Qd[1]*x[1], 2*Qd[2]*x[2], 2*Qd[3]*x[3]];
    const lu = [2*Rd[0]*u[0]];

    const Q_x  = nn.add(lx,  nn.dot(AT, V_x));
    const Q_u  = nn.add(lu,  nn.dot(BT, V_x));
    const Q_xx = nn.add(lxx, nn.dot(nn.dot(AT, V_xx), A));
    const Q_ux = nn.dot(nn.dot(BT, V_xx), A);
    const Q_uu = nn.add(luu, nn.dot(nn.dot(BT, V_xx), B));

    const quu = Math.max(Q_uu[0][0], 0) + 0.01;    // Regularisierung
    ks[k] = [-Q_u[0] / quu];                        // Feedforward k_k
    Ks[k] = [Q_ux[0].map(v => -v / quu)];           // Feedback K_k

    V_x  = nn.add(Q_x,  nn.dot(nn.transpose(Q_ux), ks[k]));
    V_xx = nn.add(Q_xx, nn.dot(nn.transpose(Ks[k]), Q_ux));
}

5c   Backtracking Line Search

Der Rückwärts-Pass liefert Richtungen $\{k_k, K_k\}$, aber keine garantierte Verbesserung bei vollem Schritt $\alpha = 1$. Die Liniensuche findet den größten Schritt, der die Kosten tatsächlich reduziert.

Affines Feedback-Gesetz

Die Steuervariante im Vorwärts-Pass bei Schrittweite $\alpha$:

$$\delta u_k = \alpha\,k_k + K_k\,\delta x_k, \qquad \delta x_k = \hat{x}_k - x_k$$ $$\hat{u}_k = u_k + \delta u_k, \qquad \hat{u}_k \leftarrow \text{clip}(\hat{u}_k,\, -F_\text{max},\, F_\text{max})$$

Der erste Term $\alpha k_k$ ist die globale Schrittweite (skaliert die Feedforward-Korrektur); der zweite Term $K_k \delta x_k$ gleicht lokal ab, wenn der neue Rollout von der nominellen Trajektorie abweicht.

Suchalgorithmus

alpha ← 1.0
FOR ls = 0 … 9:
    xs_t, us_t ← forwardRollout(x₀, us, ks, Ks, alpha)
    IF totalCost(xs_t, us_t) < totalCost(xs, us):
        xs_new, us_new ← xs_t, us_t
        BREAK
    alpha ← alpha × 0.5          // halbiere Schrittweite

Wird keine Verbesserung gefunden (alle 10 Versuche schlagen fehl), bleibt die aktuelle Trajektorie unverändert.

Code-Auszug — Line Search, Zeile 186–204 
let alpha = 1.0, xs_new = null, us_new = null;

for (let ls = 0; ls < 10; ls++) {
    const xs_t = [x0.slice()], us_t = [];
    let bad = false;
    for (let kk = 0; kk < N_TRAJ; kk++) {
        const dx = nn.sub(xs_t[kk], xs[kk]);
        const du = nn.add(nn.mul(alpha, ks[kk]), nn.dot(Ks[kk], dx));
        const dv = Array.isArray(du) ? du[0] : du;
        const Fk = Math.max(-F_MAX, Math.min(F_MAX, us[kk][0] + dv));
        if (!isFinite(Fk)) { bad = true; break; }
        us_t.push([Fk]);
        xs_t.push(dynamics(xs_t[kk], [Fk]));
    }
    if (!bad && totalCostFast(xs_t, us_t) < costOld) {
        xs_new = xs_t; us_new = us_t; break;
    }
    alpha *= 0.5;
}

5d   Konvergenzprüfung

Die Norm der Feedforward-Gains $k_k$ misst, wie groß die verbleibenden Verbesserungsmöglichkeiten sind. Bei hinreichend kleinen Werten ist das Optimum (lokal) erreicht:

$$\max_{k=0,\ldots,N-1} |k_k| < 10^{-4} \quad \Rightarrow \quad \text{Abbruch}$$
Code-Auszug — Konvergenzprüfung, Zeile 207–213 
let maxK = 0;
for (let ki = 0; ki < N_TRAJ; ki++) {
    const v = Math.abs(ks[ki][0]);
    if (v > maxK) maxK = v;
}
if (maxK < 1e-4) break;

6   Online-MPC-Architektur

Der gesamte Regelkreis läuft als Receding-Horizon MPC: In jedem Simulationsschritt wird ein neues Optimierungsproblem gelöst und nur die erste Steuerung des Plans ausgeführt.

Zeitdiagramm eines Schritts

  t = k·Δt
  │
  ├─ 1. Steuerung anwenden:  u ← activePlan.us[step]  (oder LQR-Fallback)
  ├─ 2. Zustand propagieren: x ← dynamics(x, u)
  ├─ 3. Plan-Zeiger inkrementieren: step++
  └─ 4. Neues iLQR-Problem starten: triggerSolve()
        ├─ Warm-Start: prevUs ← activePlan.us[step:]
        ├─ iLQR(x_aktuell, prevUs)  → neues {xs, us}
        └─ activePlan ← { us: res.us, step: 0 }

Horizont und Zeitskala

GrößeWertBedeutung
$N$50 SchrittePlanungshorizont = 1 Sekunde
$\Delta t$20 msSimulations- und Regelschritt
MAX_ITER10Maximale iLQR-Iterationen pro Schritt
$F_\text{max}$20 NKraft-Clipping

Fallback-Hierarchie

Bevor die erste iLQR-Lösung vorliegt (Initialisierungsphase), übernimmt der LQR-Regler:

$$u = \begin{cases} \text{activePlan.us}[\text{step}] & \text{wenn iLQR-Plan vorhanden} \\ -K_\text{lqr}\,x & \text{LQR-Fallback} \end{cases}$$
Code-Auszug — triggerSolve() + Regelkreis, Zeile 276–303 / 436–454 
// triggerSolve — startet iLQR und aktualisiert den aktiven Plan
const triggerSolve = () => {
    if (!K_lqr) return;
    const prevUs = activePlan ? activePlan.us.slice(activePlan.step) : null;
    const t0 = performance.now();
    const res = solveiLQR([...actualState], prevUs);
    solveInfo = { ms: Math.round(performance.now() - t0), iters: res.iters };
    activePlan = { us: res.us, step: 0 };
};

// loop — Animations-/Regelkreis (requestAnimationFrame)
const loop = ts => {
    if (running && ts - lastTime >= DT * 1000) {
        const u = Math.max(-F_MAX, Math.min(F_MAX, getControl(actualState)));
        draw(actualState, u);
        actualState = dynamics(actualState, [u]);
        if (activePlan) activePlan.step++;
        simTime += DT;
        triggerSolve();         // neues iLQR-Problem in jedem Schritt
        lastTime = ts;
    }
    requestAnimationFrame(loop);
};
Wichtiger Hinweis: Die Optimierung läuft synchron im Browser-Main-Thread (kein Web Worker). Bei 10 iLQR-Iterationen × 50 Schritte = 500 Rückwärts-Pass-Auswertungen pro Regelschritt ist die analytische Jacobian-Berechnung und die skalare $Q_{uu}$-Invertierung (statt Matrizeninversion) entscheidend für die Echtzeittauglichkeit.

Zusammenfassung des Algorithmen-Stacks

── Einmalige Initialisierung ──────────────────────────────────────────
1. computeDARE()     → Qf (Endkostenmatrix), K_lqr (Fallback-Regler)

── Jeden Simulationsschritt (Δt = 20 ms) ─────────────────────────────
2. getControl(x)     → u aus activePlan oder K_lqr-Fallback
3. dynamics(x, u)    → x_neu  (Euler-Integration der nichtlin. DGL)
4. triggerSolve():
      a. buildWarmStart(x, prevUs)       → us_init  (Shift + LQR-Fill)
      b. solveiLQR(x, us_init):
           FOR iter = 1…10:
              i.  rollout(x, us)              → xs  (Zustandstrajektorie)
              ii. backwardPass(xs, us)         → ks, Ks  (Gains)
              iii.lineSearch(xs, us, ks, Ks) → xs_neu, us_neu
              iv. convergenceCheck(ks)         → ggf. BREAK
      c. activePlan ← { us: res.us, step: 0 }