該博文集成了幾個重要的參考博客，首先感謝這些博主的講解和實現，因此是轉載，不是原創。
一. 首先從通俗易懂的層面來理解一下粒子濾波，主要是博主(飲水思源)的博客。
粒子濾波可以先分爲幾個主要的階段：初始化階段 ---> 預測階段--->矯正階段--->重採樣--->濾波
初始化階段：主要就是選定粒子數量。也就是博主所說的放狗去搜索目標；放狗的方式有很多中，一種是讓他們均勻分佈，第二種是讓他們按照高斯分佈，即可能性大的地方就多放一點。一般來講，比較簡單的實現都是先讓粒子們均勻分佈在一個區域，但是在實際應用中，比如目標跟蹤，SLAM中的特徵跟蹤都是先讓這些粒子們呈高斯分佈。
預測階段：說的是預測，其實這個階段就是對粒子們的返回數據進行分析，比如在目標跟蹤中，我的特徵是一個向量，那麼每個粒子（狗狗）就會返回一個同樣大小的向量回來，那麼我可以計算這些向量之間的相似度；當然在實際應用中，爲了方便比較，所有粒子的相似度之和最好等於1，因此我們需要對這些返回的相似度做歸一化處理。
決策階段： 在這個階段，首先要知道每個狗狗身上都帶着什麼，狗狗看到的東西，以及狗狗們的可信度；對應到粒子上也就是每個粒子的返回值(或返回向量)以及這個返回值的置信度；當所有的狗狗們都帶着消息過來時，主要就要有所選擇，這個部分類似於決策樹，可以選擇相信置信度最大的分支，也可以考慮所有分支的信息；粒子濾波在這裏使用的就是第二種，綜合考慮所有的信息，那麼最終的結果，也就是預測值x_hat就是這些 信息×置信度 之和；如果你需要預測的變量是座標，那麼預測值x_hat = p1*x1 + p2*x2+...pn*xn; x1..xn是每個狗狗返回的座標值，p1...pn是每個狗狗消息的可信度；
重採樣階段：在上一步中得到的數值其實還是有很大的誤差，所以接下來要做的是進一步的估計，依然是讓狗狗們去搜索信息，但是怎樣去分佈狗狗們呢？還是按照之前的那種均勻分佈嗎？這次肯定要換別的方法，也就是按照狗狗的可信度，可信度高的狗狗所報告的位置附近就多派一點狗狗過去，而其他可信度比較低的地方，就少派一點過去；也就是博主們所說的根據重要性重採樣；Sampling Importance Resampling；
最後，可以再次重複地進行決策階段和重採樣階段，直到最終的狗狗們的反饋結果符合條件(誤差值較小等等)，這個時候，根據權重和置信度計算出來的值，也就是我們最終的預測值；
接下來看一個博主的matlab實現：
clc;
clear all;
close all;
x = 0; %初始值
R = 1;
Q = 1;
tf = 100; %跟蹤時長
N = 100; %粒子個數
P = 2;
xhatPart = x;
for i = 1 : N
xpart(i) = x + sqrt(P) * randn;%初始狀態服從0均值，方差爲sqrt(P)的高斯分佈
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
for k = 1 : tf
x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %k時刻真實值
y = x^2 / 20 + sqrt(R) * randn; %k時刻觀測值
for i = 1 : N
xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) ...
+ 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%採樣獲得N個粒子
ypart = xpartminus(i)^2 / 20;%每個粒子對應觀測值
vhat = y - ypart;%與真實觀測之間的似然
q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
%每個粒子的似然即相似度
end
qsum = sum(q);
for i = 1 : N
q(i) = q(i) / qsum;%權值歸一化
end
%----重採樣階段
for i = 1 : N %根據權值重新採樣
u = rand;
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xpart(i) = xpartminus(j);
break;
end
end
end
xhatPart = mean(xpart);
%最後的狀態估計值即爲N個粒子的平均值，這裏經過重新採樣後各個粒子的權值相同
xArr = [xArr x];
yArr = [yArr y];
% xhatArr = [xhatArr xhat];
PArr = [PArr P];
xhatPartArr = [xhatPartArr xhatPart];
end
t = 0 : tf;
figure;
plot(t, xArr, 'b-.', t, xhatPartArr, 'k-');
legend('Real Value','Estimated Value');
set(gca,'FontSize',10);
xlabel('time step');
ylabel('state');
title('Particle filter')
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
figure;
plot(t,abs(xArr-xhatPartArr),'b');
title('The error of PF')
