PX4 Autonomy & Verification

SLAM & costmap.
The map holds when the odometry doesn't.

A drone with a 2-D LiDAR flies the obstacle course in PX4/Gazebo. Every real scan goes into a small SLAM written for this project: it places the scan using odometry, then shifts and rotates it until it lines up with the map built so far, and only then adds it to the map. To show what that buys, the odometry is deliberately degraded (3% scale error, 0.5°/s heading drift, the way dead reckoning drifts without GPS). The map feeds a layered costmap (the cost convention Nav2 uses) and a planner that trades route length for clearance.

From the recorded flight below (evidence/slam/). Ground truth is Gazebo's own pose, recorded only to score the run; SLAM never reads it. SLAM path error is measured after one rigid fit of SLAM's map frame to Gazebo's (see “What this does not show yet”).

How it works

1. Scan. Gazebo's GPU LiDAR: 720 beams over 270°, 30 m, 20 Hz. Each beam is tilted by the drone's roll and pitch (up to 15° in turns) and projected flat; returns from the ground or the drone itself are dropped.
2. Predict. The odometry step since the last scan moves the previous SLAM pose forward.
3. Match. A search over small shifts and rotations finds where the scan best lines up with the map, scored on a likelihood field around mapped surfaces: a coarse pass on a blurred field, then a fine pass, with a weak pull toward the prediction so a symmetric scene cannot make it jump.
4. Map. The scan is added at the corrected pose to a log-odds occupancy grid: cells along each beam become more likely free, the cell at its end more likely occupied.
5. Costmap. Occupied cells are lethal; within the drone's radius, inscribed; beyond that, cost decays with clearance; unseen space is unknown. Other drones can be added as a dynamic layer.
6. Plan. A* where each step costs its length × (1 + w·cost). With w = 0 it is the old binary planner and squeezes past obstacles at the minimum clearance (purple); with w = 3 it gives them room (amber).
7. Send. The map's frame is not the frame PX4 flies in, so every route is converted first. A tracker learns the map → PX4 transform from the pose pairs SLAM and PX4 report for each scan: with GPS it fits PX4's position track (immune to its heading error), without GPS it follows PX4's drifting frame.

Routes as flown

Each planned route is converted into PX4's frame and placed in the real world using Gazebo's pose (scoring only). The closest it then comes to a real obstacle, against sending the map's coordinates unconverted:

–

What this does not show yet

• The map itself stays rotated. SLAM's frame is fixed by its first pose, and PX4's heading was about 5° off Gazebo's then, so the drawn map is rotated by that much (tick the scoring-view box to see the fit used for the error figures). Routes are converted before they are flown (above), so this no longer moves where the drone goes; without GPS there is no absolute heading to correct the map against.
• No loop closure. Error that the matcher cannot see still builds up slowly; this course is small enough that it stays at centimetres.
• 2-D at one height. Anything above or below the scan plane is invisible: the course beacons are 1.3 m tall and never appear.
• Planning runs after the flight, not in it. The flight path here is fixed in advance; flying the costmap's own converted plans, replanning as the map grows, is next.

Run it yourself

With PX4 SITL and Gazebo Harmonic installed: scripts/run_slam_demo.sh flies the course and draws the costmap, map and both trails in the Gazebo window. Source: SLAM · costmap & planner · flight.