šÆ Problem Formulation
SLAM addresses the fundamental chicken-and-egg problem in robotics: to build an accurate map, we need to know the robot's location, but to determine the robot's location, we need an accurate map. Mathematically, SLAM seeks to estimate both the robot trajectory $\mathbf{X}_{0:t}$ and the map $\mathbf{M}$ given all observations $\mathbf{Z}_{1:t}$ and control inputs $\mathbf{U}_{1:t}$:
$$P(\mathbf{X}_{0:t}, \mathbf{M} | \mathbf{Z}_{1:t}, \mathbf{U}_{1:t})$$
Where:
- $\mathbf{X}_t = (x_t, y_t, \theta_t)$ represents the robot pose at time $t$ (position and orientation)
- $\mathbf{M} = \{m_1, m_2, \ldots, m_n\}$ represents the map containing landmark positions
- $\mathbf{Z}_t$ represents sensor observations at time $t$
- $\mathbf{U}_t$ represents control inputs (motion commands) at time $t$
š Motion Model Implementation
Our demo implements a simple differential drive motion model. When the robot moves forward, the new position is calculated using trigonometry:
$$\begin{align}
x_{t+1} &= x_t + \Delta s \cdot \cos(\theta_t) \\
y_{t+1} &= y_t + \Delta s \cdot \sin(\theta_t) \\
\theta_{t+1} &= \theta_t + \Delta\theta
\end{align}$$
In the demo code, this is implemented as:
const radians = (this.robotAngle - 90) * Math.PI / 180;
const deltaX = Math.cos(radians) * this.ROBOT_STEP_SIZE;
const deltaY = Math.sin(radians) * this.ROBOT_STEP_SIZE;
The motion model includes uncertainty, which in practice would be modeled as Gaussian noise:
$$\mathbf{X}_{t+1} = f(\mathbf{X}_t, \mathbf{U}_t) + \mathbf{w}_t$$
$$\text{where } \mathbf{w}_t \sim \mathcal{N}(0, \mathbf{Q}_t)$$
š” Sensor Model and Observations
The demo uses a simplified LiDAR sensor model that casts rays in multiple directions to detect obstacles. For each sensor beam at angle $\alpha$ relative to the robot's heading:
$$\begin{align}
x_{\text{beam}} &= x_{\text{robot}} + d \cdot \cos(\theta_{\text{robot}} + \alpha) \\
y_{\text{beam}} &= y_{\text{robot}} + d \cdot \sin(\theta_{\text{robot}} + \alpha)
\end{align}$$
The observation model relates sensor measurements to the true state:
$$\mathbf{Z}_t = h(\mathbf{X}_t, \mathbf{M}) + \mathbf{v}_t$$
$$\text{where } \mathbf{v}_t \sim \mathcal{N}(0, \mathbf{R}_t)$$
In our implementation, sensor rays are cast at multiple angles (-60Ā° to +60Ā° in 20Ā° increments), and obstacles are detected through collision detection:
for (let angle = -60; angle <= 60; angle += 20) {
const beamAngle = this.robotAngle + angle;
const hit = this.detectObstacleInDirection(beamAngle);
}
šøļø Pose Graph SLAM
Our demo implements a pose graph SLAM approach, where the robot's trajectory is represented as a graph of poses connected by constraints. Each node represents a robot pose, and edges represent spatial relationships:
š Graph Structure:
- Nodes: Robot poses $\mathbf{X}_i = (x_i, y_i, \theta_i)$
- Odometry Edges: Sequential motion constraints between consecutive poses
- Loop Closure Edges: Constraints when the robot revisits a previously mapped area
The objective is to find the set of poses that minimizes the total error across all constraints:
$$\mathbf{X}^* = \underset{\mathbf{X}}{\arg\min} \sum_{i,j} \|\mathbf{e}_{ij}(\mathbf{X}_i, \mathbf{X}_j)\|^2_{\boldsymbol{\Omega}_{ij}}$$
Where $\mathbf{e}_{ij}$ is the error function for the constraint between poses $i$ and $j$, and $\boldsymbol{\Omega}_{ij}$ is the information matrix (inverse of covariance).
š Loop Closure Detection
Loop closure occurs when the robot recognizes that it has returned to a previously visited location. Our demo implements a simple distance-based loop closure detection:
const dist = Math.hypot(currentPos.x - prevNode.x, currentPos.y - prevNode.y);
if (dist < this.LOOP_CLOSURE_THRESHOLD) {
return { from: currentNodeIndex, to: i };
}
In practice, loop closure detection uses more sophisticated methods:
- Feature matching: Comparing visual features between locations
- Place recognition: Using bag-of-words or deep learning methods
- Geometric verification: Verifying spatial consistency of matches
āļø Pose Graph Optimization
When a loop closure is detected, the entire pose graph must be optimized to correct accumulated drift. Our demo uses an iterative spring-mass optimization:
this.poseGraph.edges.forEach(edge => {
const dx = otherNode.x - node.x, dy = otherNode.y - node.y;
const distance = Math.hypot(dx, dy);
const diff = distance - edge.length;
const strength = edge.type === 'loop' ? 0.5 : 0.1;
forceX += (dx / distance) * diff * strength;
});
This simulates the Gauss-Newton or Levenberg-Marquardt optimization used in real SLAM systems:
$$\Delta\mathbf{x} = -(\mathbf{H} + \lambda\mathbf{I})^{-1} \cdot \mathbf{b}$$
Where $\mathbf{H}$ is the Hessian matrix, $\mathbf{b}$ is the gradient, and $\lambda$ is the damping parameter.
šŗļø Map Representation
Our demo stores discovered walls relative to their discovery poses, allowing the map to be corrected when poses are optimized:
this.discoveredWalls.push({
poseNodeIndex: currentPoseNode.id,
relativeX: hit.x - currentPoseNode.x,
relativeY: hit.y - currentPoseNode.y
});
When poses are optimized, map features are updated accordingly:
const correctedPose = this.poseGraph.nodes[wall.poseNodeIndex];
wall.el.style.left = (correctedPose.x + wall.relativeX) + '%';
š² Particle Filter Representation
The particle filter demonstration shows how SLAM can represent uncertainty in robot localization. Each particle represents a possible robot state:
$$\{\mathbf{X}_t^{(i)}, w_t^{(i)}\}_{i=1}^N$$
Where $\mathbf{X}_t^{(i)}$ is the $i$-th particle state and $w_t^{(i)}$ is its weight. The particles are updated through:
- Prediction: $\mathbf{X}_t^{(i)} = f(\mathbf{X}_{t-1}^{(i)}, \mathbf{U}_t) + \text{noise}$
- Update: $w_t^{(i)} \propto P(\mathbf{Z}_t | \mathbf{X}_t^{(i)}, \mathbf{M})$
- Resampling: Draw new particles proportional to weights
š§ Key Insights: This demo illustrates the core challenge of SLAM - simultaneously estimating robot poses and map features while handling uncertainty and correcting accumulated errors through loop closures. The pose graph approach provides a principled framework for global optimization that scales well to large environments.