粒子滤波

标签：

基本思想

所谓粒子滤波就是指：通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。采用数学语言描述如下: 对于平稳的随机过程, 假定k - 1 时刻系统的后验概率密度为p ( xk-1 zk-1 ) , 依据一定原则选取n 个随机样本点, k 时刻获得测量信息后, 经过状态和时间更新过程, n 个粒子的后验概率密度可近似为p ( xk zk ) 。随着粒子数目的增加, 粒子的概率密度函数逐渐逼近状态的概率密度函数, 粒子滤波估计即达到了最优贝叶斯估计的效果。

粒子滤波算法摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约条件, 并在一定程度上解决了粒子数样本匮乏问题, 因此近年来该算法在许多领域得到成功应用。

贝叶斯滤波

蒙特卡洛近似思想

蒙特卡洛积分是一项用随机釆样来估算积分值的技术，简单说就是以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样及其相应的权值来近似表示P(x)。

然而，在计算机问世以前，随机试验却受到一定限制，这是因为要使计算结果的准确度足够高，需要进行的试验次数相当大。因此人们都认为随机试验要用人工进行冗长的计算，这实际上是不可能的。

随着电子计算机的出现，利用电子计算机可以实现大量的随机抽样试验，使得用随机试验方法解决实际问题才有了能。

值，而解的精确度可用估计值的标准误差来表示。 

重要性采样 (Importance Sampling)

这是一种蒙特卡洛方法(Monte Carlo)。在有限的采样次数内，尽量让采样点覆盖对积分贡献很大的点。该方法目的并不是用来产生一个样本的，而是求一个函数的定积分的，只是因为该定积分的求法是通过对另一个叫容易采集分布的随机采用得到的。

序贯重要性采样(Sequential Importance Sainpling,SIS)

基于序贯蒙特卡洛模拟方法的粒子滤波算法存在采样粒子权值退化问题以及计算量过大的问题,在实际计算中,经过数次迭代,只有少数粒子的权值较大,其余粒子的权值较小甚至可以忽略不计,粒子权值的方差随着时间的推移而增大,状态空间中的有效粒子数减少,随着无效粒子数量的增加,使得大量的计算消耗在对估计后验滤波概率分布几乎不起作用的粒子上,使得计算量增大且估计性能下降。

SIS算法的具体步骤如下:

1.系统初始化

k = 0,随机提取N个样本点

并给采样粒子赋予相应的初始权值为

2.抽取N个样本

在时刻k=1,2,…,L通过随机釆样的方法从以下分布抽取N个样本

3.逐点计算对应的p(xk|xk-1)和p(zk|xk)

4.更新权值

该时刻的采样粒子点的权值按下式计算

3.粒子权值归一化

权值退化

由于采样粒子权值退化问题在基于序贯重要性采样的粒子滤波算法中广泛存在,在经过数次迭代之后,许多粒子的权值变得很小,大量的计算浪费在小权值的粒子上,从而影响系统效率。目前降低该问题影响的最有效方法是选择重要性函数和采用重采样方法

重要性函数的选择

重采样

重新采样获得的样本可以认为是一系列独立同分布的样本，其权重是一样的，均设为1/N。

采样贫乏

由于包含了大量重复的粒子（这些粒子具有高权值被多次选中），重采样导致了粒子多样性的丧失，此类问题在小噪声的环境下更加突出。事实上如果系统噪声非常小的话，所有粒子会在几次迭代之后崩塌为一个单独的粒子。

经典粒子滤波算法步骤

1.初始化：

取k＝0，按p(x0)抽取N个样本点，i＝1,…,N。

2.重要性采样：

令，其中i＝1,…,N。

3.计算权值：

若采用一步转移后验状态分布，该式可简化为。

4.归一化权值：

t=   

=/t

5.重采样：根据各自归一化权值的大小复制/舍弃样本，得到N个近似服从分布的样本。令＝1/N，i＝1,…,N。

6.输出结果：算法的输出是粒子集，用它可以近似表示后验概率和函数的期望

7.K=K+1,重复2步至6步。

粒子滤波其实质是利用一系列随机抽取的样本，也就是粒子，来替代状态的后验概率分布。当粒子的个数变得足够大时，通过这样的随机抽样方法就可以得到状态后验分布很好的近似。

<span style="font-size:18px;">function ParticleEx1 
 
% Particle filter example, adapted from Gordon, Salmond, and Smith paper. 
 
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; 
P = 2; 
xhatPart = x; 
 
% Initialize the particle filter. 
for i = 1 : N 
    xpart(i) = x + sqrt(P) * randn; 
end 
 
xArr = [x]; 
yArr = [x^2 / 20 + sqrt(R) * randn]; 
xhatArr = [x]; 
PArr = [P]; 
xhatPartArr = [xhatPart]; 
 
close all; 
 
for k = 1 : tf 
    % System simulation 
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%状态方程 
    y = x^2 / 20 + sqrt(R) * randn;%观测方程 
    % Extended Kalman filter 
    F = 0.5 + 25 * (1 - xhat^2) / (1 + xhat^2)^2; 
    P = F * P * F' + Q; 
    H = xhat / 10; 
    K = P * H' * (H * P * H' + R)^(-1); 
    xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));%预测 
    xhat = xhat + K * (y - xhat^2 / 20);%更新 
    P = (1 - K * H) * P; 
    % Particle filter 
    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); 
    end 
    % Normalize the likelihood of each a priori estimate. 
    qsum = sum(q); 
    for i = 1 : N 
        q(i) = q(i) / qsum;%归一化权重 
    end 
    % Resample. 
    for i = 1 : N 
        u = rand; % uniform random number between 0 and 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(m+41) = pdf(m+41) + 1; 
                end 
            end 
        end 
        figure; 
        m = -40 : 40; 
        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))]); 
        % Kalman filter pdf 
        pdf = (1 / sqrt(P) / sqrt(2*pi)) .* exp(-(m - xhat).^2 / 2 / P); 
        plot(m, pdf, 'b'); 
        legend('Particle filter', 'Kalman filter'); 
    end 
    % Save data in arrays for later plotting 
    xArr = [xArr x]; 
    yArr = [yArr y]; 
    xhatArr = [xhatArr xhat]; 
    PArr = [PArr P]; 
    xhatPartArr = [xhatPartArr xhatPart]; 
end 
 
t = 0 : tf; 
 
%figure; 
%plot(t, xArr); 
%ylabel('true state'); 
 
figure; 
plot(t, xArr, 'b.', t, xhatArr, 'k-', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:'); 
axis([0 tf -40 40]); 
set(gca,'FontSize',12); set(gcf,'Color','White');  
xlabel('time step'); ylabel('state'); 
legend('True state', 'EKF estimate', '95% confidence region');  
 
figure; 
plot(t, xArr, 'b.', t, xhatPartArr, 'k-'); 
set(gca,'FontSize',12); set(gcf,'Color','White');  
xlabel('time step'); ylabel('state'); 
legend('True state', 'Particle filter estimate');  
 
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf); 
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf); 
disp(['Kalman filter RMS error = ', num2str(xhatRMS)]); 
disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]); </span>

下面是我实现的跟踪结果，白色十字代表粒子，绿色大十字代表目标最可能的位置。

如何理解 重要性采样(importance sampling)

 Particle Filter Tutorial 粒子滤波：从推导到应用

粒子滤波小结

粒子滤波

标签：

原文地址：http://blog.csdn.net/u014568921/article/details/46574017