粒子滤波的MATLAB实现

粒子滤波算法源于Montecarlo的思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样的分布近似来表示P(x)。因此，采用此一思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman滤波只能处理高斯分布的概率问题。他的一大优势也在于此。

再来看对任意如下的状态方程：

x(t)为t时刻状态       x(t)=f(x(t-1),u(t),w(t)) (1)     状态转移方程，u(t)为控制量，w(t) 为模型噪声

y(t)=h(x(t),e(t)) (2)                  观测方程，e(t)为观测噪声

其中的x(t)为t时刻状态，u(t)为控制量，w(t) 和e(t)分别为模型噪声和观测噪声。(1)当然是状态转移方程，(2)是观测方程。那么对于这么一个问题粒子滤波怎么来从观测y(t)，和x(t-1),u(t) 滤出真实状态x(t)呢？

看看滤波的预估阶段：粒子滤波首先根据x(t-1) 和他的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西）。

进入校正阶段来：有了预测粒子，当然不是所有的预测粒子都能得到我们的时间观测值y，越是接近真实状态的粒子，当然获得越有可能获得观测值y。于是对所有的粒子得有个评价了，这个评价就是一个条件概率P(y|xi),直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。对所有粒子都进行这么一个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。好了预测信息融合在粒子的分布中，观测信息又融合在了每一粒子的权重中。

最后采用重采样算法，去除低权值的粒子，复制高权值的粒子。所得当然是需要的真实状态x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布了。

下一轮滤波，再将重采样过后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。

初始状态的问题：可以认为x(0)在全状态空间内平均分布。于是初始采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够获的高权值。重采样算法去除低权值的，将下一轮滤波的考虑重点缩小到高权值粒子附近。

下面是我最后改写和精简的一个粒子滤波Matlab算法

x = 0.1; % initial state                      初始状态

Q = 1; % process noise covariance                         过程噪声方差

R = 1; % measurement noise covariance                 测量噪声方差

tf = 50; % simulation length                                    模拟长度

N = 100; % number of particles in the particle filter                   颗粒过滤器中的粒子数

xhat = x;     %xhat=x=0.1

P = 2;

xhatPart = x;    %xhatPart=x=0.1

% Initialize the particle filter. 初始化粒子滤波，xpart值用来在不同时刻生成粒子

for i = 1 : N

xpart(i) = x + sqrt(P) * randn;   %  randn产生标准正态分布的随机数或矩阵的函数。

end      %初始化xpart(i)为生成的100个随机粒子

xArr = [x];    %xArr=x=0.1

xhatPartArr = [xhatPart];   %xhatPartArr = [xhatPart]=0.1

close all;

for k = 1 : tf %tf为时间长度，k可以理解为时间轴上的k时刻

% System simulation系统仿真

% x数据为时刻k的真实状态值

x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态方程(1)

y = x^2 / 20 + sqrt(R) * randn;%观测方程(2)。观测方程是在观测值和待估参数之间建立的函数关系式。

% Particle filter 生成100个粒子并根据预测和观测值差值计算各个粒子的权重

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;

ypart = xpartminus(i)^2 / 20;

vhat = y - ypart; %观测和预测的差

q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); %根据差值给出100个粒子对应的权重

end

% Normalize the likelihood of each a priori estimate.       每一个先验估计正常化的可能性

qsum = sum(q);

for i = 1 : N

q(i) = q(i) / qsum;%归一化权重，较大的权重除以qsum不为零，大部分较小的权重除以qsum后为零

end

% Resample.         重新取样

for i = 1 : N

u = rand; % uniform random number between 0 and 1      0和1之间的均匀随机数

qtempsum = 0;

for j = 1 : N

qtempsum = qtempsum + q(j);

if qtempsum >= u

%重采样对低权重进行剔除，同时保留高权重，防止退化的办法

xpart(i) = xpartminus(j);

break;

end

end

end

% The particle filter estimate is the mean of the particles.      粒子滤波的估计是颗粒的平均值

xhatPart = mean(xpart); %经过粒子滤波处理后的均值

% Plot the estimated pdf's at a specific time.      绘制在特定的时间估计的概率密度函数

if k == 20

% Particle filter pdf

pdf = zeros(81,1);

for m = -40 : 40

for i = 1 : N

if (m <= xpart(i)) && (xpart(i) < m+1)

%pdf为概率密度函数，这里是xpart(i)值落在[m, m+1)上的次数

pdf(m+41) = pdf(m+41) + 1;

end

end

end

figure;

m = -40 : 40;

%此图1绘制k==20时刻xpart(i)区间分布密度

plot(m, pdf/N , 'r');

hold;

title('Estimated pdf at k=20');

disp(['min, max xpart(i) at k = 20: ', num2str(min(xpart)), ', ', num2str(max(xpart))]);

end

% Save data in arrays for later plotting

xArr = [xArr x];

xhatPartArr = [xhatPartArr xhatPart];

end

t = 0 : tf;

figure;

plot(t, xArr, 'b.', t, xhatPartArr, 'g'); %此图2对应xArr为真值，xhatPartArr为粒子滤波值

xlabel('time step'); ylabel('state');

legend('True state', 'Particle filter estimate');

参考网页：

http://blog.sina.com.cn/s/blog_400d94220101bkc1.html