Motion depends on heading
Measured forward travel is rotated by the estimated midpoint heading θ + Δθ/2; measured heading change updates orientation. Jacobians propagate motion uncertainty and existing cross-covariances through this nonlinear step.
12 / SIMULTANEOUS LOCALIZATION AND MAPPING
Build a map.
Find yourself inside it.
The drone knows its starting pose, but the landmark positions are unknown. Extended Kalman Filter SLAM (EKF-SLAM) estimates the drone’s position and heading together with the map. Seeing a familiar landmark can correct both.
Predict: noisy odometry has rotated the estimated path. When the drone observes L1 again, should the correction move the landmark, the drone—or other landmarks too?
Name the estimator and its assumptions
Equations and primary sources ↗- Problem family and algorithm
- SLAM / joint-state Extended Kalman Filter
SLAM is the problem of simultaneous localization and mapping. This implementation uses an EKF: nonlinear prediction and observation functions are linearized around the current estimate.
- Estimated state and map
- [x, y, θ, L1ₓ, L1ᵧ, …]
Start with one known planar pose and an empty map. The first sighting of each landmark adds two coordinates and their cross-covariances. Reobservations update the existing joint state.
- Sensing and association
- Noisy odometry + range and body-frame bearing
A 360° synthetic sensor supplies identified landmarks within 5 m every second. IDs are given; coordinates are not. Correct association, synchronized packets and no processing delay are assumed.
- Architecture and fidelity
- One onboard estimator · prescribed planar path
A known starting pose fixes the world frame. Both views observe the same run. Heading is modeled; the drone’s 2 m display altitude and detailed airframe are presentation, with no flight physics or avoidance.
Keep truth separate: solid landmarks and the solid drone show evaluator truth. The estimator receives no landmark coordinates, true path or true current pose. Its belief does not steer this fixed comparison trajectory.
ONE DRONE · AN INITIALLY EMPTY MAP
Predict. Add a landmark. Observe it again.
Drag to orbit · scroll to zoom · Focus drone for a closer view. Solid objects are evaluator truth; dashed markers and ellipses show the belief. Camera movement never advances the estimator.
● Truth / evaluator◇ Estimated position◯ Marginal 95% contour┄ Error / evaluator
MODEL TIME0.00 sPrediction step 0 / 128
POSITION ERROR / EVALUATOR0.000 mDistance between true and estimated drone positions
HEADING ERROR / EVALUATOR0.00°Smallest angular difference, wrapped to ±180°
MAP ERROR / EVALUATORNo map yet0 / 4 landmarks initialized
SENSOR INPUTS / CURRENT INTERVAL
What did the drone actually observe?
Drone pose / evaluator comparison
The displayed true landmark positions, sensing radius and error links belong to the evaluator. Measurement rays illustrate the supplied range and bearing from the true drone pose; only the noisy packet enters the estimator.
INITIALIZE OR REOBSERVE
An empty map starts with an observation.
Inspect Jacobians, innovation and gain
Current map / final belief
| Landmark | Estimated x, y / m | Error / evaluator | Seen |
|---|
JOINT COVARIANCE / FINAL BELIEF
One map, connected to one pose.
Rows and columns expand as new landmark IDs enter the map. Off-block values retain dependence between the robot and landmarks, or between different landmarks. Position–position entries use m², heading–heading rad², and mixed entries m·rad. The scene and matrix show the final belief after all packets at this model time.
THIS RUN / EVALUATOR HISTORY
Reobservations can change the whole belief.
Solid curves show evaluator errors; dashed curves show reported RMS uncertainty, not per-run confidence bounds. Past estimates are recorded online: later corrections do not redraw the old estimated trajectory.
Landmark initialization and schedule events
PREDICT · COMPARE · EXPLAIN
A familiar landmark is new information.
Load a paused case. Step to a sensor frame, inspect how a landmark enters the map, then inspect a later observation of the same ID. Compare the same noisy trajectory without reobservation corrections.
Reproduce the reference comparisons
Independent copies use seed 7 and the same 32-second budget. Opening these results never replaces the active run. Noise realizations and scenario assumptions matter; one endpoint is not a universal ranking.
| Scenario | Method | Position / m | Heading / ° | Map RMS / m | Map size |
|---|
EXTENDED KALMAN FILTER SLAM
The map is uncertain.
So is the observer.
A landmark observation depends on the difference between its position and the robot’s position, and on the robot’s heading. The same uncertainty affects where a new landmark gets placed.
The EKF keeps these dependencies in a single Gaussian approximation. It is not a complete camera or LiDAR SLAM system: feature extraction, unknown associations and robust outlier rejection are absent.
Add an unknown landmark
Use the current pose and its first range–bearing packet to create the landmark mean. The inverse observation Jacobians also add its covariance and its cross-covariance with the existing state. The same first packet is not reused as another correction.
Compare predicted and observed
Predict the range and body-relative bearing to the mapped landmark. The innovation is measured minus predicted; its bearing component is wrapped around ±π. The gain can correct robot pose and other mapped landmarks.
A local Gaussian approximation
The EKF linearizes near its current estimate. Large errors, wrong IDs or biased sensors can violate its assumptions. Small reported ellipses alone cannot establish accuracy or consistency.
Δx = Lₓ − x · Δy = Lᵧ − y
h(m) = [√(Δx² + Δy²), wrap(atan2(Δy, Δx) − θ)]ᵀ
ν = z − h(m⁻) · νᵦ = wrap(νᵦ)
H = ∂h/∂m · S = HP⁻Hᵀ + R
K = P⁻HᵀS⁻¹ · m⁺ = m⁻ + Kν
New landmark: L = [x + r cos(θ + β), y + r sin(θ + β)]ᵀ
h(m) = [√(Δx² + Δy²), wrap(atan2(Δy, Δx) − θ)]ᵀ
ν = z − h(m⁻) · νᵦ = wrap(νᵦ)
H = ∂h/∂m · S = HP⁻Hᵀ + R
K = P⁻HᵀS⁻¹ · m⁺ = m⁻ + Kν
New landmark: L = [x + r cos(θ + β), y + r sin(θ + β)]ᵀ
- m, P / joint belief
- Robot pose [x, y, θ] followed by two coordinates per initialized landmark. Heading and bearings use radians in the model; the interface displays angles in degrees.
- r, β / supplied observation
- Range in meters and bearing relative to the robot’s forward direction. The landmark ID identifies an association, not a world coordinate.
- H and K / local correction
- The Jacobian H linearizes the range–bearing prediction. Kalman gain K maps the innovation into corrections for the entire joint state. The covariance update uses Joseph form.
- Marginal 95% contours
- Planar position contours use each 2 × 2 covariance block and χ²₂(0.95) ≈ 5.991. They describe the EKF’s Gaussian approximation, not a measured safety region or guaranteed coverage under bias.
The start fixes the reference frame
The initial pose [4, 0, π/2] is supplied exactly. The map begins empty and gets expressed in that anchored frame. There is no later absolute position fix, and the known starting pose is not re-applied on completing the circle.
Map once versus update together
The odometry-mapping baseline uses the same first sightings to initialize landmarks, but ignores later sightings of known IDs. Its map and path can drift together. The comparison isolates the contribution of repeat observations; neither method knows the true landmark positions.
Primary method sources: Durrant-Whyte and Bailey — Simultaneous Localization and Mapping, Part I and Bailey and Durrant-Whyte — Part II. This bounded EKF-SLAM experiment assumes supplied landmark identities and a known initial frame. It does not perform data association, visual place recognition, graph optimization or collaborative SLAM.