Distributed UAV Cluster Estimation & Control

The problem

What happens when the map goes dark

A formation of drones is only as good as each agent's idea of where it is. Lose GNSS — under a bridge, in a canyon, indoors, jammed — and a dead-reckoning filter drifts within seconds. In a tight formation that drift is not merely an inaccuracy: it is a collision.

This project builds the distributed alternative. Every agent keeps its own moving-horizon estimator and its own predictive controller; agents exchange estimates and relative measurements with their graph neighbours, and nothing is centralised. When an agent loses its absolute fix, it fuses relative measurements to neighbours that still have one, and stays localised to centimetres while the formation keeps flying and keeps its separation.

The mission below is the one the replay shows, and it is the one every figure on this page comes from: six hexacopters take off, assemble a ring, translate as a body, and then contract and rotate the formation — while three of the six are GNSS-denied for ten seconds in the middle of it.

The 30-second reference mission. The blackout window deliberately overlaps the start of the reconfiguration, so the estimator is tested while the formation geometry is changing underneath it.

Architecture

The loop each agent closes

Estimation, control and safety are separate blocks with separate jobs, and the coupling to the rest of the cluster enters at exactly three points.

One agent, one sample. The estimator fuses its own sensors with neighbour information; the controller tracks a formation slot agreed by consensus; the barrier filter is the last thing the command passes before the plant, so separation is enforced on the input that is actually applied, not on the one that was planned. Everything shown here runs on every agent, at 20 Hz, with no central node.

Software

Three layers, one interface

The numerical core is C, the coordination logic is C++, and the research layer — models, missions, campaigns, figures — is Python. The layers meet at a small allocation-free C interface: set the reference, push constraint rows, prepare, feed back.

That boundary is load-bearing rather than decorative. The coordination layer never sees how a solver is built, so the repository ships a dependency-free reference implementation behind the same interface and the whole stack runs standalone.

The two cores are interchangeable from the layer above. Every distributed result on this page was produced with the reference core, and reproduced with the compiled real-time core substituted in.

Vehicle models

Two airframes, derived from first principles

Both models are derived in the report from Newton–Euler mechanics with every frame, relation and Jacobian stated explicitly — and both are checked numerically before any control result is claimed.

13 states · quaternion attitude

Hexacopter

Attitude is carried as a unit quaternion rather than Euler angles, so the model has no gimbal singularity anywhere in the flight envelope:

q̇ = ½ · q ⊗ [0, ω] ‖q‖ = 1 preserved exactly

Six rotors drive a four-dimensional wrench, so allocation is over-actuated: the mixer M ∈ ℝ^4×6 has rank 4 and a two-dimensional null space, which is what gives the airframe its single-rotor-failure margin. Thrust and reaction torque follow the standard k_fΩ² / k_mΩ² laws with alternating spin directions.

Control runs as differential flatness into a geometric SO(3) attitude loop, so the predictive layer plans in position while attitude tracking stays on the manifold.

6-DOF · Beard–McLain · Aerosonde

Fixed-wing

The full rigid-body aerodynamic model: wind triangle, sigmoid-blended stall aerodynamics so lift is valid past the linear region, complete force and moment coefficient expansions, and product-of-inertia rotational dynamics in Γ form.

C_L(α) = (1−σ)·C_L0+C_Lαα + σ·2·sign(α)sin²α cos α

Straight-and-level trim is solved numerically rather than assumed — at V_a = 35 m/s it lands at α = 0.20°, δ_e = −2.83°, δ_t = 0.464 — and a cascaded autopilot with look-ahead guidance flies the coordinated-turn echelon loiter.

Four aircraft hold the loiter at 120 m with 35.5 m mean spacing; ten different initial-condition seeds converge to the same limit cycle.

Validated, not asserted. validate_models.py checks quaternion group properties and the exponential map, mixer rank and allocation round-trips, hover equilibrium, rigid-body energy consistency, the wind triangle, stall blending and the computed trim — and prints ALL CHECKS PASSED. Everything downstream rests on that.

Distributed estimation

Anchor coverage

The campaign was built to find where distributed estimation breaks. It does not degrade smoothly with how many agents lose GNSS — it falls off a cliff, and the cliff is a property of the communication graph.

A GNSS-denied agent can stay localised only if it can see someone who is not lost. Fusing a relative measurement to a neighbour transfers that neighbour's confidence — weighted by its covariance, so a shaky anchor contributes little — but a ring of agents who have all lost their fix has nothing to transfer between them, and the whole cluster drifts together.

Green holds GNSS · amber is denied but anchored · red is denied with nothing to anchor to. Cyan arrows are relative-measurement fusion, pointing at the agent whose confidence is being borrowed; faint lines are the communication graph. Left, the nominal mission — each denied agent borders a localised neighbour, over the ring or a two-hop chord. Middle, the failure the sweep exposed: on a plain ring with five of six denied, only the survivor's two ring neighbours can anchor at all, and the three agents beyond them drift as a body until separation is breached. Right, the repair: each agent takes an anchor level — the survivor is ℓ0, its neighbours ℓ1, theirs ℓ2 — and may fuse only toward a strictly lower level. That makes the anchor relation a DAG, so the fix propagates outward hop by hop and no agent ever re-absorbs its own estimate through a loop.

Why covariance-weighting matters

A relative measurement y = p_i − p_j is only as good as the neighbour's own position. Fusing it as though p̂_j were truth injects the neighbour's error straight into your estimate; the rule used here inflates the measurement covariance by the neighbour's reported covariance, R_rel + P_j, so an anchor that is itself uncertain is discounted automatically.

