State Estimation & Sensor Fusion — fly on the estimate

1 · The 5-minute recap

Real flight controllers never see the true state \( \mathbf{x} \). They get a high-rate, noisy IMU (gyro + accelerometer, ~200 Hz) and a low-rate, noisy GPS (~5 Hz), and must fuse them.

Complementary filter (attitude). Integrate the gyro through the Euler kinematics, then nudge roll/pitch toward the accelerometer's gravity direction:

\[ \hat\phi = \alpha\,\phi_{\text{gyro}} + (1-\alpha)\,\phi_{\text{acc}},\qquad \tau=\frac{\Delta t}{1-\alpha} \]

Trust the gyro fast, the accelerometer slow. Use \( \alpha=0.999 \) at 200 Hz (\( \tau=5 \) s). A low \( \alpha \) lets the vehicle's own maneuver acceleration fool the tilt estimate and destabilizes the loop — you will test this.

Position Kalman filter. Predict with world acceleration rebuilt from the IMU and estimated attitude (z-up, gravity \( -z \)), and correct when a GPS fix arrives:

\[ \mathbf{a}_w = R\,\mathbf{a}_{\text{body}} + \begin{bmatrix}0\\0\\-g\end{bmatrix},\qquad \hat{\mathbf{x}} = \hat{\mathbf{x}}^- + K\,(\mathbf{z}_{\text{gps}} - H\hat{\mathbf{x}}^-) \]

EstimatedStateController closes the loop on this estimate. A monolithic 15-state EKF (PX4 EKF2) is the production version; the cascade in quadsim/estimators.py is the readable one.

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=.

The estimators already live in quadsim/estimators.py (ComplementaryFilter, PositionKF, INSGPS, EstimatedStateController) and the sensor models in quadsim/sensors.py. This week you wire them up and fly on the estimate — read them first.

3 · Fly on the estimate

Work through the steps. You build a short script that closes the loop on \( \hat{\mathbf{x}} \) and measures the estimation error. Recall the 12-state layout: x[0:3] position, x[3:6] velocity, x[6:9] Euler \((\phi,\theta,\psi)\), x[9:12] body rates.

R = rotation_matrix(euler[0], euler[1], euler[2])
a_world = R @ imu["acc"] + np.array([0.0, 0.0, -self.p.g])   # z-up: gravity is -z

from quadsim.controllers import CascadePID
from quadsim.estimators import EstimatedStateController

ctrl = EstimatedStateController(CascadePID(p), p)   # fly on x̂, not x

from quadsim import trajectories as traj
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)

import numpy as np
est  = np.array([xh for _, xh in ctrl.est_log])
true = log.x[:len(est)]
t    = log.t[:len(est)]

from quadsim import plotting as viz
viz.plot_estimation_error(t, true, est, save="estimation_error.png", show=False)

from quadsim.estimators import INSGPS
bad = EstimatedStateController(CascadePID(p), p, estimator=INSGPS(p=p, alpha=0.9))
# re-run and compare the attitude-error curve to alpha=0.999

# examples/wk8_estimation.py — fly on the estimate under sensor noise
import numpy as np
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)

est  = np.array([xh for _, xh in ctrl.est_log])
true = log.x[:len(est)]
t    = log.t[:len(est)]
print(f"final position error: {np.linalg.norm(log.position[-1] - target):.3f} m")

from quadsim import plotting as viz
viz.plot_estimation_error(t, true, est, save="estimation_error.png", show=False)
print("saved estimation_error.png")

4 · Run & verify

You can run the shipped reference driver, or your own examples/wk8_estimation.py from the step above:

export PYTHONPATH=.
python examples/07_estimation.py            # shipped reference
python examples/07_estimation.py --show     # pop the plot window

Self-check

• The quad reaches the target on the estimate with final error < 0.2 m.
• With alpha=0.9 I saw the attitude error grow — the accelerometer was fooled by maneuver acceleration.
• I can explain why the position KF only updates at the slow GPS rate but predicts every IMU step.
• I understand we close the loop on \( \hat{\mathbf{x}} \), and that a 15-state EKF (PX4 EKF2) is the production version.

5 · Going further (optional)

• Alpha sweep. Re-run with \( \alpha \in \{0.9, 0.99, 0.999, 0.9995\} \); plot attitude RMSE vs \( \alpha \) and locate where the loop turns unstable.
• Starve the GPS. Lower GPS(rate_hz=...) to 1 Hz and watch the position error grow between fixes — pure dead-reckoning drift.
• Bias the IMU. Set a nonzero gyro_bias in the IMU and observe the steady attitude error — the motivation for a bias-estimating 15-state EKF.

6 · Deliverable & submission

Reading: Beard & McLain UAVbook (estimation) · rlabbe Kalman-and-Bayesian-Filters-in-Python. Next week → the intelligent-control block, now that you can trust a fused state.