Linearization, Trim & Hover — the design model

1 · The 5-minute recap

Hover trim is the quad's one equilibrium: level (\( \boldsymbol\eta=\mathbf 0 \)), at rest (\( \mathbf v=\boldsymbol\omega=\mathbf 0 \)), thrust = weight (\( T=mg \), along \(+\)body-z, which is world-up in our z-up / ENU convention), and zero torque. By construction the state-derivative vanishes there:

\[ \mathbf u_{eq}=\begin{bmatrix} mg & 0 & 0 & 0 \end{bmatrix}^{\!\top},\qquad f(\mathbf x_{eq},\mathbf u_{eq})=\mathbf 0. \]

Write \( \mathbf x=\mathbf x_{eq}+\delta\mathbf x \), \( \mathbf u=\mathbf u_{eq}+\delta\mathbf u \) and keep first order. analysis.linearize does this with central finite differences of the real nonlinear \( f= \)state_derivative:

\[ \dot{\delta\mathbf x}\approx A\,\delta\mathbf x + B\,\delta\mathbf u,\qquad A=\tfrac{\partial f}{\partial \mathbf x}\big|_{eq}\ (12\times12),\quad B=\tfrac{\partial f}{\partial \mathbf u}\big|_{eq}\ (12\times4). \]

The poles are \( \operatorname{eig}(A) \); at hover several are exactly zero (integrators) ⇒ marginally stable, not asymptotically stable. The fix is feedback. First check it is even fixable — controllability:

\[ \mathcal C=\big[\,B\ \ AB\ \ A^2B\ \cdots\ A^{11}B\,\big],\qquad \operatorname{rank}\mathcal C \overset{!}{=} 12. \]

2 · Setup (2 min)

From the simulator/ directory, in your virtual environment:

# one-time
python -m venv .venv && source .venv/bin/activate   # Windows: .venv\Scripts\activate
pip install -r requirements.txt
# every session — so `import quadsim` works
export PYTHONPATH=.                                # Windows: set PYTHONPATH=.

Everything today lives in quadsim/analysis.py (read it — it is short and well commented) and the reference walk-through examples/04_analysis.py. You will write a small new script, examples/wk4_analysis.py, that isolates the Week-4 pieces.

3 · Build the linear model

Work through the steps. Each adds a few lines to examples/wk4_analysis.py. The functions you need — hover_equilibrium, linearize, poles, controllability — all live in quadsim.analysis.

import numpy as np
from quadsim import QuadParams, analysis as an
from quadsim.dynamics import state_derivative

p = QuadParams()
x_eq, u_eq = an.hover_equilibrium(p)          # u_eq = [p.weight, 0, 0, 0]
print("max |f(x_eq,u_eq)| =", np.max(np.abs(state_derivative(x_eq, u_eq, p))))

A, B = an.linearize(p)                        # about hover by default
print("A:", A.shape, " B:", B.shape)   # (12, 12)  (12, 4)

ol = an.poles(A)
n_origin = int(np.sum(np.abs(ol.real) < 1e-6))
print(f"{n_origin} open-loop poles at the origin (marginally stable)")

C, rank = an.controllability(A, B)
print(f"controllability rank {rank}/12")
assert rank == 12, "system is not controllable — check A,B"

from quadsim import plotting as viz
viz.plot_poles([ol], labels=["open loop (hover)"],
               save="wk4_poles.png", show=False)
print("saved wk4_poles.png")

# examples/wk4_analysis.py — Week 4: linearize about hover, poles, controllability
import numpy as np
from quadsim import QuadParams, analysis as an
from quadsim.dynamics import state_derivative
from quadsim import plotting as viz

p = QuadParams()

# 1) hover trim — state-derivative must vanish
x_eq, u_eq = an.hover_equilibrium(p)
print("max |f(x_eq,u_eq)| =", np.max(np.abs(state_derivative(x_eq, u_eq, p))))

# 2) linearize the real nonlinear model about hover
A, B = an.linearize(p)
print("A:", A.shape, " B:", B.shape)        # (12,12) (12,4)

# 3) open-loop poles
ol = an.poles(A)
n_origin = int(np.sum(np.abs(ol.real) < 1e-6))
print(f"{n_origin} poles at the origin (marginally stable)")

# 4) controllability
C, rank = an.controllability(A, B)
print(f"controllability rank {rank}/12")
assert rank == 12

# 5) deliverable: the pole map
viz.plot_poles([ol], labels=["open loop (hover)"],
               save="wk4_poles.png", show=False)
print("saved wk4_poles.png")

4 · Run & verify

Save the script above as examples/wk4_analysis.py, then run it:

export PYTHONPATH=.
python examples/wk4_analysis.py

For the bigger picture, run the reference walk-through — it does the same Week-4 analysis, then goes on to design the Week-7 LQR gain on this very \( (A,B) \) and flies a figure-eight (a preview of where this leads):

python examples/04_analysis.py

Self-check

• I confirmed f(x_eq, u_eq) is zero — so hover is a genuine equilibrium.
• A is 12×12 and B is 12×4, and I can say what each block maps (state→state, wrench→state).
• I see several open-loop poles at the origin and can explain why that is "marginally stable, not asymptotically stable".
• Controllability rank is 12, and I understand why that is the prerequisite for LQR/MPC.

5 · Going further (optional)

• Look inside A. Print np.round(A, 2). Find the integrator blocks — where position rows pick up velocity, and Euler rows pick up body rates (a sub-diagonal of 1's).
• Off-hover trim. Call an.hover_equilibrium(p, yaw=np.deg2rad(30)) and re-linearize. Do the poles move? (They should not — hover is hover, just rotated.)
• Peek ahead to Week 7. Run K,_,cl = an.lqr(A,B,Q,R) with the \(Q,R\) from 04_analysis.py and overlay plot_poles([ol, cl], labels=["open loop","LQR closed loop"]). Watch the poles march into the left half-plane.

6 · Deliverable & submission

Next week → Week 5: attitude control. You take this marginally-stable model and close the first feedback loop — attitude — pulling the rotational poles into the left half-plane.