一、基于粒子濾波污染源定位簡介
粒子濾波定位算法是目前最精準定位可移動物體的位置，由于水域的流動，工業固體廢物污染源很可能随着水流移動位置，基于粒子濾波算法将污染物定位分為預測、測量以及重新采樣可大大提高定位準确率[10]。粒子濾波算法的定位過程首先根據水流速度以及前一時刻水域的污染濃度預測出目前水域污染濃度最高的區域，其次對每個可能為污染源的位置進行權重計算，最後将所有污染源權重進行篩選，剔除權值較小的污染源，留下權值最高的污染源，并重複訓練直到擷取唯一一個權值最高的污染源，即實作工業固體廢物污染定位。
1 預測階段
在此階段中首先設定出所需粒子個數，并将所有粒子平分到受到污染的水域中，基于粒子濾波算法得出提議分布，當污染源在時間t-1中的定位是Xt-1(xt-1,yt-1,θt-1),則時間t的污染源定位是Xt(xt,yt,θt),污染源的運動速度即為水流速度，則工業固體廢物污染源在時間t到t-1之間位置的變化表達式(5)為：
首先利用錄影機和雷射掃描器在水域污染濃度較高的區域進行掃描，假設錄影機以及雷射掃描器擷取到的固體點特征角度表達式(6)為：
為将雷射掃描器角度資料與錄影機中的物體角度資訊進行中和，需要對錄影機中的物體點角度資料進行旋轉變換根據角度資訊公式将兩種裝置之間的角度資訊進行轉換，且轉換時必須滿足誤差正态分布，則轉換表達式(7)為：
根據式(9)将兩種裝置的廢物樣本內建到提議分布内，進而使得提議分布函數更加集中，即樣本範圍更小，更容易對污染源進行定位，此時的提議分布函數表達式(9)為：
上式即為根據狀态轉移函數得出的粒子濾波狀态提議分布。2 測量階段
在此階段直接将雷射掃描資料當成輸入，進而擷取工業固體廢物的位置資訊，則通過雷射掃描器位置轉換生成裝置與工業固體廢物之間的幾何關系表達式(10)為：
式中，xt代表裝置的具體位置，θ代表裝置掃描物體的角度，d代表裝置與觀測到的物體之間的直線距離，β代表物體的角度。在雷射資料中擷取疑似為工業固體廢物污染源的觀測值，并對其中的各個粒子進行權重評價，其表達式(11)為：
根據粒子的權重判斷粒子是否為真的工業固體廢物。
3 重采樣階段
根據每個粒子的權重大小決定是否保留該粒子，設定一個可接受範圍，判斷粒子是否在此範圍内，為保證粒子不變，複制權重較大的粒子填補空缺位置，重組粒子後将所有粒子輸入狀态轉移函數内，并反複進行訓練，保證最終污染源定位的準确性，實作工業固體廢物污染源的定位。
二、部分源代碼
%粒子濾波(定位運動軌迹)
%在二維空間,假設運動物體的一組(非線性)運動位置、速度、加速度資料,用粒子濾波方法進行處理
clc,clear,close all
%% 參數設定
N = 200; %粒子總數
Q = 5; %過程噪聲(控制誤差) 狀态轉移方程中使用
R = 5; %測量噪聲 由真實位置疊加測量噪聲得到測量位置
T = 10; %測量時間(總步數)
theta = pi/T; %旋轉角度
distance = 80/T; %每次走的距離(步長)
WorldSize = 100; %世界大小
均值為0且距離大于0 是以權重随着距離增加沿高斯曲線右側遞減
w(i) = (1 / sqrt® / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);
end
PCenter(:, 1) = sum(P, 2) / N;%t=1時刻(初始時刻)所有粒子的幾何中心位置
% 初始狀态(t=1)畫圖
err(1,1) = norm(X(:, 1) - PCenter(:, 1));%粒子群幾何中心與系統真實狀态的誤差
err(2,1) = wgn(1, 1, 10log10®);
figure(1);
hold on
set(0,‘defaultfigurecolor’,‘w’)
plot(X(1, 1), X(2, 1), ‘r.’, ‘markersize’,30) %真實的初始狀态位置(紅點表示)
%grid on
axis([0 100 0 100]);
set(gca,‘XTick’,0:10:100) %改變x軸坐标間隔顯示 這裡間隔為10
set(gca,‘YTick’,0:10:100) %改變y軸坐标間隔顯示 這裡間隔為10
plot(P(1, 😃, P(2, 😃, ‘k.’, ‘markersize’,5); %各個粒子位置(N個黑點)
plot(PCenter(1, 1), PCenter(2, 1), ‘b.’, ‘markersize’,25); %所有粒子的中心位置(藍點表示)
legend(‘真實位置’, ‘粒子群’, ‘粒子群的幾何中心’);
title(‘初始狀态’);
hold off
%% 開始運動
for k = 2 : T %從t=2到T
%模拟一個弧線運動的狀态
X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10log10(Q)); %狀态方程
Z(:, k) = X(:, k) + wgn(2, 1, 10log10®); %觀測方程(狀态上疊加測量的高斯噪聲)
%粒子濾波
%預測
for i = 1 : N
P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10log10(Q));%粒子群帶入狀态方程
dist = norm(P(:, i)-Z(:, k)); %粒子群中各粒子 與 測量位置 的距離
w(i) = (1 / sqrt® / 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; %另一種重采樣規則
index = randi(N, 1);%生成一個在[1(預設值),N]之間均勻分布的僞随機整數
while(wmax > w(index))
wmax = wmax - w(index);
index = index + 1;
if index > N
index = 1;
end
end
Pnext(:, i) = P(:, index); %得到新粒子放入臨時集Pnext
end
P=Pnext;%用臨時集Pnext更新粒子集P
PCenter(:, k) = sum(P, 2) / N; %重采樣後所有粒子的中心位置
%計算誤差
err(1,k) = norm(X(:, k) - PCenter(:, k)); %粒子幾何中心與系統真實狀态的誤差
err(2,k) = norm(X(:, k) - Z(:, k));
%畫圖
figure(2);
set(0,‘defaultfigurecolor’,‘w’)
clf;%清空figure(2)中的圖像 以便循環重新畫
hold on
plot(X(1, k), X(2, k), ‘r.’, ‘markersize’,30); %系統狀态位置
plot(P(1, 😃, P(2, 😃, ‘k.’, ‘markersize’,5); %各個粒子位置
plot(PCenter(1, k), PCenter(2, k), ‘b.’, ‘markersize’,25); %所有粒子的中心位置
axis([0 100 0 100]);
title(‘運動過程’);
legend(‘真實狀态’, ‘粒子群’, ‘粒子群的幾何中心’);
hold off
pause(0.1);%停0.1s開始下次疊代
end
%% 繪制軌迹
figure(3);
set(0,‘defaultfigurecolor’,‘w’)
plot(X(1,:), X(2,:), ‘r.-’, Z(1,:), Z(2,:), ‘g.-’, PCenter(1,:), PCenter(2,:), ‘b.-’);
axis([0 100 0 100]);
set(gca,‘XTick’,0:10:100) %改變x軸坐标間隔顯示 這裡間隔為10
set(gca,‘YTick’,0:10:100) %改變y軸坐标間隔顯示 這裡間隔為10
legend(‘真實軌迹’, ‘測量軌迹’, ‘粒子群幾何中心軌迹’);
xlabel(‘橫坐标 x’); ylabel(‘縱坐标 y’);
%% 繪制誤差
figure(4);
set(0,‘defaultfigurecolor’,‘w’)
%set(gca,‘FontSize’,12);%設定圖示字型大小
plot(err(1,:),‘b.-’);%err1為各時刻 真實位置與粒子群中心的幾何距離
hold on
plot(err(2,:),‘r.-’);%err2為各時刻 真實位置與測量位置的幾何距離
hold off
xlabel(‘步數 t’);
legend(‘粒子群誤差’, ‘測量誤差’);
title(‘真實位置與粒子群中心的集合距離’); 三、運作結果
四、matlab版本及參考文獻
1 matlab版本
2014a