monteCarloLocalization

Localize robot using range sensor data and map

Description

The monteCarloLocalization System object™ creates a Monte Carlo localization (MCL) object. The MCL algorithm is used to estimate the position and orientation of a vehicle in its environment using a known map of the environment, lidar scan data, and odometry sensor data.

To localize the vehicle, the MCL algorithm uses a particle filter to estimate the vehicle’s position. The particles represent the distribution of likely states for the vehicle, where each particle represents a possible vehicle state. The particles converge around a single location as the vehicle moves in the environment and senses different parts of the environment using a range sensor. An odometry sensor measures the vehicle’s motion.

A monteCarloLocalization object takes the pose and lidar scan data as inputs. The input lidar scan sensor data is given in its own coordinate frame, and the algorithm transforms the data according to the SensorModel.SensorPose property that you must specify. The input pose is computed by integrating the odometry sensor data over time. If the change in pose is greater than any of the specified update thresholds, UpdateThresholds, then the particles are updated and the algorithm computes a new state estimate from the particle filter. The particles are updated using this process:

1. The particles are propagated based on the change in the pose and the specified motion model, MotionModel.

2. The particles are assigned weights based on the likelihood of receiving the range sensor reading for each particle. These likelihood weights are based on the sensor model you specify in SensorModel.

3. Based on the ResamplingInterval property, the particles are resampled from the posterior distribution, and the particles of low weight are eliminated. For example, a resampling interval of 2 means that the particles are resampled after every other update.

The outputs of the object are the estimated pose and covariance, and the value of isUpdated. This estimated state is the mean and covariance of the highest weighted cluster of particles. The output pose is given in the map’s coordinate frame that is specified in the SensorModel.Map property. If the change in pose is greater than any of the update thresholds, then the state estimate has been updated and isUpdated is true. Otherwise, isUpdated is false and the estimate remains the same. For continuous tracking the best estimate of a robot's state, repeat this process of propagating particles, evaluating their likelihood, and resampling.

To estimate robot pose and covariance using lidar scan data:

1. Create the monteCarloLocalization object and set its properties.

2. Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects?

Creation

Syntax

mcl = monteCarloLocalization

mcl = monteCarloLocalization(Name,Value)

Description

mcl = monteCarloLocalization returns an MCL object that estimates the pose of a vehicle using a map, a range sensor, and odometry data. By default, an empty map is assigned, so a valid map assignment is required before using the object.

mcl = monteCarloLocalization(Name,Value) creates an MCL object with additional options specified by one or more Name,Value pair arguments.

Name is a property name and Value is the corresponding value. Name must appear inside single quotes (''). You can specify several name-value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

Properties

InitialPose — Initial pose of vehicle

[0 0 0] (default) | three-element vector

Initial pose of the vehicle used to start localization, specified as a three-element vector, [x y theta], that indicates the position and heading of the vehicle. Initializing the MCL object with an initial pose estimate enables you to use a smaller value for the maximum number of particles and still converge on a location.

InitialCovariance — Covariance of initial pose

diag([1 1 1]) (default) | diagonal matrix | three-element vector | scalar

Covariance of the Gaussian distribution for the initial pose, specified as a diagonal matrix. Three-element vector and scalar inputs are converted to a diagonal matrix. This matrix gives an estimate of the uncertainty of the InitialPose.

GlobalLocalization — Flag to start global localization

false (default) | true

Flag indicating whether to perform global localization, specified as false or true. The default value, false, initializes particles using the InitialPose and InitialCovariance properties. A true value initializes uniformly distributed particles in the entire map and ignores the InitialPose and InitialCovariance properties. Global localization requires a large number of particles to cover the entire workspace. Use global localization only when the initial estimate of vehicle location and orientation is not available.

ParticleLimits — Minimum and maximum number of particles

[500 5000] (default) | two-element vector

Minimum and maximum number of particles, specified as a two-element vector, [min max].

SensorModel — Likelihood field sensor model

likelihoodFieldSensor object

Likelihood field sensor model, specified as a likelihoodFieldSensor object. The default value uses the default likelihoodFieldSensorModel object. After using the object to get output, call release on the object to make changes to SensorModel. For example:

mcl = monteCarloLocalization;
sm = likelihoodFieldSensorModel;
sm.Map = binaryOccupancyMap(10,10,20);
mcl.SensorModel = sm;
[isUpdated,pose,covariance] = mcl([0 0 0],ones(1,10),linspace(-pi/2,pi/2,10));
% Release object before changing motion model
release(mcl);
mcl.SensorModel.NumBeams = 120;
mcl.MotionModel.Noise = [0.25 0.25 0.4 0.4];

MotionModel — Odometry motion model for differential drive

odometryMotionModel object

Odometry motion model for differential drive, specified as an odometryMotionModel object. The default value uses the default odometryMotionModel object. After using the object to get output, call release on the object to make changes to MotionModel. For example:

