自适应粒子滤波及实现：如何计算粒子数

最新推荐文章于 2025-04-02 19:56:04 发布

置顶 SLAM科研汪

最新推荐文章于 2025-04-02 19:56:04 发布

阅读量6.6k

点赞数 7

分类专栏：粒子滤波文章标签：粒子滤波目标跟踪自适应粒子

本文链接：https://blog.csdn.net/Mark_SLAM/article/details/81266527

版权

粒子滤波专栏收录该内容

1 篇文章

订阅专栏

本文介绍了一种自适应粒子滤波算法，该算法能够根据实际情况动态调整粒子数量，提高跟踪精度。文中详细阐述了算法原理，包括如何利用KL距离评估粒子分布与真实分布之间的差距，并给出具体的实现代码。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

自适应粒子滤波：如何计算粒子数

在传统的基于图像的目标跟踪和检测算法中，粒子滤波经常被提到。但是，翻各种资料的时候，大段大段的公式，让人根本看不下去。其实，粒子滤波的基本思想还是很直接明了的，但是非常有用。下面推荐一个自己认为写得不错的内容，作为入门。

怎样从实际场景上理解粒子滤波（Particle Filter）

下面评论里边有代码和例程，可以一块学习。

下面博客的内容，打算接着上面那个基本的粒子滤波思想，写一下如何自适应计算滤波的粒子个数。主要参考论文如下：

[1]Adapting the Sample Size in Particle Filters Through KLD-Sampling

如何自适应地计算粒子数呢？

大家都知道，粒子滤波是用粒子来近似后验分布，也就是说粒子数越多的区域，那么真实状态落在这个区域的概率就越大。换句话说，越多的采样粒子数，那么是不是就能更好地反映真实情况呢？答案是这样的。

但是，现在存在一个问题：究竟在实际的场景中需要多少的粒子才认为反映出了真实情况？这里用一个计算公式来计算估计出的概率分布 p() 和真实概率分布 q() 之间的距离，也就是KL-distance。

$\fn_jvn K(p,q)=\sum_{x}{p(x)\log {p(x)\over q(x)}})\ \ \ (1)$

现在有衡量两个概率密度分布之间的距离公式啦，后面只需要用这个公式计算我们得到的概率分布和真实分布就可以啦。

等等，真实分布如何得到啊。。嗯，那是不可能得到的。

好，接下来开始是作者如何用各种办法来计算这个距离的，因为真实分布根本得不到呀。

各种假设：假设现在有n个粒子，分别来自于k个不同的bin（翻译成箱子？有点像图像梯度直方图里边那个），而且每一个bin里边分别有 $X_i\ (i=1,2,3,...k)$ 个粒子。当然，所有bin里边的粒子数加起来肯定等于n。相当于把n个粒子分别放到k个不同的bin里边去。

稍微画了个图。假设上图的红色线条是一维概率密度函数，我们用蓝色的直方图去近似这个曲线。现在图中一共有5个bin（5个蓝色条），每个bin里边放的粒子数为对应的纵坐标。一个直观的感受是：bin的数目越多，则可以更好地近似这条曲线。

还是假设：假设每一个bin的真实概率分布为 $p=(p_1,p_2,...,p_k)$ ,而此时由我们采样出来的概率为 $\widehat{p}=(\widehat{p}_1,\widehat{p}_2,...,\widehat{p}_k)$ .那么每一个 $\widehat{p_i}$ 是怎么计算的呢？很简单，这个bin里的粒子数目 ${X_i}$ 除以总数n.

$\widehat{p_i}={{X_i}\over n} \ \ \rightarrow \ \ \ {X_i}=n\widehat{p_i}$

统计数据的似然比就可以按下面的公式计算啦。

$\log \lambda_n=\sum_{j=1}^k{X_j\log{ {\widehat p_j}\over p_j}}$

可以把Xj替换啦，一定要变形成为第一个计算距离的公式(1)，所以得到下面

$\log \lambda_n=n\sum_{j=1}^k{{\widehat p_j}\log{ {\widehat p_j}\over p_j}} \ \ (2)$

