GPS-denied motion-planning prototype

A drone that loses GPS loses the one thing most navigation stacks quietly assume, which is knowing where it is. Everything else has to be rebuilt from what the vehicle can sense for itself. It has to construct a map of a space nobody surveyed, work out its own position inside that map, and plan a route through it while the map is still being discovered. This is a write-up of that stack, built from the bottom up, with the source at github.com/gradientsj/aerial-autonomy-lab.

Every number on this page is a measurement produced by the code in that repository. Where a result is unflattering, it is reported as it came out.

The system at a glance

Sensing feeds a map, the map feeds a planner, the planner feeds a controller, and the controller's actions change what the sensors see. This is the target loop; the matrix below separates the implemented planning slice from the unbuilt flight stack.

What the map has to be

The obvious representation of a discovered environment is an occupancy grid, where each voxel is free or occupied. It is also the wrong one for a planner. A planner holding only occupancy has to check an edge by sampling it at a spacing fine enough for the thinnest obstacle in the map, which wastes enormous effort in open space and is still not sound near a thin wall.

A Euclidean signed distance field stores, at every point, the distance to the nearest obstacle, negative inside obstacles and positive outside. That single change turns collision checking from blind sampling into something adaptive, because the value at a point is exactly the radius of a ball certified to be free.

Why this choice: the field also hands the trajectory optimiser a smooth gradient to push against, which occupancy cannot do, and it makes clearance a first-class quantity rather than something inferred.

Building it exactly is a solved problem that is often solved badly. The distance transform computes

where the seed function is zero on obstacles and large everywhere else. Each sample contributes a parabola, and the transform is the lower envelope of all of them. Felzenszwalb and Huttenlocher's algorithm sweeps that envelope as a stack, pushing and popping each parabola at most once, which makes it linear in the number of voxels. Because squared Euclidean distance separates as a sum over axes, running the one-dimensional transform along x, then y, then z is exact in three dimensions. Chamfer masks and two-pass approximations are common and they are all wrong by a few percent, which is precisely the size of the safety margin you are trying to reason about.

Sphere tracing, and a bug worth keeping

With a distance field the edge check becomes a sphere trace. Stand at a point, read the clearance, and step forward by that clearance, because nothing can possibly be hit within it. Repeat. In open space this takes a handful of enormous strides. Near geometry it automatically slows down.

The soundness argument rests on the distance function being 1-Lipschitz, meaning it cannot change faster than the distance you travel.

That is true of the exact Euclidean distance function. It is not true of the thing the code actually queries. The code queries a trilinear interpolant of a voxel grid. Adjacent voxels differ by at most one voxel width, so each partial derivative of the interpolant is bounded by one, which bounds the gradient norm by the square root of three along a diagonal, not by one.

Striding by the full clearance therefore steps over violations. The planner still produced beautiful paths. It simply clipped an obstacle roughly once in a thousand edges, which no demonstration would ever reveal. A soundness test comparing the sphere trace against dense sampling over four thousand random edges found it immediately, and the fix is to divide every stride by the square root of three.

src/collision/esdf.cppconst Scalar kLipschitz = 1.7320508075688772;  // sqrt(3)

Scalar t = 0;
for (int step_count = 0; t <= L; ++step_count) {
  if (step_count >= kMaxSteps) return t;   // grazing, treat as blocked
  const Vec3 p = a + dir * t;
  const Scalar slack = field_.distance(p) - clear;
  if (slack < min_slack) return t;         // blocked here
  t += slack / kLipschitz;
}
return L;

A second test caught a subtler consequence. Tracing an edge forward and tracing it backward visit different sample points, so the two directions could disagree about an edge that grazes the boundary. The planner treats edges as undirected, so the checker now canonicalises the endpoint order and is a true function of the unordered pair.

What asymptotic optimality actually requires

RRT* is famous for a guarantee that is usually quoted and rarely stated precisely. Let the optimal cost be the best achievable and let the tree's best cost after n samples be what the planner has found. The guarantee is that the second converges to the first with probability one.

That guarantee lives or dies on the connection radius. Rewiring only improves things if the radius is large enough to chain nearby samples into a path, and shrinking it too fast breaks the proof silently, leaving a planner that looks fine and converges to the wrong answer.

Here the volume term is the measure of free space and the other is the volume of the unit ball in d dimensions. The logarithm is not decoration. It is the connectivity threshold for random geometric graphs, and below it the graph disconnects with high probability, so the expected number of neighbours has to grow like the logarithm of the tree size.

There is a detail here that an interviewer enjoys. The formula needs the volume of free space, which in an unknown environment is by definition not known. Underestimating it shrinks the radius below the threshold and quietly forfeits the guarantee. The planner estimates it from the free-voxel count of the map it has built so far and keeps the bounding-box volume as a floor, because the proof requires the radius to be at least the threshold and is perfectly happy if it is larger.

Why the planner works in three dimensions and not twelve

A quadrotor has a twelve-dimensional state. The tempting move is to plan directly in it, so that the plan respects the dynamics by construction. The exponent in the radius formula is what makes that a mistake. Evaluating the shrink factor at a hundred thousand samples gives

At nine dimensions the radius has shrunk by a factor of under three after a hundred thousand samples, so the near set is effectively the whole tree and every iteration costs a full sweep. Asymptotic optimality is still true in the limit and completely unreachable in any budget that runs on real hardware.

The way out is a property of the vehicle rather than of the planner. A quadrotor is differentially flat in the four outputs of position and yaw, which means every state and every rotor input can be recovered as an algebraic function of those outputs and their derivatives. Thrust comes from the acceleration, attitude from the direction of the thrust vector, body rates from the jerk, and rotor speeds from the snap. Planning a smooth path in three dimensions and recovering the dynamics through flatness is therefore exact, not an approximation, and it keeps the search in the dimension where the radius formula still behaves.

