Simultaneous Localization and Mapping (SLAM) addresses the central circular dependency of mobile robotics: an autonomous system must construct a geometrically consistent map of an uncharted workspace while simultaneously inferring its own continuous trajectory within that newly constructed map.
This problem creates an intrinsic paradox. Accurate metric mapping requires ground-truth knowledge of the sensor platform's position and orientation (pose) from which scans originate. Conversely, accurate pose estimation requires a reference map against which instantaneous sensor observations can be matched.
Robotics engineering resolves this paradox via probabilistic state estimation and non-linear factor graph optimization. Rather than presuming deterministic odometry or ideal exteroceptive sensing, modern SLAM maintains probability distributions over robot trajectories and environmental landmarks. By incorporating kinematic state transitions, sensor measurement noise models, and loop-closure constraints, SLAM progressively contracts estimation uncertainty, producing metric and topological consistency over long operational missions.
Today, SLAM underpins spatial awareness across autonomous vehicles, unmanned aerial vehicles (UAVs), warehouse autonomous mobile robots (AMRs), planetary rovers, and augmented reality (AR/VR) head-mounted displays operating in GNSS-denied settings.
SLAM in One Paragraph
An autonomous robot navigating an unknown spatial territory receives two complementary, noise-corrupted information streams: proprioceptive odometry (wheel encoders, inertial measurement units) and exteroceptive environmental measurements (planar LiDAR, stereo vision, depth cameras). Proprioceptive integration projects forward pose transitions but exhibits unbounded cumulative integration drift due to wheel slippage, terrain compliance, and gyro bias drift. Exteroceptive observations yield relative geometric transformations to spatial landmarks, subjected to sensor quantization, occlusion, and data association ambiguities. SLAM unifies these uncertain observations through recursive Bayesian filtering or non-linear least-squares optimization over a spatial factor graph. By detecting previously visited landmarks—termed loop closure—the system introduces global spatial constraints that distribute accumulated integration error back across the historical trajectory, resolving metric distortion and yielding a globally consistent map.
Interactive Quick Review: The SLAM Circular Dependency
Test your understanding of the localization-mapping paradox and probabilistic estimation foundations before exploring kinematic and sensor models:
Quick Review Verification
1. Why is dead reckoning (integrating IMU accelerations and wheel encoder counts) fundamentally incapable of sustaining long-term autonomous navigation without SLAM?
Answer: Dead reckoning is open-loop integration. Any minor systemic error (wheel radius tolerances, encoder quantization) or non-systemic disturbance (wheel slippage, floor irregularities, IMU gyro bias drift) accumulates unbounded over time. The estimated pose uncertainty grows monotonically without an upper bound until external landmark cross-checks or loop closures tie the robot state back to physical world anchors.
2. What is the fundamental mathematical distinction between "Online SLAM" and "Full SLAM"?
Answer: Online SLAM estimates the posterior probability distribution over only the current instantaneous robot pose xk and the map m, marginalizing out past poses: p(xk, m | z1:k, u1:k). Full SLAM (smoothing) estimates the joint posterior over the entire historical trajectory x0:k alongside the map: p(x0:k, m | z1:k, u1:k), which forms the basis of factor-graph bundle adjustment.
Formal Probabilistic Problem Formulation
Let the continuous trajectory, controls, and environmental parameters be represented in discrete time steps k ∈ {1, 2, ..., K}:
- xk ∈ SE(2) or SE(3): Robot kinematic pose vector (Cartesian position and orientation) at time step k.
- m = {m1, m2, ..., mN}: Map of the environment, parameterized as discrete feature landmarks or continuous volumetric representations.
- u1:k = {u1, u2, ..., uk}: Sequence of proprioceptive control vectors or odometry measurements.
- z1:k = {z1, z2, ..., zk}: Sequence of exteroceptive observations (range, bearing, visual descriptors).
The objective of Full SLAM is to compute the joint posterior density over the full trajectory history and the map:
$$
p(x_{0:k}, m \mid z_{1:k}, u_{1:k})
$$Applying Bayes' theorem and the first-order Markov assumption (stating that state xk depends conditionally only on xk-1 and uk, while observation zk depends conditionally only on xk and visible landmarks in m), the full joint posterior factors into:
$$
p(x_{0:k}, m \mid z_{1:k}, u_{1:k}) \propto p(x_0) \prod_{t=1}^k p(x_t \mid x_{t-1}, u_t) \prod_{t=1}^k p(z_t \mid x_t, m)
$$For Online SLAM, historical poses are integrated out recursively:
$$
p(x_k, m \mid z_{1:k}, u_{1:k}) = \eta \, p(z_k \mid x_k, m) \int p(x_k \mid x_{k-1}, u_k) \, p(x_{k-1}, m \mid z_{1:k-1}, u_{1:k-1}) \, dx_{k-1}
$$where η represents a normalizing constant.
Kinematic Motion Modeling & State Transitions
Actuator imperfections, gear backlash, and dynamic friction prevent exact deterministic state propagation. System kinematics are modeled stochastically via non-linear transition function f(·) corrupted by zero-mean Gaussian process noise wk ~ N(0, Qk):
$$
x_k = f(x_{k-1}, u_k) + w_k
$$Planar Differential Drive Kinematic Model
For a differential-drive robot operating on a 2D plane with state vector xk = [xk, yk, θk]T, commanded forward velocity vk, angular velocity ωk, and integration interval Δt:
$$
\begin{bmatrix} x_k \\ y_k \\ \theta_k \end{bmatrix} = \begin{bmatrix} x_{k-1} + v_k \cos(\theta_{k-1}) \Delta t \\ y_{k-1} + v_k \sin(\theta_{k-1}) \Delta t \\ \theta_{k-1} + \omega_k \Delta t \end{bmatrix} + w_k
$$For linearized estimators, the Jacobian of the motion dynamics evaluated with respect to the state vector is:
$$
F_x = \frac{\partial f}{\partial x} = \begin{bmatrix} 1 & 0 & -v_k \sin(\theta_{k-1}) \Delta t \\ 0 & 1 & v_k \cos(\theta_{k-1}) \Delta t \\ 0 & 0 & 1 \end{bmatrix}
$$Exteroceptive Measurement Models
Exteroceptive sensors measure the spatial relationship between current pose xk and landmark mj = [mj,x, mj,y]T, perturbed by Gaussian observation noise vk ~ N(0, Rk):
$$
z_k^j = h(x_k, m_j) + v_k
$$Non-Linear Range–Bearing Measurement
When planar LiDAR or radar registers radial range r and azimuth φ:
$$
z_k^j = \begin{bmatrix} r_k^j \\ \phi_k^j \end{bmatrix} = \begin{bmatrix} \sqrt{(m_{j,x}-x_k)^2 + (m_{j,y}-y_k)^2} \\ \arctan\left(\frac{m_{j,y}-y_k}{m_{j,x}-x_k}\right) - \theta_k \end{bmatrix} + v_k
$$Defining spatial offsets Δx = mj,x - xk and Δy = mj,y - yk with squared distance q = Δx2 + Δy2, the measurement Jacobians with respect to robot state (Hx) and landmark location (Hm) are:
$$
H_x = \begin{bmatrix} -\frac{\Delta x}{\sqrt{q}} & -\frac{\Delta y}{\sqrt{q}} & 0 \\ \frac{\Delta y}{q} & -\frac{\Delta x}{q} & -1 \end{bmatrix}, \quad H_m = \begin{bmatrix} \frac{\Delta x}{\sqrt{q}} & \frac{\Delta y}{\sqrt{q}} \\ -\frac{\Delta y}{q} & \frac{\Delta x}{q} \end{bmatrix}
$$Extended Kalman Filter SLAM (EKF-SLAM)
The Extended Kalman Filter SLAM algorithm maintains the mean and uncertainty of the system in an augmented state vector μk ∈ ℝ3+2N and joint covariance matrix Σk ∈ ℝ(3+2N) × (3+2N):
$$
\mu_k = \begin{bmatrix} x_k \\ m \end{bmatrix} = \begin{bmatrix} x_k \\ y_k \\ \theta_k \\ m_{1,x} \\ m_{1,y} \\ \vdots \\ m_{N,x} \\ m_{N,y} \end{bmatrix}, \quad \Sigma_k = \begin{bmatrix} \Sigma_{xx} & \Sigma_{xm_1} & \cdots & \Sigma_{xm_N} \\ \Sigma_{m_1 x} & \Sigma_{m_1 m_1} & \cdots & \Sigma_{m_1 m_N} \\ \vdots & \vdots & \ddots & \vdots \\ \Sigma_{m_N x} & \Sigma_{m_N m_1} & \cdots & \Sigma_{m_N m_N} \end{bmatrix}
$$Algorithmic Execution Loop
- Time Update (Prediction): The robot executes control uk. Only the vehicle sub-state undergoes translation; static landmark states remain fixed: $$ \mu_k^- = f(\mu_{k-1}, u_k), \quad \Sigma_k^- = F_k \Sigma_{k-1} F_k^T + G_k Q_k G_k^T $$
- Measurement Update (Correction): Upon observing landmark j, compute the innovation residual yk and innovation covariance Sk: $$ y_k = z_k^j - h(\mu_k^-), \quad S_k = H_k \Sigma_k^- H_k^T + R_k $$ Evaluate the Kalman Gain Kk and update state and covariance: $$ K_k = \Sigma_k^- H_k^T S_k^{-1}, \quad \mu_k = \mu_k^- + K_k y_k, \quad \Sigma_k = (I - K_k H_k) \Sigma_k^- $$
Scaling Characteristic: EKF-SLAM explicitly computes cross-correlations (Σxmj, Σmimj) across every landmark pair. As observations accumulate, correlation values approach unity, acting as a rigid structural frame. However, updating the dense covariance matrix requires O(N2) computation per time step, limiting pure EKF-SLAM to environments with fewer than roughly 1,000 landmarks.
Data Association & Ambiguity Management
Data association establishes correspondence between incoming feature observations zi and established map entries mj. In Euclidean feature spaces, the normalized Mahalanobis distance accounts for relative spatial covariance:
$$
d_{M}^2(i,j) = (z_i - \hat{z}_j)^T S_{ij}^{-1} (z_i - \hat{z}_j)
$$where Sij = Hj Σ- HjT + R represents the innovation covariance. Under the Nearest Neighbor (NN) decision rule:
$$
j^* = \arg\min_j d_{M}^2(i, j)
$$If dM2(i, j*) > γgate (a chi-squared gating threshold), the observation is classified as a new landmark. In cluttered or self-similar environments, Individual Compatibility Nearest Neighbor (ICNN) can trigger false positives; modern implementations employ Joint Compatibility Branch and Bound (JCBB) to evaluate the joint spatial configuration of all correspondences concurrently.
Graph-Based SLAM & Factor Graphs
Modern production SLAM systems (such as Cartographer, GTSAM, and ORB-SLAM) treat full trajectory estimation as a Non-linear Least Squares (NLLS) optimization problem over a factor graph.
In a factor graph:
- Variable Nodes represent poses along the trajectory X = {x0, x1, ..., xK} and landmark coordinates.
- Factor Nodes represent probabilistic constraints derived from odometry, visual feature tracks, or loop closures.
Let eij(xi, xj) = zij - ẑij(xi, xj) denote the vector error residual between relative pose measurement zij and its theoretical expectation. The global objective minimizes total squared Mahalanobis distance across all graph edges:
$$
J(X) = \sum_{(i,j) \in \mathcal{E}} e_{ij}(x_i, x_j)^T \Omega_{ij} e_{ij}(x_i, x_j)
$$where Ωij = Σij-1 denotes the information matrix of constraint (i, j).
Linearizing via Taylor expansion yields normal equations solved iteratively via Gauss-Newton or Levenberg-Marquardt:
$$
(H^T \Omega H) \Delta X = -H^T \Omega e
$$Sparsity Exploitation: Because spatial observations occur only between locally adjacent poses and visible landmarks, the information matrix Λ = HT Ω H is sparse. Applying sparse Cholesky or QR factorization solves for trajectory updates in near-linear time O(K), allowing graph SLAM to scale across tens of thousands of keyframes.
Loop Closure & Global Drift Elimination
Unchecked dead reckoning causes accumulated position and orientation drift over long trajectories. Loop closure occurs when an autonomous agent re-identifies a previously visited physical site through place recognition (such as LiDAR scan context or bag-of-visual-words image matching):
$$
\| x_k - x_i \| < \epsilon_{\text{radius}} \quad \text{and} \quad \text{Score}(I_k, I_i) > \tau_{\text{match}}
$$Detecting a valid loop closure introduces an edge constraint directly linking past node xi to current node xk. During subsequent factor graph optimization, the graph acts as an elastic spring network: minimizing the residual energy smoothly redistributes the accumulated rotational and translational drift across all intermediate nodes, aligning disparate map sections into a single coordinate frame.
Connection to Perception and Navigation
SLAM connects low-level hardware sensing to high-level deliberative control within the autonomy stack:
- Perception Inputs: Provides state transformations to de-skew high-frequency laser scans and project point clouds into persistent spatial frames, as discussed in sensors and perception.
- Downstream Navigation: Generates real-time localized pose estimates within static costmaps, enabling path planning algorithms (A*, Dijkstra, TEB Local Planner) and trajectory tracking in robot control systems.
Interactive Quick Review: Data Association, Graph SLAM & Loop Closure
Reinforce key concepts regarding data association ambiguity, graph sparsity, and loop closure error redistribution before reviewing the FAQ:
Quick Review Verification
1. Why does failure in data association cause catastrophic map divergence in EKF-SLAM?
Answer: EKF-SLAM maintains fully coupled cross-covariances across all state variables. Associating an observation with the wrong landmark injects a false innovation residual into the filter. Because all mapped features are correlated with vehicle pose, this error propagates across the entire covariance matrix, causing permanent, irreversible distortion of the map and loss of localization.
2. How does factor graph SLAM leverage matrix sparsity to outperform EKF computational efficiency?
Answer: Each pose in a factor graph connects only to immediate temporal neighbors and locally visible landmarks. Consequently, the information matrix Λ = HT Ω H contains non-zero entries only at local constraint pairings, leaving over 99% of the matrix zero. Sparse linear solvers (e.g., Sparse Cholesky) solve these sparse systems in roughly linear time O(K), avoiding the dense O(N2) covariance updates required by EKF-SLAM.
External References and Further Reading
To explore state estimation theory, factor graph optimization, and open-source SLAM frameworks, consult the following authoritative sources:
- GTSAM (Georgia Tech Smoothing and Mapping) – Open-source C++ framework utilizing factor graphs and Bayes trees for non-linear state estimation in robotics.
- Google Cartographer SLAM – Real-time 2D and 3D graph-based SLAM system providing submap matching and loop closure for mobile platforms.
- IEEE Transactions on Robotics (T-RO) – Premier international research journal detailing advanced visual-inertial odometry, semantic mapping, and multi-robot SLAM.
Frequently Asked Questions about SLAM
What is the difference between localization, mapping, and SLAM?
Localization estimates where a robot is situated assuming a trusted, pre-existing map is provided. Mapping models environmental geometry assuming true vehicle trajectory coordinates are known. SLAM solves both problems concurrently when neither prior spatial knowledge nor exact pose coordinates are available.
Why does EKF-SLAM exhibit quadratic computational complexity O(N2)?
In EKF-SLAM, the state vector maintains the coordinates of all N landmarks alongside the vehicle pose. Updating the fully coupled (3 + 2N) × (3 + 2N) covariance matrix requires updating every cross-covariance cell after every measurement, which scales quadratically with landmark count.
What is visual SLAM (vSLAM), and how does it compare to LiDAR SLAM?
Visual SLAM captures optical projections using monocular, stereo, or RGB-D cameras, tracking photometric gradients or geometric keypoints (ORB, SIFT) at low hardware cost and weight. LiDAR SLAM emits laser pulses to measure direct time-of-flight distances, producing accurate 3D metric point clouds that operate reliably in complete darkness and across textureless surfaces.
What causes accumulated trajectory drift in dead reckoning?
Dead reckoning estimates pose by integrating wheel encoder velocities or IMU accelerations over time (xk = f(xk-1, uk) + wk). Small systemic biases and stochastic noise terms (wk) compound through numerical integration, resulting in position and orientation errors that grow unbounded without external landmark corrections.
How does loop closure eliminate dead-reckoning drift?
When place recognition verifies that an agent has re-entered a previously visited area, it creates a rigid constraint between past and present graph nodes. Non-linear least-squares optimization then distributes accumulated drift across all intervening graph edges, restoring global geometric alignment.
How do SLAM systems address dynamic environmental obstacles?
Classic SLAM relies on a static world assumption. Moving objects (such as pedestrians or vehicles) violate this assumption and can introduce false spatial constraints. Modern systems apply semantic segmentation, deep neural networks, or geometric residual filtering to identify and mask dynamic returns before map integration.
What is the kidnapped robot problem?
The kidnapped robot problem involves transporting an active robot to an arbitrary, unknown location without supplying sensory feedback during movement. Because its prior belief state is incorrect, standard tracking filters can diverge. Resolving this requires global relocalization, typically executed via particle filter re-seeding or visual place recognition against a keyframe database.
What academic foundations are essential for advanced SLAM research?
Core foundations include multivariable calculus, linear algebra, probability theory, Lie groups/algebras (SO(3) and SE(3) manifolds), numerical optimization methods (Gauss-Newton, Levenberg-Marquardt), and data structures for sparse matrix computation.
End-of-Page Exercises: Review, Thought-Provoking & Engineering Problems
Review Questions and Answers
1. What objective probability distribution does SLAM compute simultaneously?
Show answer
Answer: The joint posterior probability distribution p(xk, m | z1:k, u1:k) for Online SLAM, or p(x0:k, m | z1:k, u1:k) for Full SLAM.
2. State the stochastic state transition equation for robot motion.
Show answer
Answer: xk = f(xk-1, uk) + wk, where wk ~ N(0, Qk) represents zero-mean Gaussian process noise.
3. What augmented structure forms the state vector in EKF-SLAM?
Show answer
Answer: A combined vector containing both vehicle pose and all landmark coordinates: μk = [xk, yk, θk, m1,x, m1,y, ..., mN,x, mN,y]T.
4. What is the operational objective of data association in SLAM?
Show answer
Answer: To unambiguously determine whether an incoming sensor return corresponds to an existing landmark in the map or represents a previously unobserved feature.
5. State the cost function minimized in graph-based SLAM optimization.
Show answer
Answer: J(X) = ∑ eijT Ωij eij, where eij represents the relative pose residual and Ωij is the information matrix.
6. What condition initiates a loop-closure constraint in a pose graph?
Show answer
Answer: Identifying that the robot has returned to a previously mapped location via spatial proximity (||xk - xi|| < ε) and positive place recognition matching.
7. What asymptotic growth rate governs covariance matrix updates in EKF-SLAM as landmarks N increase?
Show answer
Answer: Quadratic growth O(N2) due to the dense (3 + 2N) × (3 + 2N) covariance matrix.
8. What sensor modalities are most commonly combined in visual-inertial SLAM (VIO)?
Show answer
Answer: Monocular or stereo optical cameras paired with high-frequency Inertial Measurement Units (IMUs).
9. Why is factor graph optimization computationally faster than dense EKF filtering in large workspaces?
Show answer
Answer: By taking advantage of matrix sparsity in the information matrix Λ = HT Ω H, which can be factored in near-linear time O(K) using sparse numerical linear algebra.
10. Name five key technical disciplines underpinning the mathematical foundations of SLAM.
Show answer
Answer: Linear algebra, multivariable calculus, probability and Bayesian statistics, non-linear optimization, and graph theory.
Thought-Provoking Questions and Answers
1. Why does landmark cross-covariance in EKF-SLAM act as an unchangeable geometric scaffolding?
Show answer
Answer: Every observation of a landmark updates the vehicle pose and all previously mapped landmarks through their cross-covariance values. As the number of observations grows, these correlations strengthen monotonically. Once established, observing any landmark in the correlated network provides an update that constrains all other landmarks tied to it.
2. How does pose graph optimization model error distribution mechanically?
Show answer
Answer: A factor graph can be conceptualized as a physical network of rigid nodes connected by elastic springs, where spring stiffness corresponds to the measurement information matrix Ωij. When a loop closure connects two distant nodes, it introduces residual tension across the network. Minimizing the squared error function relaxes this network, distributing the strain smoothly across intermediate springs.
3. Why does non-linear range-bearing sensing prevent the use of standard linear Kalman filters?
Show answer
Answer: Standard Kalman filters require linear operations (Ax + Bu), which preserve Gaussian distributions. The range-bearing measurement equation involves non-linear trigonometric functions (arctan) and Euclidean norms (√(Δx2 + Δy2)). Propagating a Gaussian random variable through these non-linear transformations produces a non-Gaussian probability density, necessitating linearization (EKF) or optimization techniques.
4. Why do self-similar geometric environments (such as long, uniform hallways) induce degeneracy in SLAM?
Show answer
Answer: In an invariant corridor, LiDAR scans parallel to the walls detect zero longitudinal gradients, leaving the along-corridor translation component unconstrained. The information matrix develops near-zero eigenvalues along that axis, causing optimization routines to become ill-conditioned and allowing estimated positions to drift freely along the hallway direction.
5. Why is Rao-Blackwellized particle filtering (FastSLAM) able to decouple landmark updates?
Show answer
Answer: FastSLAM factors the joint posterior into a trajectory distribution and conditional landmark estimates: p(x0:k, m | z, u) = p(x0:k | z, u) ∏ p(mj | x0:k, z). Conditioning on a known trajectory candidate removes the conditional dependence between landmark coordinates, allowing each particle to maintain N separate, independent low-dimensional EKF updates rather than one large covariance matrix.
6. How does tightly-coupled Visual-Inertial Odometry (VIO) improve tracking robustness over loose coupling?
Show answer
Answer: In loosely-coupled architectures, visual poses and IMU estimates are calculated independently before filtering. Tightly-coupled systems optimize raw optical pixel errors alongside IMU acceleration and gyro residuals in a single joint cost function. This preserves full cross-covariance information and maintains tracking even when fewer than four visual features are visible.
7. Why do uncorrected rotational errors cause position errors that grow quadratically with distance?
Show answer
Answer: An orientation error Δθ incurred early in a trajectory introduces a cross-track displacement error e⊥ ≈ s · Δθ over subsequent travel distance s. Integrating this displacement over distance L produces a net lateral translation error proportional to ½ L2 Δθ, demonstrating quadratic error growth with path length.
8. What is the fundamental difference between topological mapping and metric mapping?
Show answer
Answer: Metric mapping captures exact geometric dimensions and obstacle boundaries within a global coordinate frame (e.g., occupancy grids or 3D point clouds). Topological mapping represents the workspace as a graph of distinctive places (nodes) and traversable pathways (edges), maintaining spatial connectivity without requiring global coordinate accuracy.
9. How does IMU pre-integration prevent recalculating motion constraints during factor graph optimization?
Show answer
Answer: Standard IMU integration requires re-integrating high-frequency inertial samples from scratch whenever previous pose estimates change. IMU pre-integration parameterizes relative motion in the local body frame between keyframes. This isolates inertial constraints from global coordinate updates, allowing linear bias corrections without re-integrating raw sensor frames.
10. Why are occupancy grid maps well-suited for collision avoidance in unstructured outdoor environments?
Show answer
Answer: Natural outdoor environments (tall grass, dirt mounds, uneven terrain) often lack clean, parameterized geometric primitives. Occupancy grids divide space into discrete probabilistic cells that model occupancy evidence directly from raw range returns, providing robust spatial representation without requiring distinct geometric features.
11. How does Robust Kernel estimation (e.g., Huber or Cauchy) protect pose graphs from false loop closures?
Show answer
Answer: Standard squared error penalties (e2) increase quadratically with error size, allowing a single false loop-closure outlier to dominate the optimization cost. Robust loss functions scale linearly or sub-linearly beyond a chosen threshold, dampening the influence of outlier residuals and preventing graph distortion.
12. What economic advantages do autonomous mobile robots using SLAM offer over Automated Guided Vehicles (AGVs)?
Show answer
Answer: AGVs depend on fixed physical infrastructure, such as embedded floor magnetic strips, painted guidelines, or surveyed reflective targets. SLAM-enabled AMRs navigate dynamically using onboard sensors, map facilities autonomously, dynamically route around unexpected obstacles, and adapt to revised floorplans through software updates alone.
Numerical Engineering Problems & Solutions
Problem 1: Differential Drive Kinematic Pose Propagation
An autonomous ground robot begins at pose x0 = [1.0 m, 2.0 m, 0.0 rad]T. Over an interval Δt = 0.4 s, it commands linear velocity v = 1.5 m/s and yaw rate ω = 0.5 rad/s. Using differential-drive forward kinematics, calculate the updated pose vector [x1, y1, θ1]T.
An autonomous ground robot begins at pose x0 = [1.0 m, 2.0 m, 0.0 rad]T. Over an interval Δt = 0.4 s, it commands linear velocity v = 1.5 m/s and yaw rate ω = 0.5 rad/s. Using differential-drive forward kinematics, calculate the updated pose vector [x1, y1, θ1]T.
Show answer
Solution:
- Update x: x1 = x0 + v cos(θ0) Δt = 1.0 + (1.5 × cos(0.0) × 0.4) = 1.0 + 0.60 = 1.60 m.
- Update y: y1 = y0 + v sin(θ0) Δt = 2.0 + (1.5 × sin(0.0) × 0.4) = 2.0 + 0.0 = 2.00 m.
- Update heading θ: θ1 = θ0 + ω Δt = 0.0 + (0.5 × 0.4) = 0.20 rad (≈ 11.46°).
Answer: Updated robot pose is x1 = [1.60 m, 2.00 m, 0.20 rad]T.
Problem 2: Range-Bearing Sensor Model Evaluation
A vehicle is localized at pose xk = [3.0 m, 4.0 m, 0.0 rad]T. A landmark is situated at coordinates mj = [6.0 m, 8.0 m]T. Determine the expected range r and bearing angle φ recorded by an onboard sensor.
A vehicle is localized at pose xk = [3.0 m, 4.0 m, 0.0 rad]T. A landmark is situated at coordinates mj = [6.0 m, 8.0 m]T. Determine the expected range r and bearing angle φ recorded by an onboard sensor.
Show answer
Solution:
- Calculate coordinate offsets: Δx = 6.0 - 3.0 = 3.0 m, Δy = 8.0 - 4.0 = 4.0 m.
- Calculate range r: r = √(Δx2 + Δy2) = √(3.02 + 4.02) = √(9 + 16) = √25 = 5.0 m.
- Calculate relative azimuth φ: φ = arctan(Δy / Δx) - θk = arctan(4.0 / 3.0) - 0.0 ≈ 0.9273 rad (53.13°).
Answer: Expected observation is zkj = [5.0 m, 0.927 rad]T.
Problem 3: EKF-SLAM Covariance Dimensionality
An EKF-SLAM system tracks a planar robot alongside N = 150 point landmarks. Calculate the dimension of the complete state vector μ, the total size of the joint covariance matrix Σ, and the total number of floating-point values stored in Σ.
An EKF-SLAM system tracks a planar robot alongside N = 150 point landmarks. Calculate the dimension of the complete state vector μ, the total size of the joint covariance matrix Σ, and the total number of floating-point values stored in Σ.
Show answer
Solution:
- State vector length: dim(μ) = 3 + 2N = 3 + 2(150) = 303 variables.
- Covariance matrix dimensions: 303 × 303.
- Total stored scalar elements: 3032 = 91,809 floating-point values.
Answer: State vector dimension is 303, producing a 303 × 303 covariance matrix with 91,809 elements.
Problem 4: Nearest Neighbor Data Association via Euclidean Distance
An incoming feature observation has estimated coordinates zi = [12.5, 5.2]T. The existing map contains three landmark candidates: m1 = [12.0, 5.0]T, m2 = [15.0, 8.0]T, and m3 = [10.0, 3.0]T. Compute the squared Euclidean distance to each candidate and select the nearest-neighbor match.
An incoming feature observation has estimated coordinates zi = [12.5, 5.2]T. The existing map contains three landmark candidates: m1 = [12.0, 5.0]T, m2 = [15.0, 8.0]T, and m3 = [10.0, 3.0]T. Compute the squared Euclidean distance to each candidate and select the nearest-neighbor match.
Show answer
Solution:
- Distance to m1: (12.5 - 12.0)2 + (5.2 - 5.0)2 = 0.52 + 0.22 = 0.25 + 0.04 = 0.29.
- Distance to m2: (12.5 - 15.0)2 + (5.2 - 8.0)2 = (-2.5)2 + (-2.8)2 = 6.25 + 7.84 = 14.09.
- Distance to m3: (12.5 - 10.0)2 + (5.2 - 3.0)2 = 2.52 + 2.22 = 6.25 + 4.84 = 11.09.
- Identify minimum value: min(0.29, 14.09, 11.09) = 0.29.
Answer: Squared distances are 0.29 (m1), 14.09 (m2), and 11.09 (m3). The nearest-neighbor match is Landmark 1.
Problem 5: Loop Closure Proximity Gating Verification
A vehicle's current position estimate is xk = [15.4 m, 22.1 m]T. A candidate node in historical logs is located at xi = [15.1 m, 21.9 m]T. Calculate the Euclidean distance ||xk - xi|| and determine if a candidate loop closure is triggered under threshold ε = 0.50 m.
A vehicle's current position estimate is xk = [15.4 m, 22.1 m]T. A candidate node in historical logs is located at xi = [15.1 m, 21.9 m]T. Calculate the Euclidean distance ||xk - xi|| and determine if a candidate loop closure is triggered under threshold ε = 0.50 m.
Show answer
Solution:
- Calculate coordinate deltas: Δx = 15.4 - 15.1 = 0.3 m, Δy = 22.1 - 21.9 = 0.2 m.
- Compute distance: d = √(0.32 + 0.22) = √(0.09 + 0.04) = √0.13 ≈ 0.3606 m.
- Evaluate gating inequality: 0.3606 m ≤ 0.50 m.
Answer: The distance is 0.36 m, satisfying the threshold (< 0.50 m) to trigger candidate loop closure verification.
Problem 6: Factor Edge Mahalanobis Cost Computation
An optimization factor edge connects two poses with residual error vector eij = [0.05 m, -0.02 m]T and diagonal information matrix Ωij = diag(400, 400) m-2. Calculate the scalar error cost contribution Jij = eijT Ωij eij.
An optimization factor edge connects two poses with residual error vector eij = [0.05 m, -0.02 m]T and diagonal information matrix Ωij = diag(400, 400) m-2. Calculate the scalar error cost contribution Jij = eijT Ωij eij.
Show answer
Solution:
- Compute matrix-vector product: Ωij eij = [400 × 0.05, 400 × (-0.02)]T = [20.0, -8.0]T.
- Compute dot product: eijT (Ωij eij) = (0.05 × 20.0) + (-0.02 × -8.0) = 1.0 + 0.16 = 1.16.
Answer: The factor edge cost contribution is Jij = 1.16.
Problem 7: Odometric Dead-Reckoning Systematic Drift
A differential wheeled robot drives down a planned 100.0 m linear path. Wheel slippage causes an ongoing systemic under-reporting drift of 1.5% of distance traveled. Calculate the net position discrepancy Δd accumulated over the traversal.
A differential wheeled robot drives down a planned 100.0 m linear path. Wheel slippage causes an ongoing systemic under-reporting drift of 1.5% of distance traveled. Calculate the net position discrepancy Δd accumulated over the traversal.
Show answer
Solution:
- Calculate drift percentage: 1.5% = 0.015.
- Evaluate translation discrepancy: Δd = 100.0 m × 0.015 = 1.50 m.
Answer: The accumulated linear dead-reckoning drift is 1.50 m.
Problem 8: Discrete Process Covariance Propagation
During an EKF prediction step, initial pose covariance is P0 = 0.01 I3. Process noise is Q = 0.005 I3. Assuming an identity state transition Jacobian (A = I3), calculate the predicted covariance matrix P1|0 = A P0 AT + Q.
During an EKF prediction step, initial pose covariance is P0 = 0.01 I3. Process noise is Q = 0.005 I3. Assuming an identity state transition Jacobian (A = I3), calculate the predicted covariance matrix P1|0 = A P0 AT + Q.
Show answer
Solution:
- Evaluate product: A P0 AT = I3 (0.01 I3) I3 = 0.01 I3.
- Add process noise: 0.01 I3 + 0.005 I3 = 0.015 I3.
Answer: Predicted covariance matrix is P1|0 = 0.015 I3.
Problem 9: Angular Heading Gyro Bias Drift Accumulation
A solid-state MEMS gyroscope exhibits an uncalibrated constant bias drift of Δω = 0.02 rad/s. Calculate the net rotational heading error Δθ accumulated over T = 300 s (5 minutes) of navigation, converting the final value to degrees.
A solid-state MEMS gyroscope exhibits an uncalibrated constant bias drift of Δω = 0.02 rad/s. Calculate the net rotational heading error Δθ accumulated over T = 300 s (5 minutes) of navigation, converting the final value to degrees.
Show answer
Solution:
- Calculate angular error: Δθ = Δω × T = 0.02 rad/s × 300 s = 6.0 rad.
- Convert to degrees: 6.0 rad × (180° / π) ≈ 6.0 × 57.2958° ≈ 343.78°.
Answer: The uncorrected heading drift is 6.0 rad (343.78°).
Problem 10: 2D Occupancy Grid Map Memory Allocation
An occupancy grid mapping algorithm covers a 50 m × 50 m area with a spatial resolution of 0.05 m (5 cm) per cell. Calculate cell count per axis, total map cells, and the memory footprint in Megabytes (MB) if each cell stores an 8-bit (1 byte) log-odds value (1 MB = 1,000,000 bytes).
An occupancy grid mapping algorithm covers a 50 m × 50 m area with a spatial resolution of 0.05 m (5 cm) per cell. Calculate cell count per axis, total map cells, and the memory footprint in Megabytes (MB) if each cell stores an 8-bit (1 byte) log-odds value (1 MB = 1,000,000 bytes).
Show answer
Solution:
- Axis cell count: 50.0 m / 0.05 m = 1,000 cells per axis.
- Total grid cells: 1,000 × 1,000 = 1,000,000 cells.
- Memory required: 1,000,000 cells × 1 byte/cell = 1,000,000 bytes = 1.0 MB.
Answer: The occupancy grid consists of 1,000,000 cells and requires 1.0 MB of RAM.
Problem 11: Particle Filter Weight Normalization
A particle filter maintains four trajectory hypotheses with raw measurement importance weights: w1 = 0.10, w2 = 0.40, w3 = 0.30, and w4 = 0.20. Calculate the normalization sum ∑ wi and the resulting normalized particle probability distribution.
A particle filter maintains four trajectory hypotheses with raw measurement importance weights: w1 = 0.10, w2 = 0.40, w3 = 0.30, and w4 = 0.20. Calculate the normalization sum ∑ wi and the resulting normalized particle probability distribution.
Show answer
Solution:
- Calculate sum: ∑ wi = 0.10 + 0.40 + 0.30 + 0.20 = 1.00.
- Normalize individual weights: w̃i = wi / 1.00.
- w̃1 = 0.10 (10%)
- w̃2 = 0.40 (40%)
- w̃3 = 0.30 (30%)
- w̃4 = 0.20 (20%)
Answer: Normalized weights are w̃1 = 0.10, w̃2 = 0.40, w̃3 = 0.30, w̃4 = 0.20.
Problem 12: Information Matrix Sparsity Evaluation
A pose graph contains K = 1,000 pose nodes, with each node connected by spatial constraints to an average of 4 adjacent nodes. Calculate the total elements in an equivalent dense K × K adjacency matrix, the number of non-zero constraint entries (approx. 4 × K), and the sparsity percentage of the matrix.
A pose graph contains K = 1,000 pose nodes, with each node connected by spatial constraints to an average of 4 adjacent nodes. Calculate the total elements in an equivalent dense K × K adjacency matrix, the number of non-zero constraint entries (approx. 4 × K), and the sparsity percentage of the matrix.
Show answer
Solution:
- Total elements in dense matrix: 1,000 × 1,000 = 1,000,000 cells.
- Non-zero constraint entries: 4 × 1,000 = 4,000 off-diagonal elements.
- Zero elements: 1,000,000 - 4,000 = 996,000 cells.
- Compute sparsity: (996,000 / 1,000,000) × 100 = 99.60%.
Answer: The information matrix contains 4,000 non-zero entries, yielding a sparsity of 99.60%.