GNC/guidance/analytic_feedforward.py — Analytic EE Feedforward Twist

Pure trajectory kinematics: desired EE twist in closed form. Replaces finite-difference feedforward mission-wide.

$$\bm{z}_{ed}=\hat{\bm{r}},\quad \bm{p}_{ed}=\bm{p}_c + r_\text{cam}\hat{\bm{r}}$$

Why Analytic over Finite-Difference?

FD feedforward: numerical noise, phase lag, step-size sensitivity.

Analytic FF: closed form from the reference alone — no state, no history.

$$\bm{\nu}_{ed}^\oplus = f\!\left(\bm{r},\dot{\bm{r}},\ddot{\bm{r}},\bm{v}_{cd},\bm{a}_{cd},\bm{R}_{ed},\bm{R}_{bd}\right)$$

Adopted Jun 21: $p_e$ median $-28\%$, $p_{e,99}$ $-11\%$, coverage up.

Unit-Vector Aim Rate $\dot{\hat{\bm{r}}}$ — Radial Part Projected Out

The projector $\bm{P} = \bm{I} - \hat{\bm{r}}\hat{\bm{r}}^\top$ strips the radial component.

$$\dot{\hat{\bm{r}}} = \frac{1}{\rho}\left(\bm{I} - \hat{\bm{r}}\hat{\bm{r}}^\top\right)\dot{\bm{r}}$$

$$\dot\rho = \hat{\bm{r}}\cdot\dot{\bm{r}}$$

Only the transverse part of $\dot{\bm{r}}$ drives the aim direction.

Aim Acceleration $\ddot{\hat{\bm{r}}}$ — Three Terms

$$\ddot{\hat{\bm{r}}} = \underbrace{\frac{1}{\rho}\bm{P}\ddot{\bm{r}}}_{\text{projected accel}} - \underbrace{\frac{2\dot\rho}{\rho}\dot{\hat{\bm{r}}}}_{\text{range rate}} - \underbrace{\lVert\dot{\hat{\bm{r}}}\rVert^2\hat{\bm{r}}}_{\text{centripetal}}$$

Term 1: transverse aim acceleration. Term 2: Coriolis-like range-rate correction. Term 3: centripetal — keeps $\hat{\bm{r}}$ on $S^2$.

Roll-Free Angular Velocity & Acceleration

$\bm{z}_{ed}=\hat{\bm{r}}$ gives a natural roll-free choice:

$$\bm{\omega}_{ed} = \hat{\bm{r}}\times\dot{\hat{\bm{r}}} \qquad \dot{\bm{\omega}}_{ed} = \hat{\bm{r}}\times\ddot{\hat{\bm{r}}}$$

Verified: $\hat{\bm{r}}\cdot\bm{\omega}_{ed}=0$ and $\bm{\omega}_{ed}\times\hat{\bm{r}}=\dot{\hat{\bm{r}}}$.

The $\dot{\hat{\bm{r}}}\times\dot{\hat{\bm{r}}}=\bm{0}$ term vanishes in $\dot{\bm{\omega}}_{ed}$.

One Pair $(\dot{\hat{\bm{r}}},\ddot{\hat{\bm{r}}})$ Feeds the Full 6-DOF Twist

$$\bm{\nu}_{ed} = \begin{bmatrix}\dot{\bm{p}}_{ed}\\\bm{\omega}_{ed}\end{bmatrix} = \begin{bmatrix}\bm{v}_{cd}+r_\text{cam}\dot{\hat{\bm{r}}}\\\hat{\bm{r}}\times\dot{\hat{\bm{r}}}\end{bmatrix}$$

$$\dot{\bm{p}}_{ed}=\bm{v}_{cd}+r_\text{cam}\dot{\hat{\bm{r}}} \qquad \ddot{\bm{p}}_{ed}=\bm{a}_{cd}+r_\text{cam}\ddot{\hat{\bm{r}}}$$

Single computation of $(\dot{\hat{\bm{r}}},\ddot{\hat{\bm{r}}})$ determines all four quantities.

Local Frame & Circumcentroidal Conversion

Express in desired EE frame ($\dot{\bm{R}}_{ed}=\bm{\omega}_{ed}^\times\bm{R}_{ed}$):

$$\bm{\nu}_{ed}^\text{local}=\begin{bmatrix}\bm{R}_{ed}^\top\dot{\bm{p}}_{ed}\\\bm{R}_{ed}^\top\bm{\omega}_{ed}\end{bmatrix} \qquad \dot{\bm{\nu}}_{ed}^\text{local}=\begin{bmatrix}\bm{R}_{ed}^\top(\ddot{\bm{p}}_{ed}-\bm{\omega}_{ed}\times\dot{\bm{p}}_{ed})\\\bm{R}_{ed}^\top\dot{\bm{\omega}}_{ed}\end{bmatrix}$$

Circumcentroidal ($\oplus$) fold (Giordano eqs 13–16):

$$\bm{\nu}_{ed}^\oplus = \bm{\nu}_{ed}^\text{local} - \bm{G}_{v_c}\bm{v}_{cd} + \bm{G}_{\omega_b}\bm{\omega}_{bd}$$

$$\dot{\bm{\nu}}_{ed}^\oplus = \dot{\bm{\nu}}_{ed}^\text{local} - \left(\dot{\bm{G}}_{v_c}\bm{v}_{cd}+\bm{G}_{v_c}\bm{a}_{cd}\right) + \dot{\bm{G}}_{\omega_b}\bm{\omega}_{bd}$$

Cruise-Lag Floor $\tilde{\bm{x}}_{ss}$ (eq 4.14)

At cruise steady-state $\dot{\tilde{\bm{x}}}=\bm{0}$, stiffness balances Coriolis + CoM coupling:

$$\tilde{\bm{x}}_{ss} = -\left(\bm{J}_{\tilde x}^\top\breve{\bm{K}}\right)^{-1}\!\left[\breve{\bm{C}}\breve{\bm{v}}_d + \left(\bm{C}_c+\breve{\bm{D}}\breve{\bm{G}}_{v_c}\right)\dot{\tilde{\bm{x}}}_c\right]$$

Coriolis-only idealization (perfect CoM tracking):

$$\tilde{\bm{x}}_{ss} = -\left(\bm{J}_{\tilde x}^\top\breve{\bm{K}}\right)^{-1}\breve{\bm{C}}\breve{\bm{v}}_d$$

Solved via $\text{lstsq}$ — minimum-norm at near-singular steps.