State estimation — fusing IMU and optical flow

The state estimator runs an extended Kalman filter that fuses accelerometer and gyroscope data with optical flow measurements from a downward-facing camera. The IMU runs at a high rate and provides the prediction step; the optical flow arrives less frequently and corrects drift in the horizontal velocity estimate.

The state vector includes position, velocity, and attitude (as a quaternion). The filter is tuned conservatively — it trusts the IMU for short timescales and lets the visual data correct longer-term drift. In practice this means the hover is stable for seconds at a time even when the flow sensor drops a frame or two.

One issue that surfaced during tuning: the optical flow sensor’s reported velocity has a bias that depends on the surface texture below the drone. Flying over a uniform surface degrades the estimate noticeably. The filter handles this gracefully by increasing uncertainty, but the position estimate does drift on featureless floors.