粒子濾波器的Matlab實現

前言：粒子濾波器相較於卡爾曼濾波器或者UKF無跡卡爾曼濾波器而言，可以表達強非線性的變換且無需假設後驗分佈爲高斯分佈，在描述多峯分佈時具有非常大的優勢。粒子濾波器被廣泛的應用於機器人系統中，如著名的Gmapping算法便是在粒子濾波器的基礎上實現的，但是當前網絡中對粒子濾波器的描述往往淺嘗則止或長篇大論，導致學習起來往往是瞭解大概流程而不懂實際代碼實現，無法應用於自己機器人中或困於理論推導。因此本文將簡要的說明粒子濾波器的流程，然後在matlab中一步步的實現粒子濾波器，其中核心代碼僅有20行，非常便於理解從而打通由理論到實際的過程。

原理介紹

算法流程

1. 根據上一次的粒子分佈和控制命令更新粒子位置
2. 對更新後的粒子計算在該粒子的情況下，獲得傳感器觀測的概率作爲權重
3. 根據權重進行重採樣 

一、根據上一次分佈和控制指令更新粒子位置

假設某一粒子位置爲,控制命令爲,即旋轉一定角度後前進一定的距離。跟新粒子的位置如下

%控制機器人前進
function pos = move(robot_pos,set_w,set_v,control_noise)
    % 先進行旋轉,加上噪聲，並歸一化到0~pi*2 
    pos(3) = mod(robot_pos(3) + set_w + control_noise*rand(),2*pi);
    % 前進距離+噪聲
    V = set_v + control_noise*rand();
    % 座標變換，因爲前進距離是車體座標系而要求的是世界座標系下的
    pos(1) = mod(robot_pos(1) - V*sin(pos(3)),200); 
    pos(2) = mod(robot_pos(2) + V*cos(pos(3)),200); 
end

二、對更新後的粒子計算在該粒子的情況下，獲得傳感器觀測的概率作爲權重

計算權重的公式爲

對於某一時刻真實的測量值爲,而對於其中一個粒子假設也進行了一次同樣的測量根據地圖估計出測量值爲，則認爲傳感器的測量噪聲是滿足高斯分佈的，因此該粒子測量值的概率分佈爲

其中x帶入傳感器測量的真實值，而爲當前粒子根據地圖信息估計出最可能的測量值，而協方差矩陣爲測量傳感器的設定的誤差。

