# Algorithms

Robotics systems rely on a rich toolbox of algorithms for sensing, estimation, control, planning and perception. Below is a comprehensive survey-organized by function-of core methods used in modern robots, with links to further reading.

### Filtering & State Estimation <a href="#filtering--state-estimation" id="filtering--state-estimation"></a>

![](/files/kljwfR78blQ6yS4qq6KH)

<figure><img src="/files/euwJS3Xk0r25EzWNdmC8" alt=""><figcaption><p>Kalman FIlter</p></figcaption></figure>

* **Kalman Filter**: Optimal linear estimator for fusing motion models and noisy measurements (e.g., IMU + odometry)\
  <https://www.vaia.com/en-us/explanations/engineering/robotics-engineering/robotics-algorithms/>
* **Extended Kalman Filter (EKF)**: Nonlinear extension of the Kalman Filter for mapping and SLAM\
  <https://www.mdpi.com/2504-446X/7/6/339>
* **Unscented Kalman Filter (UKF)**: Deterministic sampling approach to nonlinear state estimation\
  <https://www.mdpi.com/2504-446X/7/6/339>
* **Particle Filter / Monte Carlo Localization**: Uses a set of random samples (particles) to represent arbitrary distributions for robot pose\
  <https://arxiv.org/pdf/1301.0607.pdf>
* **FastSLAM / Rao–Blackwellized Particle Filter**: Combines particle filters with per-feature Kalman filters for efficient SLAM\
  <https://arxiv.org/pdf/1301.0607.pdf>
* **Moving Average & Savitzky–Golay Filters**: Simple smoothing techniques for sensor signal conditioning
* **Butterworth & Chebyshev Filters**: Frequency-domain filters for removing high-frequency noise

### Control & Trajectory Tracking <a href="#control--trajectory-tracking" id="control--trajectory-tracking"></a>

<figure><img src="/files/GZmvXRPNXm4PZvJlTknR" alt=""><figcaption></figcaption></figure>

<figure><img src="/files/fOXXVtKvF3bGlbtbQZcb" alt=""><figcaption><p>PID Based Balancer</p></figcaption></figure>

* **PID (Proportional–Integral–Derivative)**: Ubiquitous feedback controller for setpoint tracking
* **LQR (Linear Quadratic Regulator)**: Optimal state-feedback controller minimizing a quadratic cost
* **MPC (Model Predictive Control)**: Online optimization of control signals subject to constraints
* **H∞ Control**: Robust control design against modeling uncertainties
* **Feedforward / Inverse Dynamics**: Calculates required joint torques for planned accelerations

### Path Planning & Navigation <a href="#path-planning--navigation" id="path-planning--navigation"></a>

<figure><img src="/files/FHana4kEw5C0A36WUEwP" alt=""><figcaption></figcaption></figure>

<figure><img src="/files/sm8XhHWgFam9ZuIpnE5P" alt=""><figcaption><p>A* Algorithm</p></figcaption></figure>

* **Dijkstra’s Algorithm**: Guaranteed shortest paths on weighted graphs
* **A★ (A-Star)**: Heuristic search combining Dijkstra with best-first bias for efficient grid and graph planning\
  <https://roboticsbiz.com/path-planning-algorithms-for-robotic-systems/>
* **D★ / D★ Lite**: Incremental replanning in dynamic or partially known environments\
  <https://roboticsbiz.com/path-planning-algorithms-for-robotic-systems/>
* **RRT (Rapidly-Exploring Random Trees)**: Sampling-based planner for high-dimensional spaces\
  <https://roboticsbiz.com/path-planning-algorithms-for-robotic-systems/>
* **PRM (Probabilistic Roadmap)**: Builds a graph of collision-free configurations offline for query planning
* **RRT★ / PRM★**: Asymptotically optimal variants of RRT and PRM
* **Genetic Algorithms**: Evolutionary search for path optimization in complex cost landscapes
* **Ant Colony Optimization**: Swarm-intelligence-inspired heuristic for shortest path discovery\
  <https://roboticsbiz.com/path-planning-algorithms-for-robotic-systems/>
* **Firefly & Particle Swarm Optimization**: Nature-inspired metaheuristics for global path and parameter tuning

### SLAM & Mapping <a href="#slam--mapping" id="slam--mapping"></a>

<figure><img src="/files/qrRUEuguZokPqLwCvakf" alt=""><figcaption></figcaption></figure>

<figure><img src="/files/AcmWUvLSRdrEfCpOId4D" alt=""><figcaption><p>GRAPH SLAM</p></figcaption></figure>

<figure><img src="/files/Z95rr5YdCDbZ2Yj3FsaH" alt=""><figcaption><p>ORB SLAM</p></figcaption></figure>

* **EKF-SLAM**: Maps landmarks with Gaussian estimates via EKF
* **Graph-SLAM**: Poses and landmarks jointly optimized in a large factor graph
* **FastSLAM**: Leverages particle filters for robot pose and separate Kalman filters for landmarks\
  <https://arxiv.org/pdf/1301.0607.pdf>
