卡尔曼滤波、马尔科夫模型、粒子滤波、TSP问题知识点回顾

前面有小结了概率论、线性代数、现代控制理论的一些知识点，这边再来回顾下之前看过了关于卡尔曼滤波、马尔科夫模型、粒子滤波、动态规划中的TSP问题，这边也只是知其形，便于日后应用到一些实际案例中。

一.卡尔曼滤波

这边只是记录要点，便于快速回忆起来，可以从如下5个公式来入手。

所以在代码初始化的时候要先初始化状态真实值和后验误差协方差矩阵

主要可参考博客

一文看懂卡尔曼滤波（附全网最详细公式推导） - 知乎

其它博客可参考

【基础理论】卡尔曼滤波 - 知乎

卡尔曼滤波的五大公式及python代码示例_卡尔曼滤波算法-CSDN博客

二. 隐马尔可夫模型

主要可以参考如下博客

隐马尔可夫模型（HMM）-CSDN博客

帖子中的推导是针对某个观测序列出现的概率，其是要求出所有可能的情况，故是在求和计算。此用的是前向算法。主要思路就是：比如观测序列是{红，白，白}，隐状态集合是{第一个盒子，第二个盒子，第三个盒子}。那么在算到第3个观测序列是白的时候，要考虑这是从第一个盒子、第二个盒子、第三个盒子拿到的白色的情况，同时也要结合第2序列是从第一个盒子中取出白色，第二个盒子中取出白色，第三个盒子中取出白色这三种情况来计算。可以看到计算过程只需要依赖前一个观测序列的值。

从上面可以看到状态转移变化，是按照状态转移矩阵的列来取值的。该帖子也说明了隐马尔可夫模型解决几个常见问题。

如下帖子有更为详细的推导

https://www.cnblogs.com/skyme/p/4651331.html

在根据观测序列推出最有可能的状态序列时候，提到了Viterbi algorithm算法。此贴也用前向算法推了某个观测序列出现的可能性。

其它几篇博客可以稍微参考下

马尔可夫模型概念-CSDN博客

马尔可夫链（Markov Chain）是什么鬼 - 知乎

隐马尔可夫模型（HMM）

一文带你了解隐马尔可夫模型（含详细推导） - 知乎

三. 粒子滤波

粒子滤波大概原理可以是在图像上，或者目标区域附近随机分布一些粒子。然后状态转移计算新的粒子，然后计算这些粒子的权重，并根据粒子权重，重新分布粒子（重采样），再继续状态转移，继续循环计算。可主要参考如下帖子

https://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html

主要代码如下：

 while( frame = cvQueryFrame( video ) )
    {
      hsv_frame = bgr2hsv( frame );
      frames[i] = cvClone( frame );

      /* allow user to select object to be tracked in the first frame */
      if( i == 0 )
	{
	  w = frame->width;
	  h = frame->height;
	  fprintf( stderr, "Select object region to track\n" );
	  while( num_objects == 0 )
	    {
	      num_objects = get_regions( frame, &regions );
	      if( num_objects == 0 )
		fprintf( stderr, "Please select a object\n" );
	    }

	  /* compute reference histograms and distribute particles */
	  ref_histos = compute_ref_histos( hsv_frame, regions, num_objects );
	  if( export )
	    export_ref_histos( ref_histos, num_objects );
	  particles = init_distribution( regions, ref_histos,
					 num_objects, num_particles );
	}
      else
	{
	  /* perform prediction and measurement for each particle */
	  for( j = 0; j < num_particles; j++ )
	    {
	      particles[j] = transition( particles[j], w, h, rng );
	      s = particles[j].s;
	      particles[j].w = likelihood( hsv_frame, cvRound(particles[j].y),
					   cvRound( particles[j].x ),
					   cvRound( particles[j].width * s ),
					   cvRound( particles[j].height * s ),
					   particles[j].histo );
	    }
	  
	  /* normalize weights and resample a set of unweighted particles */
	  normalize_weights( particles, num_particles );
	  new_particles = resample( particles, num_particles );
	  free( particles );
	  particles = new_particles;
	}

init_distribution函数代码如下：

particle* init_distribution( CvRect* regions, histogram** histos, int n, int p)
{
  particle* particles;
  int np;
  float x, y;
  int i, j, width, height, k = 0;
  
  particles = malloc( p * sizeof( particle ) );
  np = p / n;

  /* create particles at the centers of each of n regions */
  for( i = 0; i < n; i++ )
    {
      width = regions[i].width;
      height = regions[i].height;
      x = regions[i].x + width / 2;
      y = regions[i].y + height / 2;
      for( j = 0; j < np; j++ )
	{
	  particles[k].x0 = particles[k].xp = particles[k].x = x;
	  particles[k].y0 = particles[k].yp = particles[k].y = y;
	  particles[k].sp = particles[k].s = 1.0;
	  particles[k].width = width;
	  particles[k].height = height;
	  particles[k].histo = histos[i];
	  particles[k++].w = 0;
	}
    }

  /* make sure to create exactly p particles */
  i = 0;
  while( k < p )
    {
      width = regions[i].width;
      height = regions[i].height;
      x = regions[i].x + width / 2;
      y = regions[i].y + height / 2;
      particles[k].x0 = particles[k].xp = particles[k].x = x;
      particles[k].y0 = particles[k].yp = particles[k].y = y;
      particles[k].sp = particles[k].s = 1.0;
      particles[k].width = width;
      particles[k].height = height;
      particles[k].histo = histos[i];
      particles[k++].w = 0;
      i = ( i + 1 ) % n;
    }

  return particles;
}

其中particle类的定义如下：

typedef 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 */
  histogram* histo; /**< reference histogram describing region being tracked */
  float w;          /**< weight */
} particle;

