《概率機器人》一書用兩章介紹了幾種定位方法，一種是基於馬爾科夫決策，另外一章是柵格和蒙特卡羅方法。

1.馬爾科夫定位

2.EKF定位

3.柵格定位

4.MCL蒙特卡羅定位

　　這裡主要學習一下蒙特卡羅定位。機器人定位問題可以描述為：如何確定機器人在關聯的已知環境地圖中的位姿pose。拋開SLAM，定位問題可以單獨討論，目前的室內定位技術研究也很火熱。

　　地圖表達的是全域性座標系，地圖獨立於機器人的位姿而存在。定位問題也可以描述為確定機器人區域性座標系統在全域性座標系中對應關係的過程，即確定機器人的位姿$x_{t}=(x,y,\theta)^T$。

機器人定位的必要性

　　matlab程式碼：

function myPose = particleLocalization(ranges, angles, map, param)

% occupancy value of unexplored pixels
unknown = mode(reshape(map, size(map,1)*size(map,2), 1));

N = size(ranges, 2); % number of poses to calculate
myPose = zeros(3, N); % initialize return value

resol = param.resol; % map resolution
origin = param.origin; % origin in pixel

sig = [0.08, 0, 0; 0, 0.08, 0; 0, 0, 0.08]; % noise for particle movement

myPose(:,1) = param.init_pose; % init position

M = 200; % number of particles

P = repmat(myPose(:,1), [1, M]);

thr = ceil(3.5/5*size(angles,1)); % set the score threshold as 70%

for j = 2:N
    maxscore = 0;
    while maxscore < thr
        Q=P+(randn(size(P,2),3)*sig)'; % particles movement
        score = zeros(size(Q,2), 1); % scores
        for k = 1:size(Q,2) % calculate score for each particle
            occ_x = ceil( (ranges(:,j) .* cos(angles+Q(3,k)) + Q(1,k) )  * resol + origin(1) );
            occ_y = ceil( (-ranges(:,j) .* sin(angles+Q(3,k)) + Q(2,k) ) * resol + origin(2) );
            ids = occ_x > 0 & occ_x <= size(map,2) & occ_y > 0 & occ_y <= size(map,1);
            score(k) = size( map( map (sub2ind(size(map), occ_y(ids), occ_x(ids)) ) > unknown ), 1);
        end
        [maxscore, index] = max(score); % select particle with maximum score
    end
    myPose(:,j) = Q(:,index); % set pose(j) as the optimal particle

    Q = Q(:,score >= thr); % select particles with high score
    P = repmat(Q, 1, ceil(M/size(Q,2)) ); % regenerate particles
end

end

參考文獻：