按公式(1)整理得到如下：

$\log \lambda_n=nK(\widehat{p},p)\ \ \ (3)$

~~不知道这个公式怎么居中。。就这样子吧。~~这里，虽然不知道真实的概率分布p，但是他们之间的距离已经可以用等式左边的来替代啦。

根据论文J.A. Rice. Mathematical Statistics and Data Analysis. Duxbury Press, second edition, 1995.可以得到

$2\log (\lambda_n)\rightarrow _d\chi ^2_{k-1}\ \ as\ \ n\rightarrow \infty$

哈哈，公式(3) 等式左边是满足这个卡方分布的。（链接是wiki的资料）

重点来了！！

我们现在只需要期待两个概率之间的距离小于某一阈值 $\varepsilon$ 就可以啦。好，那么，我们构建概率来表达。

$P(K(\widehat p,p)\leqslant\varepsilon )\\ =P(2nK(\widehat p,p)\leqslant 2n\varepsilon ) \\=P(2n\log\lambda _n\leqslant 2n\varepsilon )\\=P(\chi ^2_{k-1} \leqslant 2n\varepsilon)\ \ \(4)$

根据卡方分布的性质，明显有以下等式（这个具体去看卡方分布的性质吧）

$\fn_jvn P(\chi ^2_{K-1}\leqslant \chi ^2_{K-1,1-\delta } )=1-\delta\ \ \ (5)$

结合公式（4）来看，只要 $\chi ^2_{K-1,1-\delta }=2n\varepsilon$ 就可以满足等式（5）啦，等式（5）可以由查表知道此时的概率是多少。

$n={1 \over {2\varepsilon }}\chi ^2_{k-1,1-\delta }\ \ \ \(6)$

公式6又怎么计算呢，很好。有论文给出了近似计算等式。~~这个公式我截图算了，有点难打~~。如下：

$\fn_jvn n={1 \over {2\varepsilon }}\chi ^2_{k-1,1-\delta }\doteq {{k-1}\over {2\varepsilon }} \left \{ 1-{2\over{9{k-1}}}+\sqrt{2\over{9(k-1))}}z_{1-\delta } \right\}^3\ \ \ (7)$

举个例子，当后面的 $z_{1-\delta}=2,\varepsilon =0.01$ 时，大概公式（5）计算出来的概率为98%。

也就是说我们采样出来的概率分布置信度为98%.

以上就是如何计算自适应粒子个数的全部推到导啦。公式7中的k可以用收集直方图或者用kd-tree的方式得到。

主要的思想就是用卡方分布去近似计算估计概率分布和真实概率分布之间的距离。

以上。

扔一个论文里边的算法流程，下面按这个流程编写自适应算法。

接下来是实现部分啦，打算采用C++编程实现基本粒子滤波和自适应粒子数的计算。

等待更新...

Update 2018/9/3

把之前的代码整理了一下，现在更新一下代码具体怎么实现。基本代码是基于Rob Hess源码，具体分析看下面这篇博客。

https://blog.csdn.net/hujingshuang/article/details/45535423

整个工程需要完成这样一个任务，在视频的第一幅图像上，用鼠标画一个矩阵框，然后用粒子滤波算法跟踪矩形框内的目标。

基本思想：

1）提取初始矩形框内目标的特征信息，这里直接采用颜色直方图来表示。

将图像BGR空间转换到HSV空间。
提取矩形框内颜色直方图。
归一化。

2）提取每个粒子所在位置的颜色直方图，归一化后与1）中目标信息计算距离，距离越小认为目标相似度高，权重大。

3）每次根据权重调整粒子数目的分布，简而言之，权重越大的地方认为目标出现的可能性越大，那就在附近多放一些粒子。（SIR 思想）

最简单的就是直接在该位置按高斯分布放置就行。
new_x=x+normal_distribution(0.0,variance)
new_y=y+normal_distribution(0.0,variance)

4）每次放完粒子之后，计算它的权重，把权重最大的粒子画出来，认为找到了目标。

5）循环以上过程。

好了，基本的这个流程就是这样子。