mcl = monteCarloLocalization;
sm = likelihoodFieldSensorModel;
sm.Map = binaryOccupancyMap(10,10,20);
mcl.SensorModel = sm;
[isUpdated,pose,covariance] = mcl([0 0 0],ones(1,10),linspace(-pi/2,pi/2,10));
% Release object before changing motion model
release(mcl);
mcl.SensorModel.NumBeams = 120;
mcl.MotionModel.Noise = [0.25 0.25 0.4 0.4];

UpdateThresholds — Minimum change in states required to trigger update

[0.2 0.2 0.2] (default) | three-element vector

Minimum change in states required to trigger update, specified as a three-element vector. The localization updates the particles if the minimum change in any of the [x y theta] states is met. The pose estimate updates only if the particle filter is updated.

ResamplingInterval — Number of filter updates between resampling of particles

1 (default) | positive integer

Number of filter updates between resampling of particles, specified as a positive integer.

UseLidarScan — Use lidarScan object as scan input

false (default) | true

Use a lidarScan object as scan input, specified as either false or true.

Usage

Syntax

[isUpdated,pose,covariance] = mcl(odomPose,scan)

[isUpdated,pose,covariance] = mcl(odomPose,ranges,angles)

Description

[isUpdated,pose,covariance] = mcl(odomPose,scan) estimates the pose and covariance of a vehicle using the MCL algorithm. The estimates are based on the pose calculated from the specified vehicle odometry, odomPose, and the specified lidar scan sensor data, scan. mcl is the monteCarloLocalization object. isUpdated indicates whether the estimate is updated based on the UpdateThreshold property.

To enable this syntax, you must set the UseLidarScan property to true. For example:

mcl = monteCarloLocalization('UseLidarScan',true);
...
[isUpdated,pose,covariance] = mcl(odomPose,scan);

[isUpdated,pose,covariance] = mcl(odomPose,ranges,angles) specifies the lidar scan data as ranges and angles.

Input Arguments

odomPose — Pose based on odometry

three-element vector

Pose based on odometry, specified as a three-element vector, [x y theta]. This pose is calculated by integrating the odometry over time.

scan — Lidar scan readings

lidarScan object

Lidar scan readings, specified as a lidarScan object.

Dependencies

To use this argument, you must set the UseLidarScan property to true.

mcl.UseLidarScan = true;

ranges — Range values from scan data

vector

Range values from scan data, specified as a vector with elements measured in meters. These range values are distances from a laser scan sensor at the specified angles. The ranges vector must have the same number of elements as the corresponding angles vector.

angles — Angle values from scan data

vector

Angle values from scan data, specified as a vector with elements measured in radians. These angle values are the angles at which the specified ranges were measured. The angles vector must be the same length as the corresponding ranges vector.

Output Arguments

isUpdated — Flag for pose update

logical

Flag for pose update, returned as a logical. If the change in pose is more than any of the update thresholds, then the output is true. Otherwise, it is false. A true output means that updated pose and covariance are returned. A false output means that pose and covariance are not updated and are the same as at the last update.

pose — Current pose estimate

three-element vector

Current pose estimate, returned as a three-element vector, [x y theta]. The pose is computed as the mean of the highest-weighted cluster of particles.

covariance — Covariance estimate for current pose

matrix

Covariance estimate for current pose, returned as a matrix. This matrix gives an estimate of the uncertainty of the current pose. The covariance is computed as the covariance of the highest-weighted cluster of particles.

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object named obj, use this syntax:

release(obj)

Specific to monteCarloLocalization

getParticles	Get particles from localization algorithm
setParticles	Set particles from localization algorithm

Common to All System Objects

step	Run System object algorithm
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object

Examples

Estimate Vehicle Pose from Range Sensor Data

Create a monteCarloLocalization object. Set the UseLidarScan property to true.

mcl = monteCarloLocalization;
mcl.UseLidarScan = true;

Create a likelihoodFieldSensorModel object for a range sensor. Assign the sensor model with an occupancy grid map to the monteCarloLocalization object.

sm = likelihoodFieldSensorModel;
p = zeros(200,200);
sm.Map = occupancyMap(p,20);
mcl.SensorModel = sm;

Create a sample laser scan data.

ranges = 10*ones(1,300);
ranges(1,130:170) = 1.0;
angles = linspace(-pi/2,pi/2,300);
odometryPose = [0 0 0];

Create a lidarScan object by specifying the ranges and angles.

scan = lidarScan(ranges,angles);

Estimate vehicle pose and covariance.

[isUpdated,estimatedPose,covariance] = mcl(odometryPose,scan)

isUpdated = logical
   1

estimatedPose = 1×3

    0.0350   -0.0126    0.0280

covariance = 3×3

    0.9946   -0.0012         0
   -0.0012    0.9677         0
         0         0    0.9548

References

[1] Thrun, Sebatian, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. MIT Press, 2005.

[2] Dellaert, F., D. Fox, W. Burgard, and S. Thrun. "Monte Carlo Localization for Mobile Robots." Proceedings 1999 IEEE International Conference on Robotics and Automation.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019b