Main Content

Localization and Pose Estimation

Inertial navigation, pose estimation, scan matching, Monte Carlo localization

Use localization and pose estimation algorithms to orient your vehicle in your environment. Sensor pose estimation uses filters to improve and combine sensor readings for IMU, GPS, and others. Localization algorithms, like Monte Carlo localization and scan matching, estimate your pose in a known map using range sensor or lidar readings. Pose graphs track your estimated poses and can be optimized based on edge constraints and loop closures. For simultaneous localization and mapping, see SLAM.

Functions

expand all

ahrsfilterOrientation from accelerometer, gyroscope, and magnetometer readings
ahrs10filterHeight and orientation from MARG and altimeter readings
complementaryFilterOrientation estimation from a complementary filter
ecompassOrientation from magnetometer and accelerometer readings
imufilterOrientation from accelerometer and gyroscope readings
insfilterCreate inertial navigation filter
insfilterAsyncEstimate pose from asynchronous MARG and GPS data
insfilterErrorStateEstimate pose from IMU, GPS, and monocular visual odometry (MVO) data
insfilterMARGEstimate pose from MARG and GPS data
insfilterNonholonomicEstimate pose with nonholonomic constraints
tunerconfigFusion filter tuner configuration options
tunerPlotPosePlot filter pose estimates during tuning
stateEstimatorPFCreate particle filter state estimator
getStateEstimateExtract best state estimate and covariance from particles
predictPredict state of robot in next time step
correctAdjust state estimate based on sensor measurement
matchScansEstimate pose between two laser scans
matchScansGridEstimate pose between two lidar scans using grid-based search
matchScansLineEstimate pose between two laser scans using line features
transformScanTransform laser scan based on relative pose
lidarScanCreate object for storing 2-D lidar scan
monteCarloLocalizationLocalize robot using range sensor data and map
lidarScanCreate object for storing 2-D lidar scan
getParticlesGet particles from localization algorithm
odometryMotionModelCreate an odometry motion model
likelihoodFieldSensorModelCreate a likelihood field range sensor model
resamplingPolicyPFCreate resampling policy object with resampling settings
poseGraph Create 2-D pose graph
poseGraph3D Create 3-D pose graph
addPointLandmarkAdd landmark point node to pose graph
addRelativePoseAdd relative pose to pose graph
edgeNodePairsEdge node pairs in pose graph
edgeConstraintsEdge constraints in pose graph
edgeResidualErrorsCompute pose graph edge residual errors
findEdgeIDFind edge ID of edge
nodeEstimatesPoses of nodes in pose graph
optimizePoseGraphOptimize nodes in pose graph
removeEdgesRemove loop closure edges from graph
showPlot pose graph
trimLoopClosuresOptimize pose graph and remove bad loop closures
wheelEncoderOdometryAckermannCompute Ackermann vehicle odometry using wheel encoder ticks and steering angle
wheelEncoderOdometryBicycleCompute bicycle odometry using wheel encoder ticks and steering angle
wheelEncoderOdometryDifferentialDriveCompute differential-drive vehicle odometry using wheel encoder ticks
wheelEncoderOdometryUnicycleCompute unicycle odometry using wheel encoder ticks and angular velocity

Topics

Sensor Fusion

Binaural Audio Rendering Using Head Tracking

Track head orientation by fusing data received from an IMU and then control the direction of arrival of a sound source by applying head-related transfer functions (HRTF).

Estimate Orientation Through Inertial Sensor Fusion

This example shows how to use 6-axis and 9-axis fusion algorithms to compute orientation.

Logged Sensor Data Alignment for Orientation Estimation

This example shows how to align and preprocess logged sensor data.

Lowpass Filter Orientation Using Quaternion SLERP

This example shows how to use spherical linear interpolation (SLERP) to create sequences of quaternions and lowpass filter noisy trajectories.

Pose Estimation From Asynchronous Sensors

This example shows how you might fuse sensors at different rates to estimate pose.

Choose Inertial Sensor Fusion Filters

Applicability and Limitations of Inertial Sensor Fusion Filters.

Estimate Orientation with a Complementary Filter and IMU Data

