Academic Context & Authorship
This research was conducted as part of the Seminar Smart Systems 2026: Drone & Co. at the Technical University of Applied Sciences Würzburg-Schweinfurt (THWS), Faculty of Computer Science and Business Information Systems (FIW).
Within the collaborative group paper, my colleague Steffen Krutzsch and I co-authored Section II: Kalman Filter. The work examines the theoretical mathematical foundations of recursive Bayesian estimation, investigates non-linear extensions for robotic flight dynamics, and evaluates the real-world sensor fusion performance on the Bitcraze Crazyflie 2.1 Brushless nano-quadrotor.
The Core Problem: Flying in Noise
Autonomous flight demands continuous, low-latency knowledge of a drone’s true state: position (p), velocity (v), and attitude (roll φ, pitch θ, yaw ψ). However, miniature onboard sensors are fundamentally imperfect:
- Gyroscopes provide high-rate angular velocity but suffer from continuous integration drift. Even a minor sensor bias of 0.02°/s accumulates to 12° of orientation error after just ten minutes.
- Accelerometers measure the downward gravity vector directly without drift, but are immediately corrupted during aggressive flight by dynamic translational accelerations.
- Optical Flow & ToF Sensors provide ground-relative velocities and distance, yet produce high-frequency noise spikes, depend heavily on surface texture/lighting, and scale non-linearly with flight altitude.
Simply trusting raw sensor readings leads to catastrophic loss of control. The Kalman filter solves this by treating both the physics-based process model and incoming sensor observations as Gaussian distributions with mean and covariance, fusing them into a statistically optimal state estimate whose uncertainty is strictly smaller than either source alone.
Mathematical Formulation: Predict & Update
The filter operates as a recursive two-phase cycle:
1. The Prediction Step (Propagating Forward in Time)
Given the previous state estimate and error covariance, the system model projects the state and uncertainty forward:
- State Prediction:
x̂ₖ|ₖ₋₁ = F · x̂ₖ₋₁|ₖ₋₁ + B · uₖ - Covariance Prediction:
Pₖ|ₖ₋₁ = F · Pₖ₋₁|ₖ₋₁ · Fᵀ + Q
The quadratic transformation F P Fᵀ ensures the covariance matrix remains symmetric and positive semi-definite, while the additive process noise matrix Q models unmodeled physical disturbances such as motor imbalances and aerodynamic gusts.
2. The Measurement Update (Fusing Observations)
When sensor observation zₖ arrives, the filter evaluates the innovation ỹₖ (the genuinely new information):
- Innovation:
ỹₖ = zₖ - H · x̂ₖ|ₖ₋₁ - Optimal Kalman Gain:
Kₖ = Pₖ|ₖ₋₁ · Hᵀ · (H · Pₖ|ₖ₋₁ · Hᵀ + R)⁻¹ - Posterior State Update:
x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ · ỹₖ - Posterior Covariance Update:
Pₖ|ₖ = (I - Kₖ · H) · Pₖ|ₖ₋₁
In the paper, we work through a concrete numerical 1D height-estimation example for a hovering Crazyflie equipped with a Time-of-Flight sensor (R = 0.01 m², Δt = 0.1 s), demonstrating mathematically how the Kalman gain balances trust (K_z = 0.804) and collapses the height variance from 0.0411 m² down to 0.0081 m².
📈 Real-World Flight Benchmark
To validate the filter’s real-time performance, we logged optical-flow measurements during structured hover flights on the Crazyflie 2.1 Brushless equipped with a Flow Deck v2.
The flight benchmark consisted of distinct translation maneuvers:
- First Phase (0–20s): Three deliberate forward-backward maneuvers along the body x-axis.
- Second Phase (20–40s): Three lateral left-right maneuvers along the body y-axis.
Key Experimental Insights:
- Severe Sensor Noise Rejection: While raw optical-flow readings (
kalman_pred.measNX/Y) exhibited dramatic transient spikes of up to ±4 px/frame during motor accelerations, the EKF prior state prediction (kalman_pred.predNX/Y) tracked the true physical motion smoothly without latency lag. - Axis Decoupling: During pure x-axis translation, orthogonal y-axis estimates remained virtually zero, validating that the firmware’s kinematic model cleanly decouples lateral and longitudinal states.
📑 Full Paper Access
The complete 40-page group paper encompasses all five core domains of autonomous drone systems: SLAM, Kalman Filtering, Spatial Orientation & Quaternions, Cascaded PID Control & Motor Actuation, and Swarm Intelligence.
You can read and download the entire publication directly via the button above or the link below:
- Download Full Paper (PDF): Seminar Smart Systems 2026: Drone & Co.