Localization Algorithms

Particle filters, scan matching, Monte Carlo localization, pose graphs, odometry

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

Particle Filter State Estimation

`stateEstimatorPF`	Create particle filter state estimator
`getStateEstimate`	Extract best state estimate and covariance from particles
`predict`	Predict state of robot in next time step
`correct`	Adjust state estimate based on sensor measurement

Scan Matching

`matchScans`	Estimate pose between two laser scans
`matchScansGrid`	Estimate pose between two lidar scans using grid-based search
`matchScansLine`	Estimate pose between two laser scans using line features
`transformScan`	Transform laser scan based on relative pose
`lidarScan`	Create object for storing 2-D lidar scan

Monte Carlo Localization

`monteCarloLocalization`	Localize robot using range sensor data and map
`lidarScan`	Create object for storing 2-D lidar scan
`getParticles`	Get particles from localization algorithm
`odometryMotionModel`	Create an odometry motion model
`likelihoodFieldSensorModel`	Create a likelihood field range sensor model
`resamplingPolicyPF`	Create resampling policy object with resampling settings

Pose Graphs

`poseGraph`	Create 2-D pose graph
`poseGraph3D`	Create 3-D pose graph
`poseplot`	3-D pose plot
`addPointLandmark`	Add landmark point node to pose graph
`addRelativePose`	Add relative pose to pose graph
`edgeNodePairs`	Edge node pairs in pose graph
`edgeConstraints`	Edge constraints in pose graph
`edgeResidualErrors`	Compute pose graph edge residual errors
`findEdgeID`	Find edge ID of edge
`nodeEstimates`	Poses of nodes in pose graph
`optimizePoseGraph`	Optimize nodes in pose graph
`removeEdges`	Remove loop closure edges from graph
`show`	Plot pose graph
`trimLoopClosures`	Optimize pose graph and remove bad loop closures

Factor Graph

`factorGraph`	Bipartite graph of factors and nodes
`factorGraphSolverOptions`	Solver options for factor graph
`importFactorGraph`	Import factor graph from g2o log file
`factorIMU`	Convert IMU readings to factor
`factorIMUParameters`	Factor IMU parameters
`factorGPS`	Factor for GPS measurement
`factorTwoPoseSE2`	Factor relating two SE(2) poses
`factorTwoPoseSE3`	Factor relating two SE(3) poses
`factorPoseSE2AndPointXY`	Factor relating SE(2) position and 2-D point
`factorPoseSE3AndPointXYZ`	Factor relating SE(3) position and 3-D point
`factorIMUBiasPrior`	Prior factor for IMU bias
`factorVelocity3Prior`	Prior factor for 3-D velocity
`factorPoseSE3Prior`	Full-state prior factor for SE(3) pose
`factorCameraSE3AndPointXYZ`	Factor relating SE(3) camera pose and 3-D point
`estimateGravityRotation`	Estimate gravity rotation using IMU measurements and factor graph optimization
`estimateGravityRotationAndPoseScale`	Estimate gravity rotation and pose scale using IMU measurements and factor graph optimization

Wheel Encoder Odometry

`wheelEncoderOdometryAckermann`	Compute Ackermann vehicle odometry using wheel encoder ticks and steering angle
`wheelEncoderOdometryBicycle`	Compute bicycle odometry using wheel encoder ticks and steering angle
`wheelEncoderOdometryDifferentialDrive`	Compute differential-drive vehicle odometry using wheel encoder ticks
`wheelEncoderOdometryUnicycle`	Compute unicycle odometry using wheel encoder ticks and angular velocity

Topics

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.
Monte Carlo Localization Algorithm
The Monte Carlo Localization (MCL) algorithm is used to estimate the position and orientation of a robot.
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.
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.

Featured Examples

Factor Graph-Based Pedestrian Localization with IMU and GPS Sensors

Estimate the position of a pedestrian using logged sensor data from an inertial measurement unit (IMU) and Global Positioning System (GPS) receiver and a factor graph.

Open Live Script

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. The example uses a pose graph approach and a factor graph approach, and compares the two graphs.

Open Live Script

Localize TurtleBot Using Monte Carlo Localization Algorithm

Apply the Monte Carlo Localization algorithm on a TurtleBot® robot in a simulated Gazebo® environment.

Open Live Script

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

Reduce the drift in the estimated trajectory (location and orientation) of a monocular camera using 3-D pose graph optimization. Visual odometry estimates the current global pose of the camera (current frame). Because of poor matching or errors in 3-D point triangulation, robot trajectories often tends to drift from the ground truth. Loop closure detection and pose graph optimization reduce this drift and correct for errors.

Open Live Script

Monocular Visual-Inertial Odometry Using Factor Graph

Monocular visual-inertial odometry estimates the position and orientation of the robot using camera and inertial measurement unit (IMU) sensor data. Camera-based state estimation is accurate during low-speed navigation. However, camera-based estimation faces challenges such as motion blur and track loss at higher speeds. Also monocular camera-based estimation can estimate poses at an arbitrary scale. On the other hand, inertial navigation can handle high-speed navigation easily and estimate poses at world scale. You can combine the advantages of both types of sensor data to achieve better accuracy using tightly coupled factor graph optimization.

Open Live Script