蒙特卡罗定位-Matlab getParticles函数为所有粒子返回相同的权重

我目前正在Matlab中根据他们的要求实现amcl。当使用amcl函数getParticles可以找到哪个定义时，它表示所有粒子都具有相同的权重。为什么会出现这种情况，如何解决？我从运行ROS的VM接收到过滤器的信息，粒子确实会聚，这使我认为粒子的权重是正确的。（否则它们不应该会聚，对吗？）。我已经被这个问题困扰了很长一段时间，因此需要所有帮助

load officemap.mat
show(map)

odometryModel = odometryMotionModel;
odometryModel.Noise = [0.2 0.2 0.2 0.2];

rangeFinderModel = likelihoodFieldSensorModel;
rangeFinderModel.SensorLimits = [0.45 8];
rangeFinderModel.Map = map;

% Query the Transformation Tree (tf tree) in ROS.
tftree = rostf;
waitForTransform(tftree,'/base_link','/base_scan');
sensorTransform = getTransform(tftree,'/base_link', '/base_scan');

% Get the euler rotation angles.
laserQuat = [sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X ...
    sensorTransform.Transform.Rotation.Y sensorTransform.Transform.Rotation.Z];
laserRotation = quat2eul(laserQuat, 'ZYX');

% Setup the |SensorPose|, which includes the translation along base_link's
% +X, +Y direction in meters and rotation angle along base_link's +Z axis
% in radians.
rangeFinderModel.SensorPose = ...
    [sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation(1)];
laserSub = rossubscriber('/scan');
odomSub = rossubscriber('/odom');
[velPub,velMsg] = ...
    rospublisher('/cmd_vel','geometry_msgs/Twist');
amcl = monteCarloLocalization;
amcl.UseLidarScan = true;
amcl.MotionModel = odometryModel;
amcl.SensorModel = rangeFinderModel;
amcl.UpdateThresholds = [0.2,0.2,0.2];
amcl.ResamplingInterval = 1;
amcl.ParticleLimits = [500 5000];
amcl.GlobalLocalization = false;
amcl.InitialPose = ExampleHelperAMCLGazeboTruePose;
amcl.InitialCovariance = eye(3)*0.5;
visualizationHelper = ExampleHelperAMCLVisualization(map);
wanderHelper = ...
    ExampleHelperAMCLWanderer(laserSub, sensorTransform, velPub, velMsg);
numUpdates = 60;
i = 0;
while i < numUpdates
    scanMsg = receive(laserSub);
    odompose = odomSub.LatestMessage;
    scan = lidarScan(scanMsg);
    
    % For sensors that are mounted upside down, you need to reverse the
    % order of scan angle readings using 'flip' function.
    
    % Compute robot's pose [x,y,yaw] from odometry message.
    odomQuat = [odompose.Pose.Pose.Orientation.W, odompose.Pose.Pose.Orientation.X, ...
        odompose.Pose.Pose.Orientation.Y, odompose.Pose.Pose.Orientation.Z];
    odomRotation = quat2eul(odomQuat);
    pose = [odompose.Pose.Pose.Position.X, odompose.Pose.Pose.Position.Y odomRotation(1)];
    
    % Update estimated robot's pose and covariance using new odometry and
    % sensor readings.
    [isUpdated,estimatedPose, estimatedCovariance] = amcl(pose, scan);
    [particles, weights] = getParticles(amcl);
    weights
    % Drive robot to next pose.
    wander(wanderHelper);
    
    % Plot the robot's estimated pose, particles and laser scans on the map.
    if isUpdated
        i = i + 1;
        plotStep(visualizationHelper, amcl, estimatedPose, scan, i)
    end
    
end

加载officemap.mat
显示（地图）
里程计模型=里程计模型；
里程计模型噪音=[0.20.20.20.2]；
rangeFinderModel=似然场传感器模型；
RangeFindModel.SensorLimits=[0.45 8]；
rangeFinderModel.Map=Map；
%在ROS中查询转换树（tf树）。
tftree=rostf；
waitForTransform（tftree，“/base_link，”/base_scan'）；
sensorTransform=getTransform（tftree，“/base_link，”/base_scan”）；
%获取欧拉旋转角度。
laserQuat=[sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X。。。
传感器变换。变换。旋转。Y传感器变换。变换。旋转。Z]；
laserRotation=quat2eul（laserQuat，'ZYX'）；
%设置| SensorPose |，其中包括沿基本链接的平移
%+X、+Y方向（以米为单位）以及沿基准链接的+Z轴的旋转角度
%以弧度表示。
RangeFindModel.SensorPose=。。。
[sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation（1）]；
laserSub=rossubscriber（'/scan'）；
odomSub=rossubscriber（'/odom'）；
[velPub，velMsg]=。。。
rospublisher（'/cmd_vel'，'geometry_msgs/Twist'）；
amcl=蒙特卡洛本地化；
amcl.UseLidarScan=true；
amcl.MotionModel=里程计模型；
amcl.SensorModel=RangeFindModel；
amcl.UpdateThresholds=[0.2,0.2,0.2]；
amcl.ResamplingInterval=1；
amcl.ParticleLimits=[500 5000]；
amcl.GlobalLocalization=false；
amcl.InitialPose=示例helperamlgazebotruepose；
amcl.初始协方差=眼睛（3）*0.5；
visualizationHelper=ExampleHelperAMCLVisualization（地图）；
wanderHelper=。。。
例如HelperCamcLwander（laserSub、sensorTransform、velPub、velMsg）；
numUpdates=60；
i=0；
当我更新的时候
scanMsg=接收（激光SUB）；
odompose=odomSub.LatestMessage；
扫描=激光雷达扫描（扫描信息）；
%对于倒置安装的传感器，需要反转
%使用“翻转”功能的扫描角度读数顺序。
%根据里程计信息计算机器人的姿势[x，y，偏航]。
odomQuat=[odompose.Pose.Pose.Orientation.W，odompose.Pose.Pose.Orientation.X。。。
odompose.Pose.Pose.Orientation.Y，odompose.Pose.Pose.Orientation.Z]；
odomRotation=quat2eul（odomQuat）；
pose=[odompose.pose.pose.Position.X，odompose.pose.pose.Position.Y-odomRotation（1）]；
%使用新的里程计和参数更新估计的机器人姿态和协方差
%传感器读数。
[IsUpdate，estimatedPose，estimatedCovariance]=amcl（姿势，扫描）；
[粒子，重量]=获取粒子（amcl）；
砝码
%驱动机器人到下一个姿势。
漫游（漫游助手）；
%在地图上绘制机器人的估计姿势、粒子和激光扫描。
如果更新了
i=i+1；
绘图步骤（visualizationHelper、amcl、estimatedPose、scan、i）
结束
结束