粒子滤波

粒子滤波起源于蒙特卡洛思想，即以频率代替概率，其操作方式如下：

1.初始状态：在世界范围内，设置大量的均匀分布的粒子X(k)

2.预测阶段：根据状态转移方程，对每一个粒子都可以预测出一个新的粒子

3.校正阶段：对每一个粒子根据其与真实位置的关系设置一个评价值，越接近的评价越高

4.采样阶段：对所有的粒子进行筛选，选择得分高的粒子，但是也保留一些得分低的粒子

5滤波阶段：将得到的新粒子带入状态转移方程中，回到步骤2的过程

算法用matlab程序写出来：

N=100;

Q=10;//过程噪声

R=10;//观测噪声

T=10;

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];//真实位置

Z(:,1)=[50; 20] + wgn(2, 1, 10*log10(R));//观测位置

%%初始状态

for i=1:N

P(:, i) = [WorldSize*rand; WorldSize*rand];

dist = norm(P(:, i)-Z(:, 1)); %与测量位置相差的距离

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,'FontSize',12);

hold on

plot(X(1, 1), X(2, 1), 'r.', 'markersize',30) %系统状态位置

axis([0 100 0 100]);

plot(P(1, :), P(2, :), 'k.', 'markersize',5); %各个粒子位置

plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的中心位置

legend('True State', 'Particles', 'The Center of Particles');

title('Initial State');

hold off

%%运动之后

for k = 2 : T

%模拟一个弧线运动的状态

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)); %与测量位置相差的距离

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; %另一种重采样规则

index = randi(N, 1);

while(wmax > w(index))

wmax = wmax - w(index);

index = index + 1;

if index > N

index = 1;

end

end

P(:, i) = P(:, index); %得到新粒子

end

PCenter(:, k) = sum(P, 2) / N; %所有粒子的中心位置

%计算误差

err(k) = norm(X(:, k) - PCenter(:, k)); %粒子几何中心与系统真实状态的误差

figure(2);

set(gca,'FontSize',12);

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

这就是一个通过粒子跟真实点接近程度作为概率求解真实距离的一个例子。