3. IMU attitude estimation
Three attitude estimates from your own phone's sensors: gyro-only, accel-only, and complementary fusion. Pick up your phone and feel why fusion exists.
Frames & why quaternions
The phone has a body frame (X right, Y up the screen, Z out of the glass) and the room has a world frame (X north, Z up). We store the attitude as a unit quaternion $q$, which rotates body vectors into world vectors:
$$\mathbf{a}_{\text{world}} = q\, \mathbf{a}_{\text{body}}\, q^{-1}$$We never use Euler angles internally — at pitch $\pm 90°$ two axes line up and one angle becomes undefined (gimbal lock). Quaternions have no such singularity.
What the sensors measure
Each sensor reports a known quantity rotated into the body frame, corrupted by a bias $b$ and noise $n$:
$$\omega_m = \underbrace{\omega_{\text{true}}}_{\text{gyro: rotation rate}} + b_g + n_g$$ $$\mathbf{a}_m = \underbrace{R(q)\,\mathbf{g}}_{\text{accel: gravity } 9.81\,\text{m/s}^2} + \mathbf{a}_{\text{linear}} + n_a$$ $$\mathbf{m}_m = \underbrace{R(q)\,\mathbf{m}_\oplus}_{\text{mag: Earth's field }\sim 50\,\mu\text{T}} + b_m + n_m$$$R(q)$ is the rotation matrix of $q$. Noise here is colored (AR(1)) plus a drifting bias — real IMUs, not clean white noise. The gyro's bias is why integrating it alone drifts forever.
Gyro integration (the motion model)
The quaternion evolves as
$$\dot{q} = \tfrac12\, q \otimes [0,\; \omega]$$integrated each step and renormalized. Fast and smooth, but any constant bias in $\omega$ integrates into an unbounded angle error — that's the gyro-only cube drifting.
Accelerometer tilt
At rest the accelerometer measures only gravity, so tilt follows by geometry:
$$\text{roll} = \operatorname{atan2}(a_y,\, a_z)\qquad \text{pitch} = \operatorname{atan2}(-a_x,\, \sqrt{a_y^2+a_z^2})$$No integration, no drift — but yaw is unobservable (spinning about gravity changes nothing the accelerometer sees), and linear acceleration contaminates the reading. The accel-only cube jitters under motion and never turns.
Magnetometer heading
The compass measures the Earth's field in the body frame. Tilt it back to horizontal using the accel roll/pitch, and the yaw follows:
$$\psi = \operatorname{atan2}(-m'_y,\, m'_x)\quad\text{with}\quad m' = \text{tilt-corrected } \mathbf{m}_m$$Bounded and drift-free, but noisy and perturbed by local metal. This is what finally gives us yaw — the whole point of adding the sensor.
Complementary filter
Blend the smooth-but-drifting gyro attitude with the jittery-but- bounded accel/mag tilt. One line, one knob:
$$q = \operatorname{slerp}\big(q_{\text{tilt}}^{(\psi)},\; q_{\text{gyro}},\; \alpha\big)$$$\alpha$ is the trust knob:
.
The sliders write it straight into the editable code's
params block.
The EKF (extended Kalman filter)
State is the attitude quaternion $q$ plus the 3×3 covariance $P$ of a small local attitude-error vector $\delta\theta$ (a body-frame rotation, not raw quaternion components — corrections are applied by composing a small rotation onto $q$, never by adding to its $[x,y,z,w]$ numbers directly, since those don't live on a flat space and an additive correction leaks into axes it shouldn't touch). Predict — integrate the gyro, and grow $P$ by the linearized error dynamics plus process noise:
$$q_k = q_{k-1} \otimes [1,\; \omega\tfrac{dt}{2}]\qquad F = I - dt\,[\omega]_\times\qquad P_k = F P_{k-1} F^{\top} + Q$$$[\omega]_\times$ is the cross-product ("skew") matrix of $\omega$, and $Q = q_{\text{scale}}\,dt\, I$ is the process noise — your trust in the gyro. .
Correct — each measurement (accel then mag) compares the predicted sensor reading $h(q)$ against the actual one:
$$y = z - h(q)\qquad H = [h(q)]_\times\qquad S = H P_k H^{\top} + R\qquad K = P_k H^{\top} S^{-1}\qquad \delta\theta = Ky\qquad q \mathrel{\otimes}= [1,\; \tfrac{\delta\theta}{2}]$$$H$ is the Jacobian of $h$ with respect to $\delta\theta$ — a small body-frame rotation by $\delta\theta$ rotates the predicted reading by $h(q) \times \delta\theta$, so $H=[h(q)]_\times$ falls straight out of that. $R$ — the per-axis measurement covariance — is your trust in the sensor. .
What Q and R do
Two numbers, two behaviors:
- High $R$ (little trust in a sensor): the filter barely corrects, so $K \to 0$ and the estimate stays smooth but can drift. Raise $R_{\text{mag}}$ and watch yaw wander again.
- High $Q$ (little trust in the gyro): $P$ grows, so $K \to H^{-1}$ and the filter snaps to the measurements — jittery, but never drifts. The covariance trace plot shows $P$ shrinking on each correction and regrowing between them.
The whole filter — predict, Jacobians, gain, update — is the editable code below. Change the math itself and the phone reacts live.