Odometry Pose Update

easyPROSLAMCV: Structure from Motion and SLAM

Update a robot's 2D pose given odometry measurements.

In mobile robotics, odometry estimates motion from wheel encoders or visual odometry. The standard 2D motion model is:

$\begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \begin{pmatrix} x + d \cdot \cos(\theta) \\ y + d \cdot \sin(\theta) \\ \theta + \delta\theta \end{pmatrix}$

where:

$(x, y, \theta)$ is the current pose (position and heading)
$d$ is the distance traveled forward
$\delta\theta$ is the change in heading
$(x', y', \theta')$ is the new pose

This "dead reckoning" accumulates error over time, which is why SLAM also uses landmarks.

Example

Input

odometry_update([0, 0, 0], (1, 0))

Output

[1.0, 0.0, 0]

Reasoning

Inputs: pose is the robot's current state $(x, y, \theta) = (0, 0, 0)$ : it sits at the origin with heading $\theta = 0$ rad, i.e. facing along the positive $x$ -axis. motion is $(d, \delta\theta) = (1, 0)$ : drive 1 unit forward, then turn by 0 rad.
Splitting the step: the forward distance is projected onto the axes by the heading. With $\cos 0 = 1$ and $\sin 0 = 0$ , all of it goes into $x$ : $x' = 0 + 1 \cdot 1 = 1$ and $y' = 0 + 1 \cdot 0 = 0$ .
Heading: $\theta' = \theta + \delta\theta = 0 + 0 = 0$ , so the robot still faces along $x$ .
Output: the new pose [1.0, 0.0, 0], each entry rounded to 4 decimal places. The positions are decimals because they came out of the trigonometry; the heading is a plain sum of two whole numbers.
Notice: the step uses the old heading. Had the robot started at $\theta = \pi/2$ (facing up), the same 1-unit move would land at $(0, 1)$ instead, since $\cos\tfrac{\pi}{2} = 0$ and $\sin\tfrac{\pi}{2} = 1$ .

Constraints

pose: [x, y, theta] current pose (theta in radians)
motion: (distance, delta_theta) odometry measurement
Return new pose [x', y', theta'] rounded to 4 decimal places

Learn this in

Structure from Motion and SLAMCV Study Plan

More from CV: Structure from Motion and SLAM

Map Point Visibility Testhard Triangulation of 3D Pointsmedium PnP Pose from Correspondencesmedium Checkerboard Corner Detectionhard Range-Bearing Observationmedium

Concept primerPREVIEW

Odometry Pose Update: Background & Strategy

Background Knowledge

Odometry and Dead Reckoning

Odometry is a fundamental technique in mobile robotics for estimating a robot's position and orientation by integrating motion measurements over time. In 2D SLAM systems, odometry typically comes from wheel encoders (for wheeled robots) or visual odometry (from camera-based motion estimation). The core idea is straightforward: given the robot's current pose and a measured motion (distance traveled and heading change), calculate the new pose using kinematic equations. This process is called "dead reckoning" because it relies solely on motion measurements without external references like GPS or landmarks.

The 2D Kinematic Motion Model

The motion model you've been given represents a differential-drive or unicycle robot moving in 2D space. The key insight is that when a robot moves forward by distance $d$ while at heading $\theta$ , the displacement in the global coordinate frame depends on the current orientation. The $\cos($ \theta $)$ and $\sin($ \theta $)$ terms convert the robot's local forward motion into global $x$ and $y$ components. The heading update is simply additive: $\theta' =$ \theta $+$ \delta $\theta$ .

Why Odometry Alone Isn't Sufficient

While odometry provides real-time motion estimates, it accumulates error over time—a phenomenon called pose drift. Small measurement errors in distance and heading compound with each update, causing the estimated pose to diverge from the true pose. This is why SLAM systems combine odometry with landmark observations and loop closure detection to correct accumulated errors and maintain global consistency.

Algorithm/Approach

The solution follows a straightforward state update pattern:

Extract the current pose components: $x$ , $y$ , $\theta$
Extract the odometry measurements: $d$ (distance) and $\delta\theta$ (heading change)
Apply the kinematic equations to compute new pose components
Return the updated pose

This is a direct application of the motion model—no iteration, optimization, or filtering is needed at this level. The challenge is primarily in correctly implementing the trigonometric transformations and handling angle conventions (radians vs. degrees, angle wrapping).

Step-by-Step Strategy

PRO

Continue the full explanation

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

PRO

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