Perception turns raw sensor streams into a model of the robot and its surroundings. SLAM (Simultaneous Localization and Mapping) is the core problem when no prior map exists: the robot must estimate its own pose using a map that it is building from those same pose estimates. This page covers state estimation with Kalman filters, the SLAM formulation, visual and LiDAR SLAM pipelines, and practical pitfalls.
No single sensor is enough. IMUs are fast but drift when integrated; wheel odometry slips; cameras fail in low light or texture-less scenes; LiDAR is precise but sparse and costly; GNSS is absolute but unavailable indoors. A probabilistic estimator combines them, weighting each by its noise model, and tracks uncertainty as a covariance matrix.
For a linear system xk = F xk-1 + B uk + w, zk = H xk + v with Gaussian noise w ~ N(0,Q), v ~ N(0,R), the Kalman filter is the optimal estimator:
x̂ = F x̂ + B u, P = F P FT + QK = P HT(H P HT + R)-1, x̂ = x̂ + K(z − H x̂), P = (I − K H) PThe Extended Kalman Filter (EKF) linearizes nonlinear models with Jacobians; the Unscented KF propagates sigma points instead; particle filters represent multimodal beliefs with samples (used in Monte Carlo localization against a known map).
import numpy as np
dt = 0.1
F = np.array([[1, dt], [0, 1]]) # state: [position, velocity]
H = np.array([[1.0, 0.0]]) # we measure position only
Q = np.diag([1e-4, 1e-2]) # process noise
R = np.array([[0.5]]) # measurement noise (variance)
x = np.zeros(2); P = np.eye(2)
rng = np.random.default_rng(0)
true_x, est = np.array([0.0, 1.0]), []
for k in range(100):
true_x = F @ true_x
z = H @ true_x + rng.normal(0, np.sqrt(R[0, 0]), 1)
x = F @ x; P = F @ P @ F.T + Q # predict
S = H @ P @ H.T + R; K = P @ H.T @ np.linalg.inv(S) # gain
x = x + K @ (z - H @ x); P = (np.eye(2) - K @ H) @ P # update
est.append(x.copy())
print("final estimate", est[-1], "truth", true_x) # velocity recovered without measuring itSLAM estimates the robot trajectory x0:t and map m from controls and observations: p(x0:t, m | z1:t, u1:t). Early approaches used EKF-SLAM (a state vector holding all landmarks, quadratic cost) and FastSLAM (particle filter). Modern systems use graph-based SLAM: nodes are poses (and optionally landmarks), edges are relative constraints from odometry and observations, and a sparse nonlinear least-squares solver (g2o, GTSAM, Ceres) finds the configuration that best satisfies all of them.
Loop closure — recognizing a previously visited place — adds a constraint that corrects the drift accumulated along the loop. It is also the most dangerous step: one false loop closure can corrupt the whole map, so robust kernels and geometric verification are standard.
Visual SLAM systems such as ORB-SLAM track sparse features; direct methods (LSD-SLAM, DSO) use pixel intensities. Monocular cameras cannot observe metric scale, so they are usually fused with an IMU (visual-inertial odometry, e.g. VINS-Mono, OKVIS) or replaced by stereo / RGB-D. LiDAR SLAM (LOAM, LIO-SAM, Cartographer) registers point-cloud scans using ICP or feature-based matching and is the default on autonomous vehicles and outdoor robots.
Learned components increasingly replace hand-crafted ones: learned keypoints and matchers, monocular depth networks, and place-recognition embeddings for loop closure. Neural scene representations (NeRF, 3D Gaussian Splatting) are being used for dense mapping.
Consider a robot moving along a line with state x = position. Its belief is x̂ = 0.0 m with variance P = 1.0 m2. It commands a 1.0 m move; odometry adds process noise Q = 0.5. Predict: x̂ = 0 + 1.0 = 1.0, P = 1.0 + 0.5 = 1.5. A range sensor then measures the position as z = 1.4 with noise R = 0.5 (H = 1). Update: the Kalman gain is K = P / (P + R) = 1.5 / 2.0 = 0.75. The new estimate is x̂ = 1.0 + 0.75 × (1.4 − 1.0) = 1.3, and the variance shrinks to P = (1 − 0.75) × 1.5 = 0.375.
The gain expresses relative trust: because the prediction had become more uncertain than the sensor, the estimate moved three-quarters of the way toward the measurement. Repeating predict and update keeps variance bounded as long as measurements keep arriving; without them it grows with every step, which is drift. In multi-dimensional problems the same equations run on vectors and covariance matrices, and the innovation z − Hx̂ with its covariance is also used to reject outliers: a measurement with a large Mahalanobis distance is gated out instead of being fused.
Practical SLAM systems split into two halves. The front end is sensor-specific: it detects and matches features or registers point clouds (ICP, NDT), estimates frame-to-frame motion, decides when to create a keyframe, and runs place recognition, for example bag-of-words image retrieval or LiDAR scan descriptors, to propose loop closures. The back end is sensor-agnostic: it treats every relative measurement as a constraint with an information matrix and solves for the poses that minimize the total weighted squared error, using Gauss-Newton or Levenberg-Marquardt on a sparse problem. Bundle adjustment is the same idea for cameras, jointly refining poses and 3-D points by minimizing reprojection error. Incremental solvers such as iSAM2 update the solution as new nodes arrive instead of solving from scratch.
| Setting | Common choice | Watch out for |
|---|---|---|
| Indoor mobile robot, 2-D | 2-D LiDAR with a grid-based SLAM (Cartographer, slam_toolbox), then AMCL localization | Glass, mirrors, long featureless corridors |
| Drone or handheld device | Visual-inertial odometry (camera + IMU) | Motion blur, low light, IMU calibration and time sync |
| Outdoor vehicle | 3-D LiDAR + IMU (LIO-SAM style), fused with GNSS | Dynamic objects, rain and dust, GNSS multipath |
| AR on phones | Platform VIO (ARKit, ARCore) | Scale drift without depth, relocalization after tracking loss |
Trajectory accuracy is measured against ground truth from motion capture or survey-grade GNSS. Absolute trajectory error aligns the estimated and true paths and reports the RMSE of position differences; relative pose error measures drift over fixed distances or time windows and is less dominated by a single bad segment. Public datasets such as KITTI, EuRoC and TUM RGB-D allow comparison, with tools like evo for computing the metrics. Beyond accuracy, track CPU load and latency, tracking-loss rate, and how often relocalization succeeds.
In deployment, most failures trace back to calibration and timing: incorrect camera intrinsics, camera-to-IMU extrinsics, or unsynchronized timestamps degrade every estimate downstream. Recalibrate after hardware changes, log raw sensor data so failures can be replayed offline, and treat long-term map maintenance (furniture moves, seasons change) as an explicit problem rather than assuming a map built once stays valid. In ROS 2, the robot_localization package provides EKF/UKF fusion and Nav2 consumes the resulting pose for path planning and control.
Deep learning now supplies many front-end components: learned feature detectors and matchers (SuperPoint, SuperGlue), monocular depth estimation, semantic segmentation that labels map points as road, building or person so that moving objects can be excluded, and object detection feeding semantic maps. End-to-end learned odometry exists but classical geometric back ends remain the norm for reliability and interpretability. Neural scene representations (NeRF, 3-D Gaussian splatting) are an active research area for dense mapping.
Further reading. Thrun, Burgard and Fox (2005), Probabilistic Robotics; Kalman (1960), "A New Approach to Linear Filtering and Prediction Problems"; Durrant-Whyte and Bailey (2006), "Simultaneous Localization and Mapping: Part I", IEEE Robotics and Automation Magazine; Cadena et al. (2016), "Past, Present, and Future of Simultaneous Localization and Mapping", arXiv:1606.05830; Mur-Artal, Montiel and Tardós (2015), "ORB-SLAM"; Grisetti et al. (2010), "A Tutorial on Graph-Based SLAM"; Kaess et al. (2012), "iSAM2".