首先，定义基本粒子如下：

struct histogram
{
    float histo[NH*NS + NV];   /**< histogram array */
    int n;                     /**< length of histogram array */
};

struct particle {
  float x;          /**< current x coordinate */
  float y;          /**< current y coordinate */
  float s;          /**< scale */
  float xp;         /**< previous x coordinate */
  float yp;         /**< previous y coordinate */
  float sp;         /**< previous scale */
  float x0;         /**< original x coordinate */
  float y0;         /**< original y coordinate */
  int width;        /**< original width of region described by particle */
  int height;       /**< original height of region described by particle */
  shared_ptr<histogram> hist; /**< reference histogram describing region being tracked */
  float w;          /**< weight */
};

解释一下，struct histogram 表示提取的直方图，很简单，就是一个浮点数组，n=NH*NS + NV表示数组大小。

每个粒子particle拥有很多信息，当前的图像坐标x，y，先前的坐标xp，yp，矩形框信息width，height，每个框的缩放比例s，当然还有这个矩形框提取得到颜色直方图啦，放在指针 hist中。

现在我们用鼠标提取第一帧图像的目标如下：

然后，得到要跟踪的目标信息，将这些信息放在minitParticle中，初始化粒子如下：

void adaptivePF::init_distribution()
{
    float normW=1.0/mN;
    float  x=minitParticle->x0;
    float y=minitParticle->y0;

    vec_w.resize(mN);
    //init_distribution
        for(int i=0;i<mN;i++)
    {

        mParticles[i].x0=x;
        mParticles[i].xp=x;
        mParticles[i].x=x;
        mParticles[i].y0=y;
        mParticles[i].yp=y;
        mParticles[i].y=y;
        mParticles[i].sp=1.0;
        mParticles[i].s=1.0;
        mParticles[i].height=minitParticle->height;
        mParticles[i].width=minitParticle->width;
        mParticles[i].hist=shared_ptr<histogram>(new histogram);
        mParticles[i].w=normW;
        mCDF[i]=i*normW;
        //mParticles_tmp
        mParticles_tmp[i].hist=shared_ptr<histogram>(new histogram);
        vec_w[i]=mParticles[i].w;
    }
    mCDF[mN]=1.0;
}

所有粒子都初始化为初始位置。然后决定下一帧图像到来时，这些粒子应该怎么运动呢，也就是action model，如下：

void adaptivePF::transition(int p_idx, int new_size)
{
    float x, y, s;
    // sample new state using second-order autoregressive dynamics 
 
    s=A1 * ( mParticles[p_idx].s - 1.0 ) + A2 * ( mParticles[p_idx].sp - 1.0 ) +B0 * Gaussian_s(generator) + 1.0;
    x=mParticles[p_idx].x+Gaussian_x(generator);
    y=mParticles[p_idx].y+Gaussian_y(generator);
    
    mParticles_tmp[new_size].x=MAX(1.0,MIN(float(mWidth-1.0),x));
    mParticles_tmp[new_size].y=MAX(1.0,MIN(float(mHeight-1.0),y));
    mParticles_tmp[new_size].s=MAX(0.1,s);
    
    mParticles_tmp[new_size].xp=mParticles[p_idx].x;
    mParticles_tmp[new_size].yp=mParticles[p_idx].y;
    mParticles_tmp[new_size].sp=mParticles[p_idx].s;
    mParticles_tmp[new_size].x0=mParticles[p_idx].x0;
    mParticles_tmp[new_size].y0=mParticles[p_idx].y0;
    mParticles_tmp[new_size].w=mParticles[p_idx].w;
    
    gsl_vector_set(bin_temp,0,mParticles_tmp[new_size].x);
    gsl_vector_set(bin_temp,1,mParticles_tmp[new_size].y);
}

很简单，直接在上一次位置处叠加高斯分布得到新位置即可。然后在新位置使用sensor model，检测该处的颜色直方图。

