M7-T3 — Navigation Under Pose Uncertainty

Learning objectives

After this lesson, you should be able to:

• Compute the standard deviation of the position error along a direction from a 2×2 covariance.
• Convert an accepted collision probability into a number of sigmas and a margin.
• Choose between inflating a margin, lowering speed and avoiding an area, and say what each costs.
• Explain why a small covariance does not prove that the pose is right.

The intuition

Imagine walking a narrow corridor in the dark with a rough idea of where you are. You do not walk faster because you feel sure. You keep your hands out, you stay near the middle, and in the narrowest part you slow down. The more lost you feel, the more room you want, and you want it most in the direction of the walls.

A localizer’s covariance is that feeling in numbers. For position it is a 2×2 matrix whose ellipse shows where the true position probably is, given the estimate. The ellipse is often long in one direction: in a straight corridor the scan matcher locks onto the side walls and is vague along the corridor, so the ellipse is long along the corridor and thin across it. In a glass atrium it can be the other way round.

What matters for a wall is the spread towards that wall, not the biggest spread. A long ellipse along a corridor is harmless; the same ellipse turned across the corridor can use up the margin on both sides. That is why the figure is drawn in the corridor’s own frame: the 0.26 m across the corridor is what decides if the corridor is passable.

The mathematics

The spread along a direction

Let the position error be a zero-mean Gaussian with covariance Σ = [[σ_xx, σ_xy], [σ_xy, σ_yy]] in the map frame. Take a unit vector u from the robot to the nearest obstacle. The error projected on u is a one-dimensional Gaussian whose variance is

Its extremes are the square roots of the eigenvalues of Σ, along the ellipse’s axes. Reading only σ_xx or σ_yy ignores the correlation and is wrong in every direction except along the map axes.

From a probability to a margin

For a wall at the other end of the direction u, a collision means the error towards the wall exceeds the margin beyond the robot’s radius. If the robot may collide with probability at most p, use the one-sided Gaussian tail:

r

radius of the robot used in the costmap (inscribed or circumscribed, lesson M2-T2).

σ_u

spread of the position error in the direction of the obstacle.

n

number of standard deviations, chosen from the accepted probability p.

p

probability per encounter that the real position is beyond the margin. It is not a probability of collision per mission.

The 2-D ellipse is a different thing: the ellipse that holds the true position with probability P has semi-axes scaled by k = sqrt(χ²₂(P)), which is 2.447 for 95% and 3.035 for 99%. Use the one-dimensional rule above for a wall and the ellipse when you need the region.

Three ways to use it

1. Widen the margin. Replace r by r + n σ_u in the inflation layer, per cell or per area. Safe, but narrow places close (lab M7-L2).
2. Slow down. Keep the margin and reduce the speed so that the stopping distance and the uncertainty along the direction of travel fit in the free space ahead (lesson M7-T1).
3. Avoid or fix it. Route around places where localization is poor, or stop and improve it (a landmark, a longer scan, a human fix).

A worked example

Scout in the atrium corridor. The localizer reports, in the corridor’s frame, σ_along = 0.10 m and σ_across = 0.2646 m. The corridor is 1.6 m wide (half-width 0.80 m) and Scout’s radius is 0.30 m. Accept p = 1%, so n = 2.326.

At p = 5% (n = 1.645) the margin is 0.30 + 0.435 = 0.735 m, and the corridor is open with 0.065 m to spare. The decision depends on the risk you accept, and that is a decision for people, not for the algorithm.

Why the direction matters. From the same covariance written in the map frame, Σ = [[0.04, 0.03], [0.03, 0.04]], the spread is 0.20 m along x, 0.2646 m along (0.7071, 0.7071) and 0.10 m along (0.7071, −0.7071). The eigenvalues are 0.07 and 0.01. A planner that reads σ_xx = 0.04 and uses σ = 0.20 for a wall in the diagonal direction gets a margin of 0.30 + 2.326 · 0.20 = 0.765 m instead of 0.9155 m: 0.15 m too small, exactly the amount that decides this corridor.

