SLAM in robotics: estimate a robot’s path and its map

SLAM means simultaneous localization and mapping. A robot estimates where it has moved while building a map from its observations. Those estimates depend on each other: placing a landmark needs a robot pose, and locating the robot can use landmarks.

A small example makes that dependence concrete. Here, a robot moves along one line, observes two named landmarks, and combines six readings. You can supply the map, supply the robot positions, or ask the estimator to recover both.

Start with the joint-estimation example below. If you already understand that distinction, choose a next question about motion estimates, scan matching, or evaluating the resulting path.

What SLAM in robotics estimates

With a known map, localization estimates the robot's pose. With known robot poses, mapping places observations into a common frame. SLAM estimates the trajectory and map jointly. The SLAM chapter in Introduction to Robotics and Perception develops these distinctions with planar poses and landmark examples.

A pose normally includes position and orientation. This lesson keeps orientation fixed and represents each robot pose by one position in meters. That choice makes the estimation problem linear. A real 2D or 3D robot introduces rotations, sensor geometry, and usually nonlinear equations.

Separate the known values from the unknowns

The generated reference has robot positions x₀ = 0, x₁ = 2, x₂ = 4 m, with landmarks A = 1, B = 5 m. These values let us check accuracy after fitting.

Task	Supplied exactly	Estimated
Localization	x₀ = 0, A = 1, B = 5	x₁, x₂
Mapping	x₀ = 0, x₁ = 2, x₂ = 4	A, B
SLAM	x₀ = 0	x₁, x₂, A, B

The conditional tasks receive extra information. Their lower error, when it occurs, does not prove that one algorithm outperforms SLAM. In mapping mode, the robot positions already come from the reference, so the solver has no need for odometry.

Turn observations into relative constraints

An odometry reading measures a displacement between consecutive robot positions. A landmark reading measures a signed offset from a robot position to a named landmark. Positive means to the right; negative means to the left. This is a teaching sensor model, not an unsigned range sensor.

Let bᵤ denote the bias added to each odometry reading and b𝓏 the bias added to the last landmark observation. All entries below use meters.

Reading	Predicted value	Recorded value
u₁	x₁ − x₀	2 + bᵤ
u₂	x₂ − x₁	2 + bᵤ
z₀A	A − x₀	1
z₁A	A − x₁	−1
z₁B	B − x₁	3
z₂B	B − x₂	1 + b𝓏

The reading z₁B = 3 says that B lies three meters to the right of the robot at x₁. It does not locate B at world coordinate 3. The coordinate-frame lesson explains why that reference matters.

We assume correct landmark identities throughout. The estimator knows that z₀A and z₁A describe the same A. Choosing those identities from ambiguous sensor data is a separate data association problem, discussed in Bailey and Durrant-Whyte's SLAM tutorial, Part II.

Fix an origin before solving

Every prediction above subtracts two positions. Add 10 m to every robot position and every landmark, and all six predicted readings stay the same. Relative measurements alone cannot select an absolute origin.

Fixing x₀ = 0 exactly removes this translation freedom. It chooses the coordinate frame for the example. It does not claim that a physical sensor measured the robot's absolute location without error.

For this connected measurement graph, that anchor leaves a unique linear least-squares estimate. An unobserved, disconnected landmark would remain undetermined even with x₀ fixed. The helper rejects an unanchored or disconnected system. Real systems can use uncertain pose priors, as shown in the GTSAM factor-graph tutorial.

Weight the disagreements between readings

Define each residual as prediction minus reading. Divide odometry residuals by an assumed noise scale σᵤ and landmark residuals by σ𝓏, both in meters. Squaring and summing gives a dimensionless objective:

A smaller scale gives a reading more influence. Halving a scale quadruples its squared-residual weight. Mapping uses only the four landmark rows. Localization and SLAM use all six; localization's z₀A row has zero residual because both of its positions are supplied.

The lab solves the resulting small linear system with a Cholesky factorization of its normal matrix. It minimizes this quadratic objective directly. No nonlinear initialization or iterative scan matching enters this particular model. Review least squares for the matrix form.

Calculate one joint estimate

Use the default bᵤ = 0.4 m, b𝓏 = 0, σᵤ = 0.5 m, σ𝓏 = 0.25 m. Odometry reports two 2.4 m moves. The paired observations of A imply a first move of 1 − (−1) = 2 m. The paired observations of B imply a second move of 3 − 1 = 2 m.

Eliminating each landmark from the objective gives a displacement reading with squared scale 2σ𝓏². Its weight is 1 / (2 × 0.25²) = 8, while odometry's weight is 1 / 0.5² = 4. For either displacement d:

Therefore x₁ = 2.133333 m and x₂ = 4.266667 m. Each landmark is the average of its two implied world positions:

The two normalized odometry residuals are −0.533333. The four normalized landmark residuals alternate +0.266667 and −0.266667. Their squared sum is J = 0.853333. The readings disagree, so the best fit leaves nonzero residuals.