Two further rules keep the fusion honest: it triggers only on loss of the absolute fix, so a healthy agent never degrades itself with stale neighbour information, and it runs in two passes per sample, so every agent anchors to same-step estimates rather than to yesterday's.

What the estimator actually is

Each agent runs a constrained moving-horizon estimator over a window of past measurements, with an arrival cost carrying everything older than the window. On the linear translational sub-problem it reduces exactly to an information-form Kalman filter, which is what the reference implementation here uses — a property the report derives rather than assumes.

That reduction is the reason the distributed layer can be studied honestly with the reference core: the coupling, the anchoring policy and the covariance arithmetic are identical whichever core sits underneath.

Results

210 runs, zero divergences

Single missions prove nothing. The claims below come from a Monte-Carlo campaign over noise realisations, denial severities, degraded graphs, estimator ablations and solver substitutions — all reproducible from mc_results.json in the repository.

Blackout: estimate vs. dead reckoning

Mean position error of the three denied agents, reference mission (seed 7)

DMHE dead reckoning GNSS denied

Denial sweep & the coverage cliff

Denied-agent RMSE against number of agents denied, 12 seeds each

DMHE, one-hop multi-hop anchoring p95

Compiled solvers in the closed loop

15 paired seeds per configuration — identical noise streams

denied-agent RMSE [m] min separation [m]

Per-agent solver cycle

Preparation time, single core, median of best-of-seven

rebuilt each sample warm-started

The estimator earns its place. Ablating it — flying the same missions with a per-agent filter and no relative fusion — leaves estimation looking deceptively fine (0.089 m, because the non-denied agents dominate the average) while formation keeping degrades by 4.8× and minimum separation falls to 0.75 m, well inside the 1.40 m barrier. The failure appears in the geometry, not in the error statistic.

Every scenario

Scenario	runs	RMSE mean	median	p95	shape err	min sep	breaches
Nominal — 3 of 6 denied	30	0.091	0.091	0.099	0.094	1.55	0
1 of 6 denied	12	0.068	0.068	0.074	0.103	1.49	0
2 of 6 denied	12	0.078	0.078	0.085	0.095	1.54	0
4 of 6 denied	12	0.106	0.105	0.117	0.087	1.54	0
5 of 6 — coverage broken	12	0.451	0.365	0.689	0.218	0.90	yes
5 of 6 — multi-hop	12	0.119	0.120	0.127	0.068	1.49	0
Ring graph — coverage broken	12	0.506	0.474	0.799	0.231	0.98	yes
Ring graph — multi-hop	12	0.111	0.112	0.119	0.098	1.49	0
Communication edge dropped	12	0.094	0.092	0.107	0.095	1.50	0
Distributed estimator ablated	12	0.089	0.087	0.099	0.420	0.75	yes
Reference core in the loop	15	0.091	0.090	0.100	0.093	1.55	0
Compiled estimator + controller	15	0.062	0.062	0.070	0.096	1.53	0

Errors in metres. “Shape error” is formation geometry error with the cluster centroid removed; “breaches” counts samples below the 1.40 m barrier radius. Divergence and solver-failure counts are zero in every row of the campaign.

Interactive

SwarmScope

The same mission, in 3D, replayed from the logged data — not an animation of it.

Every position, attitude quaternion, estimate and metric in the replay is read straight from the simulation output. Watch the blackout hit at T+12 s, the dead-reckoning ghosts peel away from the aircraft, the fusion beams light up to GNSS-good neighbours, and the formation contract and rotate with the closest-pair separation called out against the barrier radius.

Cinematic, orbit, top-down and per-agent chase cameras
Live telemetry charts with a scrubbable mission timeline
A second scene: the four-aircraft fixed-wing echelon loiter

▶ Open the replay

SwarmScope replay showing six hexacopters in formation with telemetry panels and a mission scrubber.

The code

Run it yourself

Clone, install NumPy, and every result on this page reproduces on your machine.

# clone and validate the models
git clone https://github.com/anilram30/uav-cluster-dmpc-dmhe
cd uav-cluster-dmpc-dmhe/python
pip install -r requirements.txt

python3 validate_models.py   # -> ALL CHECKS PASSED
python3 sim_hexacluster.py   # the reference mission
python3 sim_fixedwing.py     # echelon loiter
python3 make_figures.py      # regenerate figures/

# the scenarios behind the results table
from sim_hexacluster import run

run(denied=(1,2,3,4,5), topology='ring')
run(denied=(1,2,3,4,5), topology='ring', multihop=True)
run(ctrl_est='ekf')        # ablate the estimator
run(edge_drop=(0,1))      # degrade the graph

c_api/ C numerical core uav_solver.h the interface — MPC + MHE uav_solver_ref.c reference implementation cpp/ C++ coordination layer agent.hpp fusion · consensus · CBF cluster_demo.cpp end-to-end smoke test python/ research layer uavdmpc/ models, control, estimation sim_hexacluster.py sim_fixedwing.py mc_study.py 126-run campaign mc_study2.py 84-run campaign make_figures.py figures/ generated figures docs/ this site + SwarmScope report/ technical report (PDF)

On the solvers. The per-agent real-time-iteration NMPC controller and MHE estimator used for the in-the-loop and timing results are my own separate research work — an extension of my MSc thesis on SLQP-MPC, with the estimator being the moving-horizon counterpart of the same solver. That work is ongoing and not yet published, so its algorithms and sources are not part of this repository, and the report here is a public edition that specifies the solvers by the problems they solve, the real-time pattern they run in, the interface they are reached through and their measured timing. The distributed framework depends on none of it: the reference implementations shipped here satisfy the same interface, and everything reproduces standalone.