transition函数中代码

particle transition( particle p, int w, int h, gsl_rng* rng )
{
  float x, y, s;
  particle pn;
  
  /* sample new state using second-order autoregressive dynamics */
  x = A1 * ( p.x - p.x0 ) + A2 * ( p.xp - p.x0 ) +
    B0 * gsl_ran_gaussian( rng, TRANS_X_STD ) + p.x0;
  pn.x = MAX( 0.0, MIN( (float)w - 1.0, x ) );
  y = A1 * ( p.y - p.y0 ) + A2 * ( p.yp - p.y0 ) +
    B0 * gsl_ran_gaussian( rng, TRANS_Y_STD ) + p.y0;
  pn.y = MAX( 0.0, MIN( (float)h - 1.0, y ) );
  s = A1 * ( p.s - 1.0 ) + A2 * ( p.sp - 1.0 ) +
    B0 * gsl_ran_gaussian( rng, TRANS_S_STD ) + 1.0;
  pn.s = MAX( 0.1, s );
  pn.xp = p.x;
  pn.yp = p.y;
  pn.sp = p.s;
  pn.x0 = p.x0;
  pn.y0 = p.y0;
  pn.width = p.width;
  pn.height = p.height;
  pn.histo = p.histo;
  pn.w = 0;

  return pn;
}

likehood函数中代码

float likelihood( IplImage* img, int r, int c,
		  int w, int h, histogram* ref_histo )
{
  IplImage* tmp;
  histogram* histo;
  float d_sq;

  /* extract region around (r,c) and compute and normalize its histogram */
  cvSetImageROI( img, cvRect( c - w / 2, r - h / 2, w, h ) );
  tmp = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 3 );
  cvCopy( img, tmp, NULL );
  cvResetImageROI( img );
  histo = calc_histogram( &tmp, 1 );
  cvReleaseImage( &tmp );
  normalize_histogram( histo );

  /* compute likelihood as e^{\lambda D^2(h, h^*)} */
  d_sq = histo_dist_sq( histo, ref_histo );
  free( histo );
  return exp( -LAMBDA * d_sq );
}

normalize_weights函数中代码

void normalize_weights( particle* particles, int n )
{
  float sum = 0;
  int i;

  for( i = 0; i < n; i++ )
    sum += particles[i].w;
  for( i = 0; i < n; i++ )
    particles[i].w /= sum;
}

resample函数中代码如下，这边的意思是根据权重来产生和该粒子相同的新粒子的个数，那么权重大的就会多复制一些粒子，由于一些粒子不产生新粒子了，那么有可能k的值还不足n个，那么剩余差的粒子统一由最大的粒子来复制。

particle* resample( particle* particles, int n )
{
  particle* new_particles;
  int i, j, np, k = 0;

  qsort( particles, n, sizeof( particle ), &particle_cmp );
  new_particles = malloc( n * sizeof( particle ) );
  for( i = 0; i < n; i++ )
    {
      np = cvRound( particles[i].w * n );
      for( j = 0; j < np; j++ )
	{
	  new_particles[k++] = particles[i];
	  if( k == n )
	    goto exit;
	}
    }
  while( k < n )
    new_particles[k++] = particles[0];

 exit:
  return new_particles;
}

车辆（十）——粒子滤波 - 知乎

此贴中观测值可用于和预测值求相似性，以便于计算粒子的权重。如下贴也说明了观测值的作用。

【滤波】粒子滤波（PF）-CSDN博客

如下贴能够比较好的介绍一个实际应用

通俗理解：卡尔曼滤波和粒子滤波 - 知乎

OpenCV3学习（12.5） opencv实现粒子滤波目标跟踪_opencv粒子光-CSDN博客

四. 旅行商问题（动态规划）

主要参考如下贴

旅行商问题（动态规划方法，超级详细的）-CSDN博客

通过如下图可以快速回忆，在数据结构表示点集合上也有小技巧，是用二进制位来表示的。