Wk 6–7: outer position loop & the full cascade autopilot.
Every one of those loops read the true state \( \mathbf{x} \) from the simulator.
Today: remove the cheat. Build the estimate \( \hat{\mathbf{x}} \) and close the loop on it.
The reality: a real autopilot gets a noisy IMU at ~200 Hz and a
noisy GPS at ~5 Hz. No sensor gives you \( \mathbf{x} \) directly. Estimation is the bridge.
Learning objectives
By the end of today you can…
Explain why a controller sees a noisy IMU (high rate) + low-rate GPS, not the true state.
Build a complementary filter for attitude: trust the gyro fast, the accelerometer slow.
Build a Kalman filter for position/velocity from IMU world-accel + GPS updates.
Understand why we close the loop on the estimate \( \hat{\mathbf{x}} \), and why a 15-state EKF is the production version.
Lab Wrap CascadePID in EstimatedStateController and fly on \( \hat{\mathbf{x}} \) under noise.
The problem
What the sensors actually give you
Gyro — body rates \( \boldsymbol\omega \), fast & clean short-term, but integrating it drifts.
Accelerometer — specific force \( \mathbf{f}=R^\top(\mathbf{a}_w-\mathbf{g}) \). At rest it points along gravity → an absolute tilt reference, but any maneuver acceleration corrupts it.
GPS — absolute world position, no drift, but slow and noisy (~5 Hz).
In quadsim/sensors.py the IMU returns {"gyro": ω + bias + noise, "acc": f_body + noise}
and GPS.read returns a fix only every \(1/\text{rate\_hz}\) seconds, else None.
Attitude fusion
Complementary filter: fast gyro + slow accel
Predict by integrating the gyro through the Euler kinematics, then nudge roll/pitch toward the accelerometer's gravity direction:
Pick \( \alpha=0.999 \) at 200 Hz \( \Rightarrow \tau=5\,\)s. A low \( \alpha \) lets the vehicle's own
maneuver acceleration fool the tilt estimate and destabilizes the loop. Yaw: gyro only (no magnetometer here).
Position fusion
Position KF: predict on IMU, correct on GPS
State \( \mathbf{x}=[\mathbf p,\mathbf v] \). Reconstruct world acceleration from the IMU and the estimated attitude (z-up):
Update on a GPS fix \( \mathbf z \), \( H=[\,I\ \ 0\,] \):
\[ K=P^-H^\top S^{-1},\ \ \hat{\mathbf x}=\hat{\mathbf x}^- + K(\mathbf z-H\hat{\mathbf x}^-) \]
This is the loosely-coupled INS/GPS estimator (quadsim/estimators.py): predict every IMU step, update only when a GPS fix arrives.
Architecture
Close the loop on the estimate
truth x ─►┌──────────┐ noisy IMU+GPS ┌───────────────┐ x̂ ┌────────────┐ u ┌──────────┐
(sim) │ sensors ├──────────────►│ INSGPS ├────►│ CascadePID ├────►│ plant ├─► x
└──────────┘ gyro,acc,gps │ comp.filter+KF│ │ (on x̂!) │ └────┬─────┘
└───────────────┘ └────────────┘ │
▲ last applied wrench u ───────────────┘
EstimatedStateController wraps any base controller: read sensors → fuse with INSGPS → call base.control(t, x̂, ref).
The estimate output never peeks at the truth — apart from a one-time launch-pose init (you know where you took off).
In the simulator
Worked: fly a step on the estimate
from quadsim import Simulator, QuadParams
from quadsim.controllers import CascadePID
from quadsim.estimators import EstimatedStateController
from quadsim.dynamics import hover_state
from quadsim import trajectories as traj
p = QuadParams(); sim = Simulator(p)
ctrl = EstimatedStateController(CascadePID(p), p) # fly on x̂, not x
target = [1.0, -1.0, 1.5]
log = sim.run(hover_state(position=(0, 0, 1.0)), ctrl,
reference=traj.step(position=target, start=(0, 0, 1.0), t_step=0.5),
t_final=10.0)
# ctrl.est_log holds (t, x_hat) for plotting estimation error
Same cascade as Week 6–7 — the only change is the wrapper. It still reaches the target, now under sensor noise.
The production version
From teaching cascade to a full EKF
Our split (complementary filter + linear KF) is the readable version: every Kalman equation is exposed.
Production autopilots run one monolithic 15-state EKF (position, velocity, attitude, gyro & accel biases) — e.g. PX4 EKF2.
The E in EKF: linearize the nonlinear \( f,h \) about the current estimate each step (Jacobians \( F,H \)).
Why bother with the cascade first? You can see predict/update, the gyro/accel handover,
and the GPS correction separately. The EKF folds them into one covariance — powerful, but a black box until you've built this.
Second half · hands-on
Now you fly on the estimate
Wrap CascadePID in EstimatedStateController and fly under sensor noise.
Plot estimation error vs ground truth with plotting.plot_estimation_error.
Run python examples/07_estimation.py.
Checkpoint: stable flight on the estimate, final err < 0.2 m, plus the error plot. ⭐ graded.