MLTE03 · Week 4 · Linearization, Trim & Hover

MLTE03 · Week 4 · Lecture + Lab

Linearization, Trim & Hover

Hover equilibrium & the linear model · 線性化、配平與懸停平衡

Flight Dynamics & Intelligent Control Technologies
Freeze the nonlinear quad at hover, and out falls the \( (A,B) \) every controller is built on.

Recap → today

From a nonlinear model to a design model

  • Wk 1: frames, ZYX Euler, quaternions, the conventions sheet.
  • Wk 2–3: the 12-state nonlinear model \( \dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u}) \) and the X-frame mixer.
  • Today: pin it at hover, linearize → \( (A,B) \), read the open-loop poles.
Why bother linearizing?
Almost all the classical & optimal control we use next — pole placement, LQR (Wk 7), MPC (Wk 11–12) — is designed on a linear model. Hover is where we get one.

Learning objectives

By the end of today you can…

  • Define the hover trim: level, at rest, thrust = weight, zero torque ⇒ \( f(\mathbf{x}_{eq},\mathbf{u}_{eq})=\mathbf 0 \).
  • Numerically linearize \( f(\mathbf{x},\mathbf{u}) \) about hover by finite differences → \( (A,B) \).
  • Read the open-loop poles: hover is a chain of integrators ⇒ not asymptotically stable.
  • Confirm the pair is controllable (rank 12) — the prerequisite for LQR/MPC.
  • Recall the RK4 closed-loop loop and the SimLog you will plot.
  • Lab Build \( (A,B) \), plot the pole map, confirm rank = 12.

The equilibrium

What is hover trim?

  • Level — zero roll, pitch, yaw: \( \boldsymbol\eta=\mathbf 0 \).
  • At rest — zero linear velocity and body rates: \( \mathbf v=\boldsymbol\omega=\mathbf 0 \).
  • Thrust balances weight — \( T=mg \), pointing \(+\)body-z (which, level, is world up).
  • Zero torque — \( \boldsymbol\tau=\mathbf 0 \).
Convention check (z-up / ENU): gravity is \( (0,0,-g) \), thrust acts along \(+\)body-z, so \[ \mathbf u_{eq}=\begin{bmatrix} mg \\ 0 \\ 0 \\ 0 \end{bmatrix},\qquad f(\mathbf x_{eq},\mathbf u_{eq})=\mathbf 0. \]

In quadsim this is exactly analysis.hover_equilibrium(p) → returns (x_eq, u_eq) with u_eq = [p.weight, 0, 0, 0].

The design model

Linearize by finite differences

Write the state as a small deviation from trim, \( \mathbf x=\mathbf x_{eq}+\delta\mathbf x,\ \mathbf u=\mathbf u_{eq}+\delta\mathbf u \). To first order:

\[ \dot{\delta\mathbf x}\approx A\,\delta\mathbf x + B\,\delta\mathbf u,\qquad A=\left.\frac{\partial f}{\partial \mathbf x}\right|_{eq},\quad B=\left.\frac{\partial f}{\partial \mathbf u}\right|_{eq} \]
  • We never differentiate by hand — we hit the same nonlinear \(f\) with central differences.
  • \(A\) is 12×12 (state→state), \(B\) is 12×4 (wrench→state).
  • Column \(i\) of \(A\): \( \dfrac{f(\mathbf x_{eq}+\varepsilon\mathbf e_i,\mathbf u_{eq})-f(\mathbf x_{eq}-\varepsilon\mathbf e_i,\mathbf u_{eq})}{2\varepsilon} \).
Built from the model the students fly — not a separate black box. Same \(f\) = dynamics.state_derivative, so the linear model can never silently drift from the sim.

Why we need feedback

The open-loop poles sit at the origin

The poles are the eigenvalues of \(A\). For hover, many of them are exactly zero:

