Primers
Coordinates and poses
A pose is where a robot is and which way it’s facing. Every map, every “drive to that point” and every sensor reading placed on a map rests on it. The maths is a handful of sines and cosines, and most of the bugs come from three things: mixing up whose point of view a number is from, forgetting that angles go round in a circle, and underestimating what a small error in the heading does far away.
The examples on this page use the small robot from encoders and odometry: 60 mm wheels, about 4,300 counts a wheel turn, and wheels 12 cm apart. Pick your level at the top of the page.
High school
A frame: where zero is, and which way is positive
To say where something is, you first agree on a frame: a starting point, the origin, and two directions, the axes. On a desk you might put the origin at the front left corner, with x running along the front edge and y running back across the desk. Then (0.40, 0.25) means 40 cm along and 25 cm back.
A frame also says which way is positive. Here x gets bigger to the right and y gets bigger going away from you. Two people with different frames can give different numbers for the same spot, and both be right. Many mistakes with coordinates come from two frames mixed up.
Position and heading make a pose
A robot’s position is its (x, y). That isn’t enough to drive anywhere: you also need which way it faces, its heading. Heading is an angle measured from the x axis. In robotics it’s usually measured counter-clockwise: 0° faces along +x, 90° faces along +y, 180° faces along −x.
Position and heading together are the robot’s pose: (x, y, heading). A robot at (1.00, 0.50, 90°) is 1 m along, 50 cm back, and facing straight up the y axis.
The robot’s own frame
The robot carries a frame of its own. Its origin is a point on the robot, usually its centre, its x axis points straight ahead and its y axis points to its left. When a sensor says “a box 30 cm ahead and 10 cm to the left”, that’s (0.30, 0.10) in the robot’s frame. The desk’s frame is the world’s frame. The robot’s pose is what links the two.
From what the robot sees to the map
Say the robot is at (1.00, 0.50) facing 90°, straight up the y axis. Ahead for the robot is +y for the world, and the robot’s left is −x for the world. So a box 30 cm ahead and 10 cm left is at
world x = 1.00 − 0.10 = 0.90, world y = 0.50 + 0.30 = 0.80
For any heading, the recipe is:
world x = robot x + ahead × cos(heading) − left × sin(heading)
world y = robot y + ahead × sin(heading) + left × cos(heading)
Try it with heading 0°: cos is 1 and sin is 0, so the box is just (robot x + ahead, robot y + left), as it should be when the two frames point the same way.
Angles go round
Headings of 359° and 1° are only 2° apart. A robot facing 359° that wants to face 1° should turn 2° to the left, but subtract the two, 1 − 359, and you get −358°: 358° to the right. The fix is to wrap every difference between angles: if it’s more than 180°, take away 360°, and if it’s less than −180°, add 360°. Then −358° becomes +2°, and every turn is the short way round.
Which way to face: atan2
To drive to a point, the robot needs the direction to it. Say the robot is at (1.0, 0.5) and the goal is at (2.0, 1.5). The goal is 1.0 further in x and 1.0 further in y, so it’s at 45°.
The usual way to get that angle is a function called atan2: atan2(change in y, change in x). Notice the y comes first. Why not just the arctangent of y ÷ x? Because a goal at (−1, −1) from the robot gives the same fraction, 1, as one at (+1, +1), but it’s in the opposite direction. atan2 looks at both signs and answers −135°.
The turn the robot needs is the direction to the goal minus its heading, wrapped. Facing 90° with the goal at 45°, it turns −45°, which is 45° to the right.
Small heading errors grow with distance
If the robot thinks it’s facing 90° but is really facing 92°, everything it sees is put on the map turned a little round the robot from where it really is. Near the robot that hardly matters. Far away it does: each degree of heading error moves a point about 1.75 cm for every metre it’s away. A box 2 m away lands about 7 cm from where it really is.
On the robot from the encoders primer, with its wheels 12 cm apart, 2° of turn comes from one wheel rolling just 4.2 mm further than the other. That’s one reason heading is the hard part of a pose worked out from wheel counts.
Check yourself
- A robot is at (2.0, 1.0) facing 0°. It sees a cup 0.5 m ahead and 0.2 m to its right. Where is the cup in the world?
- A robot is at (0, 0) facing 180°. Where is a point 1 m straight ahead of it?
- The robot faces 350° and wants to face 10°. Which way should it turn, and how far?
- A goal is at (−1, −1) from the robot. The arctangent of −1 ÷ −1 says 45°. What’s the real direction?
- The heading is 1° wrong. Roughly how far out is a point 3 m away?
<details><summary>Answers</summary>
- Right is negative left, so left = −0.2. At 0°, the cup is at (2.0 + 0.5, 1.0 − 0.2) = (2.5, 0.8).
- Facing 180°, ahead is −x: (−1, 0).
- 10 − 350 = −340, wrapped to +20°: 20° to the left (counter-clockwise).
- −135° (the same direction as 225°). atan2 knows both changes are negative.
- About 1.75 cm a metre, × 3 m ≈ 5 cm.
</details>
College
Rotation matrices
Turning a vector by an angle θ, counter-clockwise, is a matrix product:
R(θ) = | cos θ −sin θ |
| sin θ cos θ |
Its columns are the robot’s own x and y axes, written in world coordinates. A rotation keeps lengths, so R is orthonormal and its inverse is its transpose, R(θ)ᵀ = R(−θ). In two dimensions rotations add, R(a) R(b) = R(a + b), which is the reason a 2D heading can be stored as one number.
The homogeneous transform
A pose is a rotation and a shift. Writing the pair as one 3 × 3 matrix lets one product do both:
T = | R t | = | cos θ −sin θ x |
| 0 1 | | sin θ cos θ y |
| 0 0 1 |
A point (pₓ, pᵧ) becomes the column (pₓ, pᵧ, 1), and T times it is the point rotated by R then moved by t. That’s the high school recipe, in one line.
Name each transform by the two frames it links: Twr takes a point in the robot’s frame ® to the world’s frame (w). With that habit the subscripts tell you what you can multiply: inner letters must match.
Composing and inverting
Transforms chain by multiplying:
Tws = Twr Trs
This reads “sensor to robot, then robot to world”, and the inner r’s cancel. The order matters. Driving 1 m and then turning 90° leaves the robot somewhere different from turning 90° and then driving 1 m, and matrix products don’t commute for the same reason.
The inverse goes the other way, world to robot:
T⁻¹ = | Rᵀ −Rᵀ t |
| 0 1 |
The goal, written in the robot’s frame, is Twr⁻¹ times the goal in the world’s frame.
The chain: sensor to robot to world
A sensor is rarely at the robot’s centre. Say a distance sensor sits 5 cm ahead of the centre and 2 cm to the left, angled 30° to the left. Its own frame has x along its beam. A reading of 0.25 m is (0.25, 0) in the sensor’s frame.
- Sensor to robot: (0.05 + 0.25 cos 30°, 0.02 + 0.25 sin 30°) = (0.267, 0.145).
- Robot to world, with the robot at (1.00, 0.50, 90°): (1.00 − 0.145, 0.50 + 0.267) = (0.855, 0.767).
Trs is fixed, measured once from the robot’s body. Twr changes all the time. A reading belongs with the pose the robot had when the reading was taken, not the pose when the program gets round to using it. A robot turning at 90° a second has turned 9° in 100 ms, which at 1 m is about 16 cm.
Radians and wrapping in code
Maths libraries take angles in radians: a full turn is 2π, and one radian is about 57.3°. Convert at the edges (input and display) and keep radians inside.
Wrapping in code:
wrap(a) = ((a + π) mod 2π) − π
gives a value in [−π, π). The line relies on mod taking the sign of its divisor, as Python’s % does. C’s fmod
keeps the sign of its first argument instead, so the same line there can return angles below −π. Python’s
math.remainder(a, 2 * math.pi) is another way, and returns a value between −π and π.
Wrap every difference of angles before using it: a heading error, a turn left to go, a change between two readings.
Distance and bearing to a goal
With the robot at (x, y, θ) and a goal at (gₓ, gᵧ):
d = √((gₓ − x)² + (gᵧ − y)²)
bearing = wrap(atan2(gᵧ − y, gₓ − x) − θ)
The bearing is the angle to turn, positive to the left. These two numbers are the errors a simple go-to controller works on: it steers to bring the bearing to zero and drives to bring the distance to zero. Each is a feedback loop of the kind in feedback control. Leave out the wrap and a robot facing 170° with a goal at −170° sees an error of −340° and turns nearly a full circle the wrong way.
The 2° from high school, in wheel counts: Δθ = (Δsᵣ − Δsₗ) ÷ b, so 2° (0.0349 rad) on a 120 mm track is 4.2 mm of difference between the wheels, about 96 counts at 4,300 a turn.
A real convention: REP 103
ROS writes its conventions down in REP 103, Standard Units of Measure and Coordinate Conventions. For a body (a robot, a sensor), it uses:
- x forward, y left, z up
- right-handed axes, so yaw (heading) increases as the body turns counter-clockwise seen from above
- SI units: metres, and radians for angles.
For places on the Earth it uses east-north-up (ENU): x east, y north, z up. That puts yaw 0 at east, counting
counter-clockwise, which is not a compass bearing (0 at north, clockwise). The REP gives this a special mention for
that reason, and asks hardware drivers to convert before they publish. Cameras often get a second frame, with the
suffix _optical, that has z forward, x right and y down.
Check yourself
- What is R(90°) times (1, 0)?
- A robot’s pose is (1.0, 0.5, 90°). Where is the world’s origin in the robot’s frame?
- The heading is 3.0 rad and the direction to the goal is −3.0 rad. What’s the unwrapped turn, and the wrapped one?
- A compass on the robot reads 30°. What’s the yaw in REP 103’s ENU convention?
- The right wheel has counted 96 more than the left since the robot faced 0°. Roughly what’s its heading now?
<details><summary>Answers</summary>
- The first column of R(90°): (0, 1). The robot’s x axis points up the world’s y.
- Use the inverse. Rᵀt = R(−90°)(1.0, 0.5) = (0.5, −1.0), so −Rᵀt = (−0.5, 1.0): half a metre behind and a metre to the left. Check by eye: the robot faces +y, and the origin is below it and to its left.
- −3.0 − 3.0 = −6.0 rad unwrapped. Add 2π: +0.28 rad, about 16° to the left.
- Compass bearings start at north and go clockwise. ENU yaw starts at east and goes counter-clockwise, so yaw = 90° − 30° = 60°.
- 96 counts × 188.5 mm ÷ 4,300 ≈ 4.2 mm. Divided by 120 mm that’s 0.035 rad, about 2° to the left.
</details>
University
SE(2) as a group
The planar poses, with composition as the operation, form the special Euclidean group SE(2), written as the 3 × 3 matrices above with R ∈ SO(2). It meets the group axioms:
- closure: a pose composed with a pose is a pose, (R₁, t₁)(R₂, t₂) = (R₁R₂, t₁ + R₁t₂)
- associativity: (AB)C = A(BC), so a long chain can be grouped any way
- identity: (I, 0), the pose of a frame relative to itself
- inverse: (R, t)⁻¹ = (Rᵀ, −Rᵀt).
It isn’t commutative. It’s also a Lie group, a smooth three-dimensional manifold, which is what lets estimation treat small pose changes as vectors (x, y, θ) in its tangent space.
Pose composition: ⊕ and ⊖
Smith, Self and Cheeseman wrote poses as vectors xij = (x, y, φ), the pose of frame j seen from frame i, with two operations. Compounding chains two relationships head to tail:
xik = xij ⊕ xjk = (xjk cos φij − yjk sin φij + xij, xjk sin φij + yjk cos φij + yij, φij + φjk)
The reverse relationship turns one round:
xji = ⊖xij = (−xij cos φij − yij sin φij, xij sin φij − yij cos φij, −φij)
These are the SE(2) product and inverse in vector form. Compounding is associative but not commutative. Odometry is compounding: xₖ = xₖ₋₁ ⊕ uₖ, with uₖ the motion measured in the robot’s frame. The relative pose of two poses both known in the world is the tail-to-tail combination:
xij = ⊖xwi ⊕ xwj
For example, with xwi = (1, 0.5, 90°) and xwj = (1, 1.5, 180°), ⊖xwi = (−0.5, 1, −90°) and xij = (1, 0, 90°): the second pose is 1 m ahead of the first and turned 90° left.
The ⊕ and ⊖ symbols live on in modern work with a different meaning. Solà, Deray and Atchuthan define, for a Lie group element X and a tangent vector τ, X ⊕ τ = X ∘ Exp(τ) and Y ⊖ X = Log(X⁻¹ ∘ Y). There, ⊕ adds a small increment expressed in X’s local frame, and ⊖ is the increment between two poses. When a text uses ⊕, check which of the two it means.
How heading error becomes position error
The Jacobian of compounding shows it directly. Differentiate xik with respect to φij:
∂(xik, yik) ÷ ∂φij = (−(yik − yij), xik − xij)
That’s the lever arm from frame j to the point, turned 90°. A heading error δφ moves a point at distance d by exactly 2d sin(δφ ÷ 2), at right angles to the line of sight, and to first order by d δφ. With a heading standard deviation σφ, the cross-range standard deviation of a point at distance d is about d σφ. A 2° error (0.0349 rad) puts a point 2 m away 69.8 mm out. A heading error alone doesn’t change the range at all, because a rotation keeps distances from its centre. So the spread from a heading error is long and thin, across the line of sight: an arc, which a linearised filter draws as a thin ellipse.
On a moving robot the heading error itself grows with distance driven, so the cross-track error of the robot’s own position grows faster than the distance. The encoders primer gives the d^3/2 rate for a random walk in heading.
Small-angle approximations
For small δ, sin δ ≈ δ and cos δ ≈ 1 − δ²/2, so
R(δ) ≈ | 1 −δ |
| δ 1 |
The relative error of sin δ ≈ δ is about δ² ÷ 6: 0.02% at 2°, 0.5% at 10°, and 1% at about 14°. First-order estimators (the extended Kalman filter, and Smith, Self and Cheeseman’s covariance propagation) rest on these linearisations. They hold well for heading errors of a few degrees. With tens of degrees of uncertainty the linearised ellipse stops looking like the real, curved spread of possible positions.
Three dimensions, briefly
In 3D a rotation is a 3 × 3 matrix in SO(3): nine numbers held to six constraints (orthonormal columns, determinant +1), and a pose is a 4 × 4 matrix in SE(3). Unlike in 2D, rotations alone don’t commute.
Three numbers, such as Euler angles or roll, pitch and yaw, can’t describe every orientation without a singularity. For ZYX Euler angles, at a pitch of ±90° the first and last rotations are about the same axis, and only their difference is defined: a whole family of angle sets gives the same rotation, and one degree of freedom is lost. In a mechanical gimbal, where two of the three axes line up, this is called gimbal lock, and the name is used for the angles too.
A unit quaternion uses four numbers held to one constraint (length 1) and has no singularities. Two rotations compose by a quaternion product, and rounding that drifts it off length 1 is fixed by renormalising. Each rotation has two, q and −q, so comparing two quaternions needs care with the sign. REP 103 lists quaternions as ROS’s preferred representation, then rotation matrices, and discourages Euler angles because there are 24 conventions in use.
Check yourself
- Show that (Rᵀ, −Rᵀt) is the inverse of (R, t) under the composition rule above.
- Using the formulas, check that x ⊕ (⊖x) is the identity for x = (1, 0.5, 90°).
- A robot’s heading has a standard deviation of 1°. A landmark is 3 m away. What’s the cross-range standard deviation of its mapped position, and the shape of its uncertainty?
- Why does an extended Kalman filter tracking a pose with 30° of heading uncertainty give poor ellipses?
- Why does REP 103 discourage Euler angles, and what goes wrong with any three-number description of 3D orientation?
<details><summary>Answers</summary>
- (R, t)(Rᵀ, −Rᵀt) = (R Rᵀ, t + R(−Rᵀt)) = (I, t − t) = (I, 0). The other order gives (RᵀR, −Rᵀt + Rᵀt) = (I, 0) too.
- ⊖x = (−0.5, 1, −90°). Compounding with φ = 90°: x = −0.5 cos 90° − 1 sin 90° + 1 = 0, y = −0.5 sin 90° + 1 cos 90° + 0.5 = 0, φ = 90° − 90° = 0. (0, 0, 0).
- 3 m × 0.01745 ≈ 5.2 cm, across the line of sight. Its range error doesn’t grow, so the uncertainty is long and thin, at right angles to the line to the landmark.
- At 30°, sin δ ≈ δ is about 5% out and cos δ is 0.87, not 1, so the linearised spread misses the curved, banana shape of the positions the robot could really be at.
- REP 103’s reason is that there are 24 Euler angle conventions in use, so two programs can read the same three numbers differently. And any three-number description has a singularity somewhere, such as gimbal lock at ±90° pitch for ZYX angles. Quaternions and rotation matrices have neither problem.
</details>
Try it
No robot needed: a short Python program that puts what a robot sees on the map, works out the distance and turn to a goal, and shows what 2° of heading error does. The robot is at (1.00, 0.50), facing 170°. You need only a container runtime. Start a shell with Python in it:
container
container run --rm -it docker.io/library/python:3.14-alpine sh
docker
docker run --rm -it python:3.14-alpine sh
podman
podman run --rm -it docker.io/library/python:3.14-alpine sh
Inside it, save the program and run it:
cat > poses.py <<'EOF'
"""Points a robot sees, put in the world's frame, then the distance and bearing to a goal.
A pose is (x, y, heading). x is forward and y is left, in metres. Heading is counter-clockwise from the world's
x axis, in degrees on screen and radians inside.
"""
import math
WRAP = True # wrap the turn into -180 to 180 degrees
def wrap(a):
"""An angle in radians, brought into [-pi, pi)."""
return (a + math.pi) % (2 * math.pi) - math.pi if WRAP else a
def to_world(pose, ahead, left):
"""A point the robot sees, ahead and left of it, in the world's frame: rotate by the heading, then shift."""
x, y, h = pose
return (x + ahead * math.cos(h) - left * math.sin(h),
y + ahead * math.sin(h) + left * math.cos(h))
def to_goal(pose, gx, gy):
"""How far the goal is, and how far to turn to face it, positive to the left."""
x, y, h = pose
return math.hypot(gx - x, gy - y), wrap(math.atan2(gy - y, gx - x) - h)
robot = (1.0, 0.5, math.radians(170))
print("Robot at x 1.00, y 0.50, heading 170 deg")
for ahead, left in [(0.30, 0.10), (2.00, 0.00)]:
wx, wy = to_world(robot, ahead, left)
print(f" sees {ahead:.2f} ahead, {left:.2f} left: in the world, x {wx:5.2f}, y {wy:5.2f}")
d, turn = to_goal(robot, 0.0, 0.3)
print(f"Goal at x 0.00, y 0.30: {d:.2f} m away, turn {math.degrees(turn):+.1f} deg")
off = (1.0, 0.5, math.radians(172)) # the heading 2 degrees off
for ahead in (0.30, 2.00):
a, b = to_world(robot, ahead, 0), to_world(off, ahead, 0)
print(f"Heading off by 2 deg: a point {ahead:.2f} m ahead moves {100 * math.dist(a, b):.1f} cm")
EOF
python poses.py
Robot at x 1.00, y 0.50, heading 170 deg
sees 0.30 ahead, 0.10 left: in the world, x 0.69, y 0.45
sees 2.00 ahead, 0.00 left: in the world, x -0.97, y 0.85
Goal at x 0.00, y 0.30: 1.02 m away, turn +21.3 deg
Heading off by 2 deg: a point 0.30 m ahead moves 1.0 cm
Heading off by 2 deg: a point 2.00 m ahead moves 7.0 cm
The box 30 cm ahead and 10 cm left lands at (0.69, 0.45): the robot faces nearly along −x, so ahead is mostly −x and left is mostly −y. atan2 puts the goal at −168.7° from the world’s x axis. From a heading of 170° the raw difference is −338.7°, and wrapped it’s a short 21.3° to the left. The same 2° of heading error moves a point 30 cm away by 1 cm and one 2 m away by 7 cm.
Now switch the wrap off and run it again:
sed -i 's/^WRAP = True/WRAP = False/' poses.py
python poses.py | grep Goal
Goal at x 0.00, y 0.30: 1.02 m away, turn -338.7 deg
The direction to the goal is the same, but the turn is now 338.7° to the right, nearly a full circle the long way round.
Type exit to leave.
Learn more
- REP 103, Standard Units of Measure and Coordinate Conventions, by T. Foote and M. Purvis: ROS’s axes, units, yaw sign, ENU and camera frames, and its order of preference for rotation representations.
- R. Smith, M. Self and P. Cheeseman, Estimating Uncertain Spatial Relationships in Robotics, 1986 (full version 1988): compounding (⊕), the reverse relationship (⊖), and how their Jacobians carry uncertainty along a chain of frames.
- J. Solà, J. Deray and D. Atchuthan, A micro Lie theory for state estimation in robotics: SO(2), SE(2), SO(3) and SE(3) as Lie groups, the modern ⊕ and ⊖, and the Jacobians a pose estimator needs.
- K. M. Lynch and F. C. Park, Modern Robotics, free preprint and video lectures. Chapter 3 covers rotation matrices and homogeneous transforms in 3D, and Appendix B covers Euler angles, their singularity, and unit quaternions.
- Python’s math module:
atan2,hypot,remainder, and how%andfmoddiffer in sign. - Wikipedia’s atan2 page, for the quadrants and the argument order, and its gimbal lock page, for the mechanical picture.