This example shows how to stream IMU data from an Arduino and estimate orientation using a complementary filter.

Estimating Orientation Using Inertial Sensor Fusion and MPU-9250

This example shows how to get data from an InvenSense MPU-9250 IMU sensor and to use the 6-axis and 9-axis fusion algorithms in the sensor data to compute orientation of the device.

Wireless Data Streaming and Sensor Fusion Using BNO055

This example shows how to get data from a Bosch BNO055 IMU sensor through HC-05 Bluetooth® module and to use the 9-axis AHRS fusion algorithm on the sensor data to compute orientation of the device.

Custom Tuning of Fusion Filters

Use the tune function to optimize the noise parameters of several fusion filters, including the ahrsfilter object.

Localization Algorithms

Localize TurtleBot Using Monte Carlo Localization

This example demonstrates an application of the Monte Carlo Localization (MCL) algorithm on TurtleBot® in simulated Gazebo® environment.

Compose a Series of Laser Scans with Pose Changes

Use the matchScans function to compute the pose difference between a series of laser scans.

Minimize Search Range in Grid-based Lidar Scan Matching Using IMU

This example shows how to use an inertial measurement unit (IMU) to minimize the search range of the rotation angle for scan matching algorithms.

Reduce Drift in 3-D Visual Odometry Trajectory Using Pose Graphs

This example shows how to reduce the drift in the estimated trajectory (location and orientation) of a monocular camera using 3-D pose graph optimization.

Monte Carlo Localization Algorithm

The Monte Carlo Localization (MCL) algorithm is used to estimate the position and orientation of a robot.

Particle Filter Parameters

To use the stateEstimatorPF (Robotics System Toolbox) particle filter, you must specify parameters such as the number of particles, the initial particle location, and the state estimation method.

Particle Filter Workflow

A particle filter is a recursive, Bayesian state estimator that uses discrete particles to approximate the posterior distribution of the estimated state.

Featured Examples

IMU and GPS Fusion for Inertial Navigation

IMU and GPS Fusion for Inertial Navigation

How you might build an IMU + GPS fusion algorithm suitable for unmanned aerial vehicles (UAVs) or quadcopters.

Open Live Script
Estimate Position and Orientation of a Ground Vehicle

Estimate Position and Orientation of a Ground Vehicle

Estimate the position and orientation of ground vehicles by fusing data from an inertial measurement unit (IMU) and a global positioning system (GPS) receiver.

Open Script
Estimate Orientation and Height Using IMU, Magnetometer, and Altimeter

Estimate Orientation and Height Using IMU, Magnetometer, and Altimeter

Fuse data from a 3-axis accelerometer, 3-axis gyroscope, 3-axis magnetometer (together commonly referred to as a MARG sensor for Magnetic, Angular Rate, and Gravity), and 1-axis altimeter to estimate orientation and height.

Open Live Script
Estimate Phone Orientation Using Sensor Fusion

Estimate Phone Orientation Using Sensor Fusion

MATLAB Mobile™ reports sensor data from the accelerometer, gyroscope, and magnetometer on Apple or Android mobile devices. Raw data from each sensor or fused orientation data can be obtained. This examples shows how to compare the fused orientation data from the phone with the orientation estimate from the ahrsfilter object.

Open Live Script
Visual-Inertial Odometry Using Synthetic Data

Visual-Inertial Odometry Using Synthetic Data

Estimate the pose (position and orientation) of a ground vehicle using an inertial measurement unit (IMU) and a monocular camera. In this example, you:

Open Script
Landmark SLAM Using AprilTag Markers

Landmark SLAM Using AprilTag Markers

Combine robot odometry data and observed fiducial markers called AprilTags to better estimate the robot trajectory and the landmark positions in the environment.

Open Live Script
Automatic Tuning of the insfilterAsync Filter

Automatic Tuning of the insfilterAsync Filter

The insfilterAsync object is a complex extended Kalman filter that estimates the device pose. However, manually tuning the filter or finding the optimal values for the noise parameters can be a challenging task. This example illustrates how to use the tune function to optimize the filter noise parameters.

Open Live Script

Navigation Toolbox Documentation

Support