float adaptivePF::evaluate(int new_size)
{
    calc_histogram(mParticles_tmp[new_size]);
    normalize_histogram(mParticles_tmp[new_size]);
    mParticles_tmp[new_size].w=histo_dist_sq(mParticles_tmp[new_size].hist,ref_histo);
    vec_w[new_size]=mParticles_tmp[new_size].w;
    return mParticles_tmp[new_size].w;
}
int adaptivePF::histo_bin(float h, float  s, float  v)
{
    int hd, sd, vd;
    s=s/255.0;
    v=v/255.0;
      /* if S or V is less than its threshold, return a "colorless" bin */
  vd = MIN( (int)(v * NV / V_MAX), NV-1 );
  if( (s) < S_THRESH  ||  v < V_THRESH )
    return NH * NS + vd;
  
  /* otherwise determine "colorful" bin */
  hd = MIN( (int)(h * NH / H_MAX), NH-1 );
  sd = MIN( (int)(s * NS / S_MAX), NS-1 );
  return sd * NH + hd;
    
}
void adaptivePF::normalize_histogram(APF::particle &mm)
{
    int sum=0;
    int n=mm.hist->n;
    for(int i=0;i<n;i++)
    {
        sum+=mm.hist->histo[i];
    }
    float inv_sum=1.0/sum;
    for(int i=0;i<n;i++)
    {
      mm.hist->histo[i] *=inv_sum;
    }

}
float adaptivePF::histo_dist_sq(shared_ptr<APF::histogram> h1, shared_ptr<APF::histogram> h2)
{
       /*
    According the the Battacharyya similarity coefficient,
    
    D = \sqrt{ 1 - \sum_1^n{ \sqrt{ h_1(i) * h_2(i) } } }
  */
    float sum = 0;
    int n = h1->n;
    for(int  i = 0; i < n; i++ )
    {
        sum += sqrt( h1->histo[i]* h2->histo[i]);
    }
    return std::exp( -LAMBDA *(1.0-sum) );
}

粒子滤波最重要的两个过程：action model和sensor model就是这样。

下面是自适应部分。

直接放代码：

void adaptivePF::update()
{
    int p_idx;
    //Initialize kd-tree
    kd_tree->reset();
    mWeightSum=0;
    int new_size = 0;
    do{
        p_idx = SIR();
        transition(p_idx,new_size);
        mWeightSum+=evaluate(new_size);
        kd_tree->insert(bin_temp);
        new_size++;
    }while(new_size < min_num ||(new_size < max_num && new_size < proper_size(kd_tree->size())));
    mN=new_size;
     
    mCDF[0]=0.0;
    float invW=1.0/mWeightSum;
    for(int i=0;i<mN;i++)
    {
        mParticles_tmp[i].w=mParticles_tmp[i].w*invW;
        mCDF[i+1]=mCDF[i]+mParticles_tmp[i].w;
    }
    //swap data 
    particle* tmp=mParticles;
    mParticles=mParticles_tmp;
    mParticles_tmp=tmp;
    
}

while(new_size < min_num ||(new_size < max_num && new_size < proper_size(kd_tree->size())));

do-while这个循环控制了每次采样的粒子数目，其中kd_tree->size()就是我们公式 (7)中的k，稍后解释一下这个k怎么得到的。

$n={1 \over {2\varepsilon }}\chi ^2_{k-1,1-\delta }\doteq {{k-1}\over {2\varepsilon }} \left \{ 1-{2\over{9{k-1}}}+\sqrt{2\over{9(k-1))}}z_{1-\delta } \right\}^3\ \ \ (7)$

函数 proper_size() 完成了公式(7)的计算。

float err_bound=0.02;
float conf_quantile=2.0537489;
int adaptivePF::proper_size(int k)
{
    double t0 = 2.0f / (9 * (k-1));
    double t1 = 1.0 - t0 + sqrt(t0) * conf_quantile;
    return int((k-1)/(2*err_bound) * t1*t1*t1);
}

变量err_bound就是公式里边的 $\epsilon$ ，给定bin的大小k，就能根据公式（7）计算得到最合适的粒子数n，然后每次更新通过n来控制下次的粒子采样数目，就可以达到自适应粒子数的目的。