* **ORB-SLAM / LSD-SLAM**: Visual SLAM systems using ORB features or direct image alignment

### Optimization & Numerical Methods <a href="#optimization--numerical-methods" id="optimization--numerical-methods"></a>

* **Gauss–Newton & Levenberg–Marquardt**: Nonlinear least-squares solvers for bundle adjustment, calibration, and pose-graph optimization. See [GO-SLAM](/robotics-handbook/authors-projects/go-slam.md) for an implementation from scratch.
* **Gradient Descent & Stochastic Gradient Descent**: Iterative minimization for learning and control parameter tuning
* **Convex Optimization (CVX, CVXPY)**: Fast solvers for quadratic and semidefinite programs in control and state estimation
* **Trajectory Optimization (iLQR, DDP)**: Nonlinear optimization of state-action trajectories - the math behind MPC for robotics
* **Constraint Programming (CP-SAT, OR-Tools)**: Discrete optimization for task sequencing, scheduling.
* **Factor Graphs (g2o, GTSAM, Ceres)**: Library-backed graph optimization - the modern way to solve SLAM, calibration, and sensor fusion. See [Optimization Libraries](/robotics-handbook/programming-for-robotics/optimization-libraries.md).

### Pseudocode: Core Algorithms

**A\* search** (grid path planning):

```python
def a_star(start, goal, neighbors_fn, h_fn):
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from, g_score = {}, {start: 0}
    while not open_set.empty():
        _, current = open_set.get()
        if current == goal:
            return reconstruct_path(came_from, current)
        for nb, cost in neighbors_fn(current):
            tentative = g_score[current] + cost
            if tentative < g_score.get(nb, float('inf')):
                came_from[nb] = current
                g_score[nb] = tentative
                f = tentative + h_fn(nb, goal)
                open_set.put((f, nb))
    return None  # no path
```

**Kalman filter** (1D linear):

```python
def kf_step(x, P, u, z, A, B, H, Q, R):
    # Predict
    x = A @ x + B @ u
    P = A @ P @ A.T + Q
    # Update
    y = z - H @ x                  # innovation
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S) # Kalman gain
    x = x + K @ y
    P = (np.eye(len(x)) - K @ H) @ P
    return x, P
```

**RRT** (sampling-based motion planning):

```python
def rrt(start, goal, sample_fn, steer_fn, collision_free, max_iter=5000):
    tree = {start: None}
    for _ in range(max_iter):
        q_rand = sample_fn()
        q_near = nearest(tree, q_rand)
        q_new  = steer_fn(q_near, q_rand)
        if collision_free(q_near, q_new):
            tree[q_new] = q_near
            if dist(q_new, goal) < goal_tol:
                return reconstruct(tree, q_new)
    return None
```

### Perception & Computer Vision <a href="#perception--computer-vision" id="perception--computer-vision"></a>

<figure><img src="/files/jZtwIGvfqleT8iHsPk08" alt=""><figcaption></figcaption></figure>

* **SIFT / SURF / ORB**: Feature detection and description for landmark recognition and place recognition
* **FAST & Shi–Tomasi Corner Detectors**: Lightweight keypoint extractors for real-time tracking
* **Lucas–Kanade & Horn–Schunck Optical Flow**: Dense and sparse methods for image motion estimation
* **RANSAC**: Robust model fitting (e.g., fundamental matrix estimation) in the presence of outliers
* **CNNs (Convolutional Neural Networks)**: Deep learning models for object detection and semantic segmentation
* **YOLO / SSD / Mask R-CNN**: Real-time detection and instance segmentation frameworks

### Machine Learning & AI <a href="#machine-learning--ai" id="machine-learning--ai"></a>

<figure><img src="/files/BaR4ZxxVnvAXraoxWY0O" alt=""><figcaption></figcaption></figure>

* **Reinforcement Learning (Q-Learning, DQN, PPO, SAC)**: Autonomous policy learning from interaction rewards
* **Imitation Learning (BC, DAgger, ACT, Diffusion Policy)**: Learning policies from human demonstrations - the dominant 2024-2026 paradigm for manipulation
* **Foundation Models / VLAs (RT-2, OpenVLA, π0)**: Vision-language-action models pretrained at scale, generalist robot policies
* **Gaussian Processes**: Nonparametric models for regression and uncertainty quantification in terrain modeling
* **Support Vector Machines (SVM)**: Classification of sensor or vision data in low-dimensional feature spaces

For a deep dive on modern robot learning - imitation, RL, foundation models, world models, sim-to-real - see the [Robot Learning](/robotics-handbook/robot-learning/robot-learning.md) section.

By combining these algorithms-choosing the right filter for robust sensing, the optimal controller for precise motion, and the most suitable planner for agile navigation-robots can perceive, plan, and act reliably in complex, dynamic environments.


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://panav.gitbook.io/robotics-handbook/programming-for-robotics/algorithms.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.