Informed sampling

Once any solution of cost c exists, a state can only improve it if the detour through that state is no longer than c. That condition describes an ellipse with the start and goal at its foci.

Every sample drawn outside it is provably wasted. Sampling the ellipse by rejection is correct and degenerates badly, because the acceptance rate falls toward zero exactly when the ellipse gets interesting. Sampling it directly by transforming a uniform draw from the unit ball costs the same at any tightness, which is what the third visualisation above is showing.

Results

Forty seeds per cell, four thousand iterations, a 0.35 m vehicle radius, in a 40 by 40 by 12 m world. Cost is path length in metres with a 95 percent confidence interval. The final column is the paired difference, where both planners see the same seed, which removes the run-to-run variance that otherwise swamps the effect.

Rewiring beats plain RRT everywhere and the effect is far larger than its uncertainty. Informed sampling is a clear win on the wall map, where the forced detour makes the ellipse collapse tightly around one corridor.

On the dense forest it buys nothing at all. The two means differ by 0.01 m, which is a dead heat, and informed sampling spends 26 percent more planning time to achieve it, 62.4 ms against 49.5 ms. The reason is visible in the geometry. When the first solution found is already close to the straight-line lower bound, the ellipse stays nearly as large as the free space, so restricting sampling to it changes almost nothing while still paying for the transform on every draw. Reporting that is more useful than hiding it, and it is the kind of result that only shows up if the benchmark is built before the conclusion.

The hardware shaped the architecture

The development machine is a headless node with two H100 GPUs, and probing it produced a genuinely useful constraint. Hopper has no ray tracing cores and no display engine, and the machine carries no OpenGL, Vulkan or EGL userspace at all. Isaac Sim, Unreal and Unity camera output are all unavailable, and containers do not rescue it because the container runtime can only inject compute libraries.

This matters less than it sounds. Depth and range sensing are raycasting problems, not rasterisation problems. A hand-written CUDA sphere tracer renders a 1920 by 1080 depth image in 0.025 ms on one H100, which is roughly forty thousand frames per second, and thousands of parallel environments share one kernel launch. The photorealism a game engine would have provided is worth very little to a planner that consumes geometry, and the thing worth visualising is the algorithm, which belongs in a browser anyway.

Rendering is not absent from the project, it is relocated. The workstation tier is an RTX 4090 and an RTX 5090, and both of those carry the ray tracing cores, display engine and video encoder that Hopper lacks. Photorealistic imagery is produced there, where it is cheap, and it feeds the one part of the system that genuinely wants pixels rather than geometry, which is the perception front end. Learned depth and sim-to-real domain randomisation need images. The planner does not.

Why this choice: each kind of compute goes where it is cheapest, and the training loop stays free of any engine. Putting a renderer in the inner loop would cost determinism and throughput, and would tie every published number to one specific build of somebody else's simulator. The engine is a data source and a cross-check rather than the simulator itself.

Testing

A randomised planner reported from a single run is not a measurement, so the suite tests properties rather than outputs wherever it can.

• Exactness: the distance field equals a brute-force transform on every voxel.
• Soundness: the sphere trace never certifies an edge that dense sampling finds in collision.
• Convergence: in an empty map the optimum is the straight line, so the optimality gap is measured against a true reference rather than against the planner itself.
• Reproducibility: a fixed seed gives bitwise identical paths, costs, node counts and query counts.
• Significance: the claim that rewiring helps is a paired t-test over thirty seeds.

Reproducibility took more care than expected. The standard library's uniform and normal distributions are specified by their distribution and not by their bit sequence, so identical seeds produce different draws under libstdc++ and MSVC. Golden trajectory fixtures would pass on Linux and fail on Windows for no real reason. Every random number here is generated from raw Mersenne Twister bits, whose sequence the standard does pin down exactly. The build also sets the flag that stops the compiler fusing a multiply and an add on its own initiative, since that changes the low bits of every distance computation.

That is necessary and it is not sufficient, which is worth stating rather than glossing. Uniform draws are bit-portable because they use only integer operations and one multiply. Normal draws are not, because they need a logarithm, and neither the GNU nor the Microsoft implementation of it is correctly rounded, so the two disagree in the last bits. The same applies to the connection radius, which needs a logarithm and a cube root. Reproducibility here is therefore bitwise within a platform and tolerance-based across platforms, and the fixtures are written to match that rather than to pretend otherwise.

Limitations

The planner is real and the map is real. The state estimator is not yet, and the current pipeline runs on odometry with injected drift rather than a working visual-inertial filter, which is the defining hard problem of GPS-denied flight. The numbers above are therefore about planning quality in a known map and say nothing about behaviour under estimation error. The dynamics and the SE(3) controller are designed but not yet wired into the closed loop, so path length is a proxy for cost rather than time or energy. Replanning against a map that is still being discovered is the next milestone, and it is where the interesting failures will be.

Links

• Source on GitHub
• Motion planning, A* and RRT, the background notes this project builds on.
• SLAM, mapping while localizing, the companion problem to the map built here.
• State estimation with Kalman filters, the prerequisite for the estimator still to come.
• Karaman and Frazzoli, Sampling-based Algorithms for Optimal Motion Planning (2011), the source of the radius formula.
• Gammell, Srinivasa and Barfoot, Informed RRT* (2014).
• Mellinger and Kumar, Minimum snap trajectory generation and control for quadrotors (2011), the differential flatness result.
• Felzenszwalb and Huttenlocher, Distance Transforms of Sampled Functions (2012).