Navigation Toolbox

Design, simulate, and deploy alogrithms for planning and navigation

Navigation Toolbox™ provides algorithms and analysis tools for designing motion planning and navigation systems. The toolbox contains customizable search and sampling-based path-planners. It also contains sensor models and algorithms for multi-sensor pose estimation. You can create 2D and 3D map representations using your own data or generate maps using the simultaneous localization and mapping (SLAM) algorithms included in the toolbox. Reference examples are provided for self-driving and robotics applications.


You can generate metrics for comparing path optimality, smoothness, and performance benchmarks. The SLAM map builder app lets you interactively visualize and debug map generation. You can test your algorithms by deploying them directly to hardware (with MATLAB Coder™ or Simulink Coder™).


Mapping and Localization

Create an occupancy map of the environment using SLAM algorithms. Use pose estimation to localize a vehicle.

Simultaneous Localization and Mapping (SLAM)


Implement SLAM algorithms with lidar scans using pose graph optimization. Use SLAM Map Builder app to find and modify loop closures. Build and export the resulting map as an occupancy grid.

Implement Simultaneous Localization and Mapping (SLAM) with Lidar Scans

Perform SLAM Using 3D Lidar Point Clouds


Map generation using lidar SLAM

Localization and Pose Estimation

Apply Monte Carlo Localization (MCL) to estimate the position and orientation of a vehicle using sensor data and a map of the environment.


Estimate pose of nonholonomic and aerial vehicles using inertial sensors and GPS. Determine pose without GPS by fusing inertial sensors with altimeters or visual odometry.

IMU and GPS Fusion for Inertial Navigation

Estimate Position and Orientation of a Ground Vehicle

Localize Turtlebot Using Monte Carlo Localization


Monte Carlo Localization in an indoor environment. 

2D and 3D Map Representations

Create a binary or probabilistic occupancy grid using real or simulated sensor readings. Use egocentric maps that are fast to query and memory efficient.

Create Egocentric Occupancy Maps Using Range Sensors

Build Occupancy Map from Depth Images Using Visual Odometry and Optimized Pose Graph

Occupancy Grids


3D occupancy grid visualization.

Motion Planning

Use extensible path planners, choose optimal paths, and compute steering commands for path following.

Path Planning

Use sampling-based path planners such as Rapidly-Exploring Random Tree (RRT) and RRT* to find a path from start to goal locations. Adapt the planner interface to your application’s state space. Use Dubins and Reeds-Shepp motion primitives to create smooth, drivable paths.

Plan Mobile-Robot Paths Using RRT

Motion Planning with RRT for a Robot Manipulator

Lane Change for Highway Driving

RRT Path Planner

Metrics for Path Planning

Use metrics to validate paths for smoothness and clearance from obstacles. Choose the best path using numerical and visual comparisons.

Path Metrics Object


Path Following and Controls

Tune control algorithms to follow a planned path. Compute steering and velocity commands using vehicle motion models. Avoid obstacles with algorithms such as vector field histogram.

Pure Pursuit Controller

Vector Field Histogram


Sensor Modeling and Simulation

Simulate measurements from IMUs, GPS receivers, and range sensors under various environmental conditions.

Sensor Models

Model IMU, GPS, and INS sensors. Tune parameters such as temperature and noise to emulate real-world conditions. Estimate distances to objects using range sensors and measure vehicle motion with odometry sensors.


Model IMU, GPS, and INS/GPS

Inertial Sensor Noise Analysis Using Allan Variance


Sensor Motion Simulation

Plot a vehicle’s orientation, velocity, trajectories, and sensor measurements. Generate trajectories to emulate sensors traveling through the world. Export trajectories to external simulators or to a scenario designer.

Introduction to Simulating IMU Measurements

IMU and GPS Fusion for Inertial Navigation


Have Questions About Robotics System Toolbox

30 Days of Exploration at Your Fingertips