GNC/com_guidance.py — CoM Guidance

Root of the guidance tower. Emits desired $(\bm p_c,\bm v_c,\bm a_c)$ at each step by tracking monotonic progress $s$ along the discrete helix $\Lambda=\{\bm\lambda_i\}_{i=1}^N$.

Startup Speed Ramp

Speed scales smoothly from rest; the ramp prevents an impulsive CoM demand at $t=0$:

$$v_d(t) = \Big[1 - \exp\!\big(-t/\tau_{\mathrm{su}}\big)\Big] v_{\mathrm{ref}}$$

(eq 5.1, final.tex §2.1)

With the ramp on, $v_d(0)=0$; the centripetal seed $\bm a_c(0)\approx\bm 0$ is cached in a_c0 to warm-start [[analytic_feedforward]].

Monotonic Progress & Lookahead

Progress $s$ is strictly non-decreasing — once a segment is passed, guidance never walks back:

$$s_k = \max\!\big(s_{k-1},\ \mathrm{proj}(\bm p_c \to \Lambda_{\mathrm{local}})\big)$$

The reference point lies a lookahead time $t_{\mathrm{look}}$ ahead:

$$s_{\mathrm{ref}} = s_k + \frac{v_d t_{\mathrm{look}}}{\Delta s}$$

Local search radius re-centers each step; full polyline scan runs once only.

Velocity Field: Direction Blend

At the lookahead reference, retrieve the unit tangent $\hat{\bm t}$ and curvature $\bm\kappa$. Split the tracking error into along-track and lateral components:

$$\bm e = \bm p_{\mathrm{track}} - \bm p_c,\quad e_{\parallel} = \bm e\cdot\hat{\bm t},\quad \bm e_{\perp} = \bm e - e_{\parallel}\hat{\bm t}$$

$$\bm d = \hat{\bm t} + k_{\mathrm{track}} \bm e_{\perp},\quad v_{\mathrm{cmd}} = \mathrm{sat}\!\big(v_d - k_{\mathrm{progress}} e_{\parallel}\big)$$

$$\bm v_c = v_{\mathrm{cmd}} \hat{\bm d}$$

Centripetal Acceleration Feedforward

The output acceleration $\bm a_c$ is the centripetal term required to hold the curved path at speed — not a position-error correction:

$$\bm a_c = \lVert\bm v_c\rVert^2 \bm\kappa$$

(eq 5.3, final.tex §2.2)

$\bm\kappa$ is evaluated analytically from the spherical-helix parameterisation (no finite-difference ripple).

Analytic Tangent & Curvature

Helix parameter $u\in[0,1]$: $\theta = w_\theta u$, $\phi = \phi_0 + w_\phi u$.

$$\bm p(u) = R\begin{bmatrix}\cos\phi\cos\theta\\\cos\phi\sin\theta\\\sin\phi\end{bmatrix},\qquad \hat{\bm t} = \frac{\bm p'}{\lVert\bm p'\rVert}$$

$$\bm\kappa = \frac{1}{\lVert\bm p'\rVert^2} \left(\bm p'' - \frac{\bm p'\cdot\bm p''}{\lVert\bm p'\rVert^2} \bm p'\right)$$

(eqs 5.4–5.5, final.tex §2.2) $\bm p',\bm p''$ follow analytically; $\theta,\phi$ are linear in $u$.

Reference Smoothing

After the centripetal demand is formed, a low-pass filter stabilises the reference across steps:

$$\bm v_c^{\mathrm{sm}} = (1-\alpha)\bm v_c^{\mathrm{prev}} + \alpha \bm v_c,\qquad \bm a_c^{\mathrm{sm}} = \frac{\bm v_c^{\mathrm{sm}} - \bm v_c^{\mathrm{prev}}}{\Delta t}$$

Both signals are saturated against $v_{\max}$ and $\dot v_{\max}$ from constraints.com. The smoothed $\bm a_c$ replaces the curvature-based value so $(p,v,a)$ remain dynamically consistent.