% 獲取機器人在當前位置觀測到對應數據的概率
function weight = get_probability(P,Z,landMark,sensor_noise)
    particle_Z = getDistance(P,landMark,sensor_noise);%根據地圖獲取理論上最有可能的觀測結果
    distance = particle_Z - Z;%計算和真實值之間的差值
    noise = zeros([1,12]) + sensor_noise*sensor_noise;%根據設定觀測噪聲，構建協方差
    sigma = diag(noise);
    %帶入多維高斯分佈的公式中，這裏去掉了歸一化常數項
    weight = exp(-1/2 * (distance)' / (sigma) * (distance));
end

三、根據權重進行重採樣 

使用如下的重採樣策略，假設由M個樣本，每個樣本的權重爲,則根據權重進行歸一化

將0~1的圓弧分成份，每一份的長度爲,然後在該圓上隨機取值，落到哪一段則選取該段對應的粒子，如圖所示當前落在2區域內，然後隨機生成前進距離，然後根據隨機距離進入了第四個粒子的範圍則選中第四個粒子，然後不斷重複直至找到M個粒子。

    % 粒子濾波：根據權重進行重採樣
    sum_weight = sum(weight);
    weight = weight./sum_weight;%歸一化權重
    max_weight = max(weight);
    index = ceil(rand()*particle_num);%開始時隨機選取一個位置
    for i = 1:particle_num %隨機採樣的過程
        beta = rand() * 2 * max_weight;%隨機生成前進距離，避免太遠處或太近，設置爲最大值的兩倍
        while beta > weight(index+1)%重採樣過程
            beta = beta - weight(index+1);
            index = mod(index + 1,particle_num-1);
        end
        temp_particle(:,i) = particle(:,index+1);
    end
    particle = temp_particle;

Matlab代碼實現

%% begin
clc;
clear;
%% set param 
map_length = 200;%場地範圍爲200m
% 路標
landMarkNum = 12;%設定12個路標
%隨機生成路標作爲地圖
landMark = [rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length
            rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length rand()*map_length];
control_noise = 5; %控制機器人運動的噪聲
sensor_noise = 10; %傳感器對地圖的測量噪聲
robot_v = 20; %機器人在20m內隨機運動
robot_w = 0;%0.1*pi;
T = 20;%進行T次迭代 
particle_num = 1000;%使用1000個粒子
particle = zeros([3,particle_num]);%粒子的位置
temp_particle = zeros([3,particle_num]);%更新粒子時的臨時變量
weight = zeros([1,particle_num]);%粒子的權重
%% 粒子濾波步驟         
% 隨機生成初始機器人的位置
robot_pos = [rand()*map_length,rand()*map_length,rand()*2*pi]';
% 隨機生成初始粒子分佈的位置
for i = 1:particle_num
    particle(:,i) = [rand()*map_length,rand()*map_length,rand()*2*pi]';
end
%% plot
plot(robot_pos(1),robot_pos(2),'b*');
xlim([0 200])
ylim([0 200])
hold on;
for i=1:landMarkNum
     plot(landMark(1,i),landMark(2,i),'go');
     hold on;
end
for i=1:particle_num
	plot(particle(1,i),particle(2,i),'y.');
	hold on;
end
for t = 1:T
    % 假設機器人一直在隨機運動
    set_v = rand()*robot_v;
    set_w = rand()*robot_w;
    % 將命令給實際的機器人，但是會有一定的誤差
    robot_pos = move(robot_pos,set_w,set_v,control_noise/10);
    % 在當前位置測量,測量同樣會有一定誤差
    Z = getDistance(robot_pos,landMark,sensor_noise);
    % 粒子濾波：根據上一次的分佈和當前指令得到預測分佈，並計算權重
    for i = 1:particle_num
        particle(:,i) = move(particle(:,i),set_w,set_v,control_noise);
        weight(i) = get_probability(particle(:,i),Z,landMark,sensor_noise);
    end
    % 粒子濾波：根據權重進行重採樣
    sum_weight = sum(weight);
    weight = weight./sum_weight;%歸一化權重
    max_weight = max(weight);
    index = ceil(rand()*particle_num);%開始時隨機選取一個位置
    for i = 1:particle_num %隨機採樣的過程
        beta = rand() * 2 * max_weight;%隨機生成前進距離，避免太遠處或太近，設置爲最大值的兩倍內
        while beta > weight(index+1)%重採樣過程
            beta = beta - weight(index+1);
            index = mod(index + 1,particle_num-1);
        end
        temp_particle(:,i) = particle(:,index+1);
    end
    particle = temp_particle;
    %% update plot
    clf;
    for i=1:particle_num
        plot(particle(1,i),particle(2,i),'r.');
        hold on;
    end
    plot(robot_pos(1),robot_pos(2),'b*');
    xlim([0 200])
    ylim([0 200])
    hold on;
    pause(0.1);
end
     
%% function define
% 獲取機器人在當前位置觀測到對應數據的概率
function weight = get_probability(P,Z,landMark,sensor_noise)
    particle_Z = getDistance(P,landMark,sensor_noise);%根據地圖獲取理論上最有可能的觀測結果
    distance = particle_Z - Z;%計算和真實值之間的差值
    noise = zeros([1,12]) + sensor_noise*sensor_noise;%根據設定觀測噪聲，構建協方差
    sigma = diag(noise);
    %帶入多維高斯分佈的公式中，這裏去掉了歸一化常數項
    weight = exp(-1/2 * (distance)' / (sigma) * (distance));
end
%計算當前機器人位置觀測到地圖中的地標位置的距離
function distance = getDistance(position,landMark,sensor_noise)
    dis = [0,0,0,0,0,0,0,0,0,0,0,0]';
    for j=1:length(landMark) %遍歷所有地標
        % 計算和當前機器人的距離
        l = sqrt((position(1) - landMark(1,j))^2 + (position(2) - landMark(2,j))^2);
        dis(j) = l + sensor_noise*rand();% 加上隨機生成的噪聲項
    end
    % 返回各距離數據
    distance = dis;
end

%控制機器人前進
function pos = move(robot_pos,set_w,set_v,control_noise)
    % 先進行旋轉,加上噪聲，並歸一化到0~pi*2 
    pos(3) = mod(robot_pos(3) + set_w + control_noise*rand(),2*pi);
    % 前進距離+噪聲
    V = set_v + control_noise*rand();
    % 座標變換，因爲前進距離是車體座標系而要求的是世界座標系下的
    pos(1) = mod(robot_pos(1) - V*sin(pos(3)),200); 
    pos(2) = mod(robot_pos(2) + V*cos(pos(3)),200); 
end