📘

Range-Bearing Observation

MediumSLAM 📚 CV: Structure from Motion and SLAM

Compute the expected range and bearing observation of a landmark from a robot pose.

In SLAM, robots often observe landmarks using range-bearing sensors (like lidar). Given the robot pose and landmark position, we predict what the sensor should observe:

$r = \sqrt{(l_x - x)^2 + (l_y - y)^2}$ $\phi = \arctan2(l_y - y, l_x - x) - \theta$

where:

$(x, y, \theta)$ is the robot pose
$(l_x, l_y)$ is the landmark position
$r$ is the range (distance to landmark)
$\phi$ is the bearing (angle relative to robot heading)

This observation model is used in EKF-SLAM and particle filter SLAM.

Example:

Input:

observe_landmark([0, 0, 0], [3, 4])

Output:

[5.0, 0.9273]

Reasoning:

Robot at origin, landmark at (3, 4):

Compute displacement: dx = 3-0 = 3, dy = 4-0 = 4
Range: r = sqrt(3² + 4²) = sqrt(9 + 16) = sqrt(25) = 5
Absolute angle: atan2(4, 3) ≈ 0.9273 rad (≈ 53°)
Bearing: 0.9273 - 0 = 0.9273 (angle relative to heading) The landmark is 5 units away, about 53° to the left.

Constraints:

pose: [x, y, theta] robot pose
landmark: [lx, ly] landmark world position
Return [range, bearing] rounded to 4 decimal places

More from CV: Structure from Motion and SLAM

Map Point Visibility TestHard Triangulation of 3D PointsMedium PnP Pose from CorrespondencesMedium Checkerboard Corner DetectionHard Odometry Pose UpdateEasy

Range-Bearing Observation in SLAM

Background Knowledge

SLAM Fundamentals: Simultaneous Localization and Mapping (SLAM) is a foundational problem in robotics where a mobile robot must build a map of its environment while simultaneously determining its own location within that map. Range-bearing sensors are a common observation model in SLAM systems, providing two key pieces of information about detected landmarks: the distance (range) to the landmark and the angular direction (bearing) relative to the robot's heading.

Observation Models in SLAM: The observation model defines how a robot's sensors measure the state of the world. In range-bearing SLAM, sensors like LiDAR or ultrawideband (UWB) tags measure both the Euclidean distance to landmarks and their angular position relative to the robot's current orientation. This two-dimensional measurement is more informative than range-only observations (which lack bearing information) and enables faster landmark initialization and more robust localization compared to bearing-only approaches. The observation model you're implementing is used in Extended Kalman Filter (EKF) SLAM and particle filter-based SLAM algorithms to predict expected measurements and update robot pose and landmark estimates.

Coordinate Transformations: The core challenge involves transforming between the global coordinate frame (where the landmark position is known) and the robot's local frame (where the sensor makes observations). The range is invariant to rotation, but the bearing is fundamentally a relative angle—it must account for the robot's current heading orientation. This transformation is essential for all SLAM algorithms that fuse sensor observations with state estimates.

Algorithm/Approach

The solution follows a coordinate transformation and trigonometric computation pattern:

Translate to robot-centric coordinates: Compute the displacement vector from the robot to the landmark in the global frame
Calculate range: Use the Euclidean distance formula on this displacement
Calculate global bearing: Use arctan2 to find the absolute angle to the landmark
Normalize to robot frame: Subtract the robot's heading to get the relative bearing

This approach is deterministic and computationally efficient, making it suitable for real-time SLAM implementations.

Step-by-Step Strategy

Step 1: Compute Displacement Vector

Calculate the difference between the landmark position and robot position in the global frame:

$\Delta x = l_x - x$
$\Delta y = l_y - y$

This vector points from the robot toward the landmark in global coordinates.

Step 2: Calculate Range

Apply the Euclidean distance formula: $r = \sqrt{(\Delta x)^2 + (\Delta y)^2}$

👑

Continue the full explanation

You're reading the free preview. Unlock the complete walkthrough, the code editor, test runner and reference solution with Premium.

🔒

Editor locked

The code editor is locked for Pro problems. It is only available for free problems. Please upgrade to gain access to the code editor for all problems.

solution.py

Python 3.13

Test Results

0/0

Run code to see test results.