激光似然场模型及在AMCL中的应用

1.beam model与Likehood field model
beam model测量光束模型
纯物理模型，针对激光发出的测量光束建模，将一次测量误差分解为四个误差。
期望值的计算需要用retraying，每一个位姿需要进行N次retracing， 为一帧激光的激光束数量。
在非结构化环境中（clutter），位姿微小的改变会造成期望值的巨大变化，从而导致得分进行突变。
likehood field model
和测量光束模型相比，考虑了地图的因素，不再是对激光的扫描线物理建模，而是考虑测量到的物体的因素。
对图像进行高斯平滑，在任何环境中的期望值对于位姿都是平滑的。
得分的计算不需要经过raytracing，直接通过查表即可得到，计算量低。
同时适合结构和非结构的环境。

2.似然场模型及matlab代码实现

引自 https://www.cnblogs.com/yhlx125/p/6661544.html
http://www.cnblogs.com/yhlx125/p/5586499.html

for k=1:size(zt,1)
    if zt(k,2)>0
        d = -grid_dim/2;
    else
        d = grid_dim/2;
    end
    phi = pi_to_pi(zt(k,2) + x(3));
    if zt(k,1) ~= Z_max
        ppx = [x(1),x(1) + zt(k,1)*cos(phi) + d];
        ppy = [x(2),x(2) + zt(k,1)*sin(phi) + d];
        end_points = [end_points;ppx(2),ppy(2)];
         
        wm = likelihood_field_range_finder_model(X(j,:)',xsensor,...
                   zt(k,:)',nearest_wall, grid_dim, std_hit,Z_weights,Z_max);
        W(j) = W(j) * wm;
    else
        dist = Z_max + std_hit*randn(1);
        ppx = [x(1),x(1) + dist*cos(phi) + d];
        ppy = [x(2),x(2) + dist*sin(phi) + d];
        missed_points = [missed_points;ppx(2),ppy(2)];               
    end
    set(handle_sensor_ray(k),'XData', ppx, 'YData', ppy)
end

function q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max)
% retorna probabilidad de medida range finder :)
% X col, zt col, xsen col
[n,m] = size(N);
 
% Robot global position and orientation
theta = X(3);
 
% Beam global angle
theta_sen = zt(2);
phi = pi_to_pi(theta + theta_sen);
 
%Tranf matrix in case sensor has relative position respecto to robot's CG
rotS = [cos(theta),-sin(theta);sin(theta),cos(theta)];
 
% Prob. distros parameters
sigmaR = std_hit;
zhit  = Zw(1);
zrand = Zw(2);
zmax  = Zw(3);
 
% Actual algo
q = 1;
if zt(1) ~= z_max
    % get global pos of end point of measument
    xz = X(1:2) + rotS*x_sensor + zt(1)*[cos(phi);
                                         sin(phi)];
    xi = floor(xz(1)/dim) + 1;
    yi = floor(xz(2)/dim) + 1;
     
    % if end point doesn't lay inside map: unknown
    if xi<1 || xi>n || yi<1 || yi>m
        q = 1.0/z_max; % all measurements equally likely, uniform in range [0-zmax]
        return
    end
     
    dist2 = N(xi,yi);
    gd = gauss_1D(0,sigmaR,dist2);
    q = zhit*gd + zrand/zmax;
end
 
end

3.AMCL中的laser model parameters

laser_min_range

被考虑的最小扫描范围；参数设置为-1.0时，将会使用激光上报的最小扫描范围

laser_max_range

被考虑的最大扫描范围；参数设置为-1.0时，将会使用激光上报的最大扫描范围

laser_max_beams

更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准）

laser_model_type

模型使用，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征–官网的注释），默认是“likehood_field”
光束模型使用所有4：Z_HIT、z_Short、z_max和z_rand。似然场模型只使用2：Z_HIT和z_rand。使用中的权重和应等于1。

laser_z_hit

模型的z_hit部分的权重，默认0.95。测量点到对应物体之间的距离的误差所占最终计算得到的距离的权重。

laser_z_rand

模型的z_rand部分的权重，默认0.05。随机噪声的权重。（如声呐的多次反射，传感器串扰）

laser_sigma_hit

被用在模型的z_hit部分的高斯模型的标准差，默认0.2m

laser_likelihood_max_dist

dist —— laser_likelihood_max_dist
σ —— laser_sigma_hit
prob为laser_likelihood_max_dist为中心标准方差为σ（laser_sigma_hit）的高斯分布的距离概率）
那么观测到z的概率 p = laser_z_hit * prob(dist, σ) + laser_z_rand * p_rand;

待解决的问题
laser_likelihood_max_dist是啥？