1. Localization Algorithms
Estimating robot 2D/3D state (x, y, theta, v) from noisy sensors:
- Extended Kalman Filter (EKF) Localization: Non-linear motion models linearized via Jacobian matrices. Uses speed and yaw rate inputs with range-bearing landmark measurements.
- Particle Filter (Monte Carlo Localization): Non-parametric Bayesian filter using weighted particles to represent arbitrary, multi-modal probability density distributions.
- Unscented Kalman Filter (UKF): Uses deterministic sigma points through non-linear transformations for higher accuracy than EKF without computing analytical Jacobians.
2. SLAM (Simultaneous Localization & Mapping)
- Iterative Closest Point (ICP): Rigid point-cloud matching algorithm to align successive 2D LiDAR scans.
- EKF SLAM: Simultaneously estimates the robot state vector augmented with Cartesian landmark coordinate positions.
- FastSLAM 1.0 & 2.0: Rao-Blackwellized particle filter combining particle-based trajectory estimation with independent 2D landmark Kalman filters.
- Graph-Based SLAM: Poses represented as graph vertices with spatial constraints as edges; solved via non-linear least-squares optimization.
3. Path Planning & Navigation
Sampling-Based Planning (RRT, RRT*, Informed RRT*)
Rapidly-exploring Random Trees incrementally build trees toward randomly sampled free-space points. RRT* adds rewiring loops to guarantee asymptotic optimality. Informed RRT* restricts sampling to an ellipse defined by the best cost found so far.
Python (Informed RRT* Elliptical Sampling Concept)
import numpy as np
def sample_in_ellipse(c_best, c_min, x_start, x_goal):
if c_best < float("inf"):
r1 = c_best / 2.0
r2 = np.sqrt(c_best**2 - c_min**2) / 2.0
L = np.diag([r1, r2])
theta = 2.0 * np.pi * np.random.random()
r = np.random.random()
x_ball = np.array([r * np.cos(theta), r * np.sin(theta)])
d_x = x_goal - x_start
C = np.array([[d_x[0], -d_x[1]], [d_x[1], d_x[0]]]) / c_min
x_center = (x_start + x_goal) / 2.0
return C @ L @ x_ball + x_center
return np.random.uniform(-10, 10, size=2)
Graph Search: A*, D* Lite, Voronoi Roadmaps
Optimal discrete grid planning with admissible heuristic cost estimates. D* Lite dynamically replans when unmapped obstacles appear.
4. Path Tracking & Motion Control
- Pure Pursuit: Geometrically calculates the steering curvature needed to reach a lookahead point on the path ahead.
- Stanley Controller: Front-axle steering controller combining heading error with cross-track error.
- Model Predictive Control (MPC): Solves a constrained finite-horizon quadratic optimization problem over future timesteps to follow paths smoothly within motor torque and acceleration limits.