Compare the three estimation tasks

One-dimensional landmark estimation

Choose what the robot knows

Fit two robot positions and two landmarks to relative measurements. Switching tasks supplies exact map or robot positions; it changes the information available to the estimator.

Estimation taskSLAM: estimate poses and mapLocalization: supply the mapMapping: supply the poses

Odometry bias per move (m)

0.4

Last observation bias (m)

0

Odometry noise scale (m)

0.5

Landmark noise scale (m)

0.25

Biased odometryConsistent observationsBiased landmark observationTrust biased odometryReset SLAM experiment

Each preset returns to SLAM. Noise scales set assumed weights, 1 / scale²; they do not generate noise. The biases deliberately violate a zero-mean noise model.

Supplied exactly: x0 = 0 m. Estimated: x1, x2, A, and B together.

Supplied and estimated positions

Which readings disagree with the fit?

Unknown positions

4

Robot x1 (m)

2.133333

Robot x2 (m)

4.266667

Landmark A (m)

1.066667

Landmark B (m)

5.200000

Weighted squared residual

0.853333

Largest absolute normalized residual

0.533333

Robot RMSE against reference (m)

0.210819

Map RMSE against reference (m)

0.149071

The generated reference is x1 = 2 m, x2 = 4 m, A = 1 m, B = 5 m. Robot RMSE averages the two non-anchor robot errors; map RMSE averages the two landmark errors. Supplied exact values have zero error by construction. These three tasks are not a benchmark with equal information.

Start with Biased odometry and switch among the tasks. Localization supplies A and B and estimates x₁ = 2.008163, x₂ = 4.081633 m. Mapping supplies all robot positions and recovers A = 1, B = 5 m from the unbiased landmark readings.

Next choose Trust biased odometry. In SLAM, the smaller odometry scale moves x₂ to 4.784314 m. Weighted cost falls to 0.627451, even though robot RMSE rises from 0.210819 to 0.620054 m. The objective changed when its weights changed; those two costs cannot rank physical accuracy.

Finally, choose Biased landmark observation, then switch to mapping. The two implied B positions are 5 and 5.75 m, so their equal-weight average is 5.375 m. Exact robot poses do not remove bias in a landmark reading.

Interpret weights without inventing certainty

Under independent, zero-mean Gaussian measurement errors with known standard deviations, this weighted squared-residual objective corresponds to a Gaussian likelihood. The joint estimate also couples robot and landmark errors: moving a robot position changes the world position implied by its landmark observation. GTSAM's factor-graph tutorial develops the probabilistic formulation and covariance calculations.

The demo inserts deterministic biases into a fixed record. Its scale controls express the estimator's assumptions, not measured noise statistics. It therefore shows no calibrated confidence intervals. Six consistent readings could still share an unmodeled error.

The reference-error readouts are available because we generated the example. A robot operating in an unknown environment usually lacks that answer key. Residual checks can expose disagreement, but evaluating accuracy needs suitable independent reference information and a stated coordinate-frame alignment.

Place odometry and ICP in the SLAM pipeline

Wheel odometry supplies motion estimates between robot poses. ICP can estimate relative motion from overlapping scans. A SLAM estimator can combine these constraints with landmark observations or other measurements. The robotics book's PoseSLAM example makes that scan-registration connection explicit.

A new observation of a previously visited place can add a loop-closure constraint. Read SLAM loop closure to examine how that connection changes an estimated trajectory.

This demo supplies correspondences, uses a static map, and solves a small batch after every change. It omits rotations, moving objects, outliers, sensor timing, and correlated errors. Large SLAM systems need suitable representations and solvers for those conditions. Bailey and Durrant-Whyte's Part II tutorial discusses computation and data association beyond the basic estimation model.

The pose-graph optimization lesson adds headings and local-frame measurements to a small route. Inspect the joint nonlinear solve, its fixed first pose, and what happens when a supplied loop constraint is wrong.

Reproduce the estimates in Python

This standard-library example uses algebraic elimination for the same six readings. It is an independent closed-form calculation for this specific graph, not a general SLAM package. The browser solves the full weighted normal equations.

from math import hypot, sqrt


def joint(bias=0.4, last_bias=0.0, su=0.5, sz=0.25):
    # Relative landmark differences have squared scale 2 * sz**2.
    alpha = 2 * sz**2 / (su**2 + 2 * sz**2)
    x1 = 2 + alpha * bias
    x2 = x1 + 2 + alpha * bias - (1 - alpha) * last_bias
    a = x1 / 2
    b = (x1 + x2 + 4 + last_bias) / 2
    residuals = [(x1 - 2 - bias) / su,
                 (x2 - x1 - 2 - bias) / su,
                 (a - 1) / sz, (a - x1 + 1) / sz,
                 (b - x1 - 3) / sz, (b - x2 - 1 - last_bias) / sz]
    cost = sum(value**2 for value in residuals)
    error = hypot(x1 - 2, x2 - 4) / sqrt(2)
    return x1, x2, a, b, cost, error