What σ can Scout afford here? With r = 0.30 m, half-width 0.80 m and n = 2.326, the largest cross-corridor spread is (0.80 − 0.30) / 2.326 = 0.215 m. Above that the corridor is closed for the planner; the choices are to accept more risk, slow down, take another route or localize better.

A confident, wrong pose. Suppose the localizer reports σ = 0.03 m and the robot was in fact carried into another corridor of the same shape. The margin is 0.30 + 2.326 · 0.03 = 0.37 m, and every statement the planner makes about the walls refers to walls that are not there. No margin computed from the covariance protects against this, because the covariance is the filter’s opinion of itself.

Engineering implications and limitations

The covariance is the filter’s belief, not the error

It is correct only if the sensor and motion models are. Particle filters tend to under-report spread when the particle set has collapsed; extended Kalman filters are over-confident when the process noise is too small or the linearisation is poor. The consistency of a filter can be measured: over many steps, the normalised innovation squared should average its dimension, and a filter that sits well below it is over-confident. Many teams add a floor and a multiplier to the reported covariance before using it for anything safety-related.

Rotate before you use it

Covariances are given in the map or odom frame. Rotate them into the frame of the obstacle direction, or use uᵀΣu directly. An axis-aligned reading is the single most common error.

Put the number in one place

As with the footprint margin (lesson M2-T2), pick one place for the uncertainty allowance: the inflation layer, the planner’s cost, or a speed limit. Applying it in two places doubles it, and doors close. Most stacks do not feed the pose covariance into the costmap automatically; you add that link yourself and test it, for example with a map of where localization is known to be poor.

When uncertainty is too big

Set an upper threshold. Above it the right action is not a wider margin but to stop and relocalize (lesson M7-T7), and to tell someone. A robot that keeps driving with a σ of a metre is not being careful.

What this does not cover

Heading uncertainty, the footprint corners swinging out because of it, and correlated errors along a path are all real and ignored here. None of this is a safety case; it is the planning-side use of a covariance (lessons M7-T1 and M7-T2 give the other parts).

Common misconceptions

Engineering takeaways

1. The spread towards an obstacle is σ_u = sqrt(uᵀΣu); the biggest spread and the relevant spread are not the same.
2. A margin is r + n·σ_u, where n comes from the probability you accept (1.645 for 5%, 2.326 for 1% one-sided).
3. You can widen the margin, slow down, or avoid areas where localization is poor; each has a cost.
4. The covariance is the filter’s belief: check it for consistency, and add an allowance before using it for safety.
5. A confident plan on a wrong pose is the dangerous case, and only independent checks (lesson M7-T7) can catch it.

References and further reading

Probabilistic Robotics and the estimation texts give the background; LQG-MP is a primary source for planning with motion uncertainty.

• S. Thrun, W. Burgard and D. Fox, “Probabilistic Robotics”, MIT Press, 2005.
• Y. Bar-Shalom, X. R. Li and T. Kirubarajan, “Estimation with Applications to Tracking and Navigation”, Wiley, 2001 (consistency tests: NEES and NIS).
• J. van den Berg, P. Abbeel and K. Goldberg, “LQG-MP: Optimized Path Planning for Robots with Motion Uncertainty and Imperfect State Information”, International Journal of Robotics Research, 30(7), 2011.
• F. Dellaert, D. Fox, W. Burgard and S. Thrun, “Monte Carlo Localization for Mobile Robots”, IEEE ICRA, 1999.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: the sensing failures that make the costmap itself lie, and the checks that expose them.

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T2: Safety Filters: Control Barrier Functions for Navigation

  Explain how a safety filter minimally corrects an unsafe command, and where it sits in the stack.

• Lab
  M7-L2: Plan with Pose Uncertainty

  Localization is poor near the glass atrium: inflate the safety margin with the uncertainty and keep clear.