\[ \lambda_i=\operatorname{eig}(A),\qquad \text{several } \lambda_i = 0 \;\;(\text{Re}=0). \]
  • Position integrates velocity; attitude integrates rate ⇒ chains of integrators.
  • Poles on the imaginary axis ⇒ marginally stable, NOT asymptotically stable.
  • A torque bias is never corrected — it drifts away. This is why the open-loop quad tips.
Marginal stability is the whole motivation for the course: feedback must pull these poles into the left half-plane. Wk 5 does it for attitude; Wk 7 (LQR) & Wk 11–12 (MPC) for the rest.

Can we even fix it?

Controllability: full rank 12

Before designing any controller, check the four inputs can actually steer all twelve states. Build the controllability matrix and take its rank:

\[ \mathcal C=\big[\,B\ \ AB\ \ A^2B\ \ \cdots\ \ A^{11}B\,\big],\qquad \operatorname{rank}\mathcal C = 12. \]
Full rank (\(=n=12\)) ⇒ every state is reachable from the wrench \( [T,\tau_x,\tau_y,\tau_z] \). This is the green light for LQR (Wk 7): a stabilizing gain exists. In quadsim:
C, rank = analysis.controllability(A, B) # rank == 12

How the sim runs

The RK4 closed-loop & SimLog

   reference(t) ─►┌──────────────┐  wrench  ┌──────────┐  motors  ┌───────────────┐
                  │  controller  ├──[T,τ]──►│  mixer + ├──f_i────►│  RK4 step on  ├─► x(t+dt)
   state x ──────►│  .control()  │          │ saturate │          │ state_deriv f │
                  └──────────────┘          └──────────┘          └───────▲───────┘
                          ▲                  records into SimLog          │ loop
                          └──────────────────────────────────────────────┘
  • Each step: wrench = controller.control(t, x, ref) → mixed to motor thrusts → clipped to \([f_{min},f_{max}]\) → applied.
  • SimLog records t, x (N×12), u (N×4 applied wrench), ref, f (per-motor) — what you plot & grade.
  • Integrator is fixed-step RK4 at p.dt (200 Hz); optional wind(t) adds a disturbance accel.

Worked example · examples/04_analysis.py

The whole Week-4 analysis in code

from quadsim import QuadParams, analysis as an
import numpy as np

p = QuadParams()

# 1) Linearize the real nonlinear model about hover
A, B = an.linearize(p)             # A: 12x12,  B: 12x4

# 2) Open-loop poles — count how many sit at the origin
ol = an.poles(A)
n_origin = np.sum(np.abs(ol.real) < 1e-6)

# 3) Controllability rank (must be 12)
_, rank = an.controllability(A, B)

print(f"{n_origin} poles at the origin (marginally stable) "
      f"· controllability rank {rank}/12")

Output: several poles at the origin, controllability rank 12/12. This same \( (A,B) \) is what Wk 7 hands to an.lqr(A,B,Q,R) and Wk 11–12 to the MPC.

Second half · hands-on

Now you build it

  • Use analysis.linearize to get \( (A,B) \) about hover.
  • Plot the open-loop poles with analysis.poles + plotting.plot_poles.
  • Confirm controllability rank = 12 with analysis.controllability.
  • Deliverable: the pole map ⭐ + the printed rank.

Open the Week 4 lab sheet →

Wrap-up

What to remember

  • Hover trim: level, at rest, \( T=mg \), zero torque ⇒ \( f(\mathbf x_{eq},\mathbf u_{eq})=\mathbf 0 \).
  • Linearize the real \(f\) by finite differences ⇒ \(A\) (12×12), \(B\) (12×4).
  • Open-loop poles sit at the origin — integrators, not asymptotically stable ⇒ feedback needed.
  • The pair is controllable (rank 12) ⇒ LQR/MPC will work.
  • Next week: close the first loop — attitude control — to drag those poles left.

References: CONVENTIONS.md (z-up, ZYX, X-frame mixer) · MIT 16.323 (How), optimal control notes. Deliverable & deadline on the lab sheet.