# 【直觀理解】粒子濾波 原理及實現

``````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')``````