www.gusucode.com > robotics 案例源码程序 matlab代码 > robotics/EstimateRobotPoseFromRangeSensorDataExample.m

    %% Estimate Robot Pose from Range Sensor Data
% Create a |MonteCarloLocalization| object,
% assign a sensor model and calculate a pose estimate using the |step|
% method.
%
% 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.

%%
% Create a Monte Carlo localization object.
mcl = robotics.MonteCarloLocalization;
%%
% Assign a sensor model with an occupancy grid map to the object.
sm = robotics.LikelihoodFieldSensorModel;
p = zeros(200,200);
sm.Map = robotics.OccupancyGrid(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];
%%
% Estimate robot pose and covariance.
[isUpdated,estimatedPose,covariance] = mcl(odometryPose,ranges,angles)