二維粒子濾波純代碼

% 參數設置
N = 100;   %粒子總數
Q = 5;      %過程噪聲
R = 5;      %測量噪聲
T = 20;     %測量時間
theta = pi/T;       %旋轉角度
distance = 80/T;    %每次走的距離
WorldSize = 100;    %世界大小
X = zeros(2, T);    %存儲系統狀態
Z = zeros(2, T);    %存儲系統的觀測狀態
P = zeros(2, N);    %建立粒子群
PCenter = zeros(2, T);  %所有粒子的中心位置
w = zeros(N, 1);         %每個粒子的權重
err = zeros(1,T);     %誤差
X(:, 1) = [50; 20];     %初始系統狀態
%wgn（m,n,p）產生一個m行n列的高斯白噪聲的矩陣，p以dBW為單位指定輸出噪聲的強度。
Z(:, 1) = [50; 20] + wgn(2, 1, 10*log10(R));    %初始系統的觀測狀態

%初始化粒子群
for i = 1 : N
    P(:, i) = [WorldSize*rand; WorldSize*rand];%在worldSize區域內隨機生成N個粒子
    dist = norm(P(:, i)-Z(:, 1));     %與測量位置相差的距離，用於估算該粒子的權重
    %由於上面已經隨機生成了N個粒子，現在將其與真實的測量值z進行比較，越接近則權重越大，或者說差值越小權重越大
    %這里的權重計算是關於p(z/x)的分布，即觀測方程的分布，假設觀測噪聲滿足高斯分布，那么w（i）=p(z/x)
    w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求權重
end
PCenter(:, 1) = sum(P, 2) / N;      %所有粒子的幾何中心位置

%%
err(1) = norm(X(:, 1) - PCenter(:, 1));     %粒子幾何中心與系統真實狀態的誤差
figure(1);
%利用set(gca,'propertyname','propertyvalue'......)命令可以調整圖形的坐標屬性。
%propertyname和property value分別為坐標屬性名稱（不區分字母大小寫）及對應的屬性值。
set(gca,'FontSize',10);
hold on%使當前及圖形保持而不被刷新，准備接受此后將繪制的新曲線
plot(X(1, 1), X(2, 1), 'r.', 'markersize',30)   %系統狀態位置
%axis[xmin,xmax,ymin,ymax]設定坐標范圍，必須滿足xmin<xmax，后面同理
axis([0 100 0 100]);
plot(P(1, :), P(2, :), 'kx', 'markersize',5);   %繪出各個粒子位置
plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的幾何中心位置
%圖形標注函數legend(string1,string2,string3...),在當前圖中添加圖例

legend('True State', 'Particles', 'The Center of Particles');
%添加圖形標題命令title('string'),在當前坐標系的頂部加一個文本串string,作為該圖形的標題
title('Initial State');
%使當前軸及圖形不再具備不被刷新的性質
hold off

%%
%開始運動
for k = 2 : T

    %模擬一個弧線運動的狀態
    %wgn（m,n,p）產生一個m行n列的高斯白噪聲的矩陣，p以dBW為單位指定輸出噪聲的強度。
    X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q));     %狀態方程
    Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R));     %觀測方程 

    %粒子濾波
    %預測
    for i = 1 : N
        %將之前生成的粒子帶入到狀態方程，進行下一步狀態的預測
        P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));
        dist = norm(P(:, i)-Z(:, k));     %與測量位置相差的距離，用於估算該粒子的權重
        %由於上面已經隨機生成了N個粒子，現在將其與真實的測量值z進行比較，越接近則權重越大，或者說差值越小權重越大
        %這里的權重計算是關於p(z/x)的分布，即觀測方程的分布，假設觀測噪聲滿足高斯分布，那么w（i）=p(z/x)
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求權重
    end
%歸一化權重
    wsum = sum(w);
    for i = 1 : N
        w(i) = w(i) / wsum;
    end

    %重采樣（更新）
    for i = 1 : N
        wmax = 2 * max(w) * rand;  %另一種重采樣規則,獲取權重當中的最大值與均勻分布在0-1之間數的乘積再乘以2作為后面判斷選出大權重粒子的依據
        %randi()函數生成均勻分布的偽隨機整數，范圍為imin--imax，如果沒指定imin，則默認為1
        %r = randi([imin,imax],...)返回一個在[imin,imax]范圍內的偽隨機整數
        index = randi(N, 1);
        %在這里隨機產生N個粒子當中的某個粒子，然后選擇該粒子的權值，判斷是否是大的粒子權重，是的話就把該粒子選出來
        while(wmax > w(index))
            %使vmax遞減，不然的話單個粒子的權重是不可能大於vmax的值的
            wmax = wmax - w(index);
            index = index + 1;
            %如果從某個隨機的粒子開始找，沒找到從該粒子開始到最后的粒子權值總和大於vmax，那么就重新從第一個粒子開始查找
            if index > N
                index = 1;
            end          
        end
        P(:, i) = P(:, index);     %從上面的index中得到新粒子
    end
    %sum(X,2)表示把X按行求和;如果是sum(X),那就是按列求和
    PCenter(:, k) = sum(P, 2) / N;      %所有粒子的幾何中心位置

    %計算誤差
    err(k) = norm(X(:, k) - PCenter(:, k));     %粒子幾何中心與系統真實狀態的誤差

    figure(2);
    set(gca,'FontSize',12);
    %clf; 用來清除圖形的命令。一般在畫圖之前用
    clf;
    hold on
    plot(X(1, k), X(2, k), 'r.', 'markersize',50);  %系統狀態位置
    axis([0 100 0 100]);
    plot(P(1, :), P(2, :), 'k.', 'markersize',5);   %各個粒子位置
    plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置
    legend('True State', 'Particle', 'The Center of Particles');
    hold off
    pause(0.1);
end

%%
figure(3);
set(gca,'FontSize',12);
plot(X(1,:), X(2,:), 'r', Z(1,:), Z(2,:), 'g', PCenter(1,:), PCenter(2,:), 'b-');
axis([0 100 0 100]);
legend('True State', 'Measurement', 'Particle Filter');
xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20);

%%
figure(4);
set(gca,'FontSize',12);
plot(err,'.-');
xlabel('t', 'FontSize', 20);
title('The err');