Localize robot using range sensor data and map
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.
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:
The particles are propagated based on the change in the pose and the specified motion model,
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
Based on the
ResamplingIntervalproperty, 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
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
To estimate robot pose and covariance using lidar scan data:
monteCarloLocalizationobject and set its properties.
Call the object with arguments, as if it were a function.
To learn more about how System objects work, see What Are System Objects?
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
creates an MCL object with additional options specified by one or more
mcl = monteCarloLocalization(
Name,Value pair arguments.
Name is a property name and
the corresponding value.
Name must appear inside single
''). You can specify several name-value pair
arguments in any order as
InitialPose — Initial pose of vehicle
0 0 0] (default) | three-element vector
Initial pose of the vehicle used to start localization, specified as a
[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
GlobalLocalization — Flag to start global localization
false (default) |
Flag indicating whether to perform global localization, specified as
true. The default value,
false, initializes particles using the
true value initializes uniformly
distributed particles in the entire map and ignores the
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
SensorModel — Likelihood field sensor model
Likelihood field sensor model, specified as a
likelihoodFieldSensor object. The default value uses
object. After using the object to get output, call
release on the object to make changes to
SensorModel. For example:
mcl = monteCarloLocalization; [isUpdated,pose,covariance] = mcl(ranges,angles); release(mcl) mcl.SensorModel.NumBeams = 120;
MotionModel — Odometry motion model for differential drive
Odometry motion model for differential drive, specified as an
odometryMotionModel object. The default value uses
odometryMotionModel object. After
using the object to get output, call
release on the
object to make changes to
MotionModel. For example:
mcl = monteCarloLocalization; [isUpdated,pose,covariance] = mcl(ranges,angles); release(mcl) 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) |
lidarScan object as scan input, specified as either
estimates the pose and covariance of a vehicle using the MCL algorithm. The
estimates are based on the pose calculated from the specified vehicle
odomPose, and the specified lidar scan sensor
mcl is the
isUpdated indicates whether the estimate is updated
based on the
To enable this syntax, you must set the
true. For example:
mcl = monteCarloLocalization('UseLidarScan',true); ... [isUpdated,pose,covariance] = mcl(odomPose,scan);
odomPose — Pose based on odometry
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
Lidar scan readings, specified as a
To use this argument, you must set the
UseLidarScan property to
mcl.UseLidarScan = true;
ranges — Range values from scan data
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
ranges vector must have the same number of
elements as the corresponding
angles — Angle values from scan data
Angle values from scan data, specified as a vector with elements
measured in radians. These angle values are the angles at which the
ranges were measured. The
angles vector must be the same length as the
isUpdated — Flag for pose update
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
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
pose — Current pose estimate
Current pose estimate, returned as a three-element vector,
y theta]. The pose is computed as the mean of the
highest-weighted cluster of particles.
covariance — Covariance estimate for current pose
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.
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
Estimate Vehicle Pose from Range Sensor Data
monteCarloLocalization object, assign a sensor model, and calculate a pose estimate using the
Note: Starting in R2016b, instead of using the step method to perform the operation defined by the System object, you can call the object with arguments, as if it were a function. For example,
y = step(obj,x) and
y = obj(x) perform equivalent operations.
monteCarloLocalization object. Set the
UseLidarScan property to
mcl = monteCarloLocalization; mcl.UseLidarScan = true;
Assign a sensor model with an occupancy grid map to the object.
sm = likelihoodFieldSensorModel; p = zeros(200,200); sm.Map = occupancyMap(p,20); mcl.SensorModel = sm;
Create sample laser scan data input.
ranges = 10*ones(1,300); ranges(1,130:170) = 1.0; angles = linspace(-pi/2,pi/2,300); odometryPose = [0 0 0];
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
 Thrun, Sebatian, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. MIT Press, 2005.
 Dellaert, F., D. Fox, W. Burgard, and S. Thrun. "Monte Carlo Localization for Mobile Robots." Proceedings 1999 IEEE International Conference on Robotics and Automation.