x1, x2, a, b, cost, error = joint()
print(f"SLAM: x1={x1:.6f}, x2={x2:.6f}, A={a:.6f}, B={b:.6f}")
print(f"Weighted cost: {cost:.6f}")
print(f"Robot RMSE: {error:.6f} m")
# Known-map equations: 40*x1 - 4*x2 = 64; -4*x1 + 20*x2 = 73.6.
det = 40 * 20 - 4**2
print(f"Localization: x1={(20*64 + 4*73.6)/det:.6f}, "
      f"x2={(4*64 + 40*73.6)/det:.6f}")
print(f"Mapping with last bias 0.75: B={(5 + 5.75)/2:.6f}")
*_, cost, error = joint(su=0.1, sz=0.5)
print(f"Trust odometry: cost={cost:.6f}, robot RMSE={error:.6f} m")

Expected output:

SLAM: x1=2.133333, x2=4.266667, A=1.066667, B=5.200000
Weighted cost: 0.853333
Robot RMSE: 0.210819 m
Localization: x1=2.008163, x2=4.081633
Mapping with last bias 0.75: B=5.375000
Trust odometry: cost=0.627451, robot RMSE=0.620054 m

Try two estimation checks

Exercise 1. Keep the default readings, but set both noise scales to 0.5 m. In SLAM, what are x₁, x₂, A, and B? Remember that subtracting two independent landmark readings adds their variances.

Check the equal-scale estimate

Odometry's displacement weight is 4. The paired-landmark displacement weight is 1 / (2 × 0.5²) = 2. Each fitted move is (4 × 2.4 + 2 × 2) / 6 = 2.266667 m. Therefore x₁ = 2.266667, x₂ = 4.533333, A = 1.133333, and B = 5.4 m. Equal scales on individual readings do not give equal weights to one reading and a difference of two readings.

Exercise 2. Use mapping mode with b𝓏 = 0.75 m and σ𝓏 = 0.25 m. What is B? What are its two normalized residuals and their contribution to J? Does changing odometry's bias alter the result?

Check the biased-landmark estimate

B = (5 + 5.75) / 2 = 5.375 m. Its predicted readings are 3.375 and 1.375 m, compared with recorded readings 3 and 1.75 m. The residuals are +0.375 and −0.375 m, or +1.5 and −1.5 after division by 0.25 m. Their squared sum is 4.5. Mapping supplies the robot positions and ignores odometry, so changing its bias has no effect here.

Choose your next SLAM question

This route starts from the distinction between localization, mapping, and SLAM in this lesson. You should be comfortable reading a pose in a coordinate frame and comparing a predicted reading with a measurement. Each linked example supplies its own small dataset or model; you can follow the sequence without a robot or a recorded sensor run.

Work through these six questions in order, or go directly to the stage you need:

1. Where does the motion estimate come from? Use wheel odometry to turn wheel travel into a pose update and trace how calibration errors accumulate.
2. How can two scans suggest relative motion? Follow the correspondences and rigid-fit updates in the ICP algorithm. Run the same points with its downloadable Python exercise, then compare the correct pose with two misleading fits.
3. When is a good scan fit misleading? Test ICP failure cases with repeated geometry, partial overlap, and unmatched points before treating a fitted pose as trustworthy.
4. What changes when the robot revisits a place? Add a constraint in SLAM loop closure and inspect how it changes the estimated route.
5. How are the route's constraints solved together? Work through pose-graph optimization, including its fixed first pose and a wrong loop constraint.
6. How will you judge the resulting trajectory? Use trajectory error metrics to compare matched poses, choose an alignment, and distinguish full-path error from endpoint error. Run the timestamped CSV exercise to inspect a clock mistake that alignment conceals.

These are separate teaching experiments with stated assumptions. Together they help you inspect the stages of a SLAM system.

To run a mapping node, continue with the SLAM Toolbox tutorial. Its supplied laser scans and odometry first expose a missing transform, then produce a checked occupancy grid. That short ROS workflow has its own fixture; it does not combine the six examples above into a navigation stack.

For the uncertainty behind an estimate, start with covariance and uncertainty ellipses, then follow the Kalman filter through prediction and measurement updates. Particle-filter localization explores a different question: weighting candidate robot locations while holding the map fixed.

For another view of motion and map representations, compare visual odometry with SLAM, including the role of a metric reference for monocular scale. Then inspect occupancy-grid mapping, which estimates cell occupancy from range readings and supplied robot poses.

Sources and further study

• Dellaert and Hutchinson, Introduction to Robotics and Perception, Chapter 6.4: localization, mapping, planar poses, and ICP within PoseSLAM.
• GTSAM, Factor Graphs and GTSAM: probabilistic factors, pose priors, landmark SLAM, and uncertainty calculations.
• Bailey and Durrant-Whyte, Simultaneous Localisation and Mapping, Part II: computation and data association challenges beyond the supplied-correspondence model.