基于粒子滤波器的目标跟踪算法及实现

机器学习 专栏收录该内容
53 篇文章 0 订阅

原文地址:http://blog.csdn.net/jinshengtao/article/details/30970733

                  http://blog.csdn.net/jinshengtao?viewmode=contents


推荐大家看论文《An adaptive color-based particle filter》

这次我直接截图我的硕士毕业论文的第二章的一部分,应该讲得比较详细了。最后给出我当时在pudn找到的最适合学习的实现代码











代码实现:

运行方式:按P停止,在前景窗口鼠标点击目标,会自动生成外接矩形,再次按P,对该选定目标进行跟踪。

[cpp]  view plain  copy
  1. // TwoLevel.cpp : 定义控制台应用程序的入口点。  
  2. //  
  3.   
  4. /************************************************************************/  
  5. /*参考文献real-time Multiple Objects Tracking with Occlusion Handling in Dynamic Scenes  */  
  6. /************************************************************************/  
  7.   
  8. #include "stdafx.h"  
  9. #include <cv.h>  
  10. #include <cxcore.h>  
  11. #include <highgui.h>  
  12. #include <math.h>  
  13. # include <time.h>  
  14. #include <iostream>  
  15. using namespace std;  
  16.   
  17.   
  18. #define B(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3]       //B  
  19. #define G(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+1] //G  
  20. #define R(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+2] //R  
  21. #define S(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)]   
  22. #define  Num 10  //帧差的间隔  
  23. #define  T 40    //Tf  
  24. #define Re 30     //  
  25. #define ai 0.08   //学习率  
  26.   
  27. #define CONTOUR_MAX_AREA 10000  
  28. #define CONTOUR_MIN_AREA 50  
  29.   
  30. # define R_BIN      8  /* 红色分量的直方图条数 */  
  31. # define G_BIN      8  /* 绿色分量的直方图条数 */  
  32. # define B_BIN      8  /* 兰色分量的直方图条数 */   
  33.   
  34. # define R_SHIFT    5  /* 与上述直方图条数对应 */  
  35. # define G_SHIFT    5  /* 的R、G、B分量左移位数 */  
  36. # define B_SHIFT    5  /* log2( 256/8 )为移动位数 */  
  37.   
  38. /* 
  39. 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数 
  40. 算法详细描述见: 
  41. [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING. 
  42. Cambridge University Press. 1992. pp.278-279. 
  43. [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,  
  44. vol. 31, pp. 1192–1201. 
  45. */  
  46.   
  47. #define IA 16807  
  48. #define IM 2147483647  
  49. #define AM (1.0/IM)  
  50. #define IQ 127773  
  51. #define IR 2836  
  52. #define MASK 123459876  
  53.   
  54.   
  55. typedef struct __SpaceState {  /* 状态空间变量 */  
  56.     int xt;               /* x坐标位置 */  
  57.     int yt;               /* x坐标位置 */  
  58.     float v_xt;           /* x方向运动速度 */  
  59.     float v_yt;           /* y方向运动速度 */  
  60.     int Hxt;              /* x方向半窗宽 */  
  61.     int Hyt;              /* y方向半窗宽 */  
  62.     float at_dot;         /* 尺度变换速度 */  
  63. } SPACESTATE;  
  64.   
  65.   
  66. bool pause=false;//是否暂停  
  67. bool track = false;//是否跟踪  
  68. IplImage *curframe=NULL;   
  69. IplImage *pBackImg=NULL;  
  70. IplImage *pFrontImg=NULL;  
  71. IplImage *pTrackImg =NULL;  
  72. unsigned char * img;//把iplimg改到char*  便于计算  
  73. int xin,yin;//跟踪时输入的中心点  
  74. int xout,yout;//跟踪时得到的输出中心点  
  75. int Wid,Hei;//图像的大小  
  76. int WidIn,HeiIn;//输入的半宽与半高  
  77. int WidOut,HeiOut;//输出的半宽与半高  
  78.   
  79. long ran_seed = 802163120; /* 随机数种子,为全局变量,设置缺省值 */  
  80.   
  81. float DELTA_T = (float)0.05;    /* 帧频,可以为30,25,15,10等 */  
  82. int POSITION_DISTURB = 15;      /* 位置扰动幅度   */  
  83. float VELOCITY_DISTURB = 40.0;  /* 速度扰动幅值   */  
  84. float SCALE_DISTURB = 0.0;      /* 窗宽高扰动幅度 */  
  85. float SCALE_CHANGE_D = (float)0.001;   /* 尺度变换速度扰动幅度 */  
  86.   
  87. int NParticle = 75;       /* 粒子个数   */  
  88. float * ModelHist = NULL; /* 模型直方图 */  
  89. SPACESTATE * states = NULL;  /* 状态数组 */  
  90. float * weights = NULL;   /* 每个粒子的权重 */  
  91. int nbin;                 /* 直方图条数 */  
  92. float Pi_Thres = (float)0.90; /* 权重阈值   */  
  93. float Weight_Thres = (float)0.0001;  /* 最大权重阈值,用来判断是否目标丢失 */  
  94.   
  95.   
  96. /* 
  97. 设置种子数 
  98. 一般利用系统时间来进行设置,也可以直接传入一个long型整数 
  99. */  
  100. long set_seed( long setvalue )  
  101. {  
  102.     if ( setvalue != 0 ) /* 如果传入的参数setvalue!=0,设置该数为种子 */  
  103.         ran_seed = setvalue;  
  104.     else                 /* 否则,利用系统时间为种子数 */  
  105.     {  
  106.         ran_seed = time(NULL);  
  107.     }  
  108.     return( ran_seed );  
  109. }  
  110.   
  111. /* 
  112. 计算一幅图像中某个区域的彩色直方图分布 
  113. 输入参数: 
  114. int x0, y0:           指定图像区域的中心点 
  115. int Wx, Hy:           指定图像区域的半宽和半高 
  116. unsigned char * image:图像数据,按从左至右,从上至下的顺序扫描, 
  117. 颜色排列次序:RGB, RGB, ... 
  118. (或者:YUV, YUV, ...) 
  119. int W, H:             图像的宽和高 
  120. 输出参数: 
  121. float * ColorHist:    彩色直方图,颜色索引按: 
  122. i = r * G_BIN * B_BIN + g * B_BIN + b排列 
  123. int bins:             彩色直方图的条数R_BIN*G_BIN*B_BIN(这里取8x8x8=512) 
  124. */  
  125. void CalcuColorHistogram( int x0, int y0, int Wx, int Hy,   
  126.                          unsigned char * image, int W, int H,  
  127.                          float * ColorHist, int bins )  
  128. {  
  129.     int x_begin, y_begin;  /* 指定图像区域的左上角坐标 */  
  130.     int y_end, x_end;  
  131.     int x, y, i, index;  
  132.     int r, g, b;  
  133.     float k, r2, f;  
  134.     int a2;  
  135.   
  136.     for ( i = 0; i < bins; i++ )     /* 直方图各个值赋0 */  
  137.         ColorHist[i] = 0.0;  
  138.     /* 考虑特殊情况:x0, y0在图像外面,或者,Wx<=0, Hy<=0 */  
  139.     /* 此时强制令彩色直方图为0 */  
  140.     if ( ( x0 < 0 ) || (x0 >= W) || ( y0 < 0 ) || ( y0 >= H )   
  141.         || ( Wx <= 0 ) || ( Hy <= 0 ) ) return;  
  142.   
  143.     x_begin = x0 - Wx;               /* 计算实际高宽和区域起始点 */  
  144.     y_begin = y0 - Hy;  
  145.     if ( x_begin < 0 ) x_begin = 0;  
  146.     if ( y_begin < 0 ) y_begin = 0;  
  147.     x_end = x0 + Wx;  
  148.     y_end = y0 + Hy;  
  149.     if ( x_end >= W ) x_end = W-1;  
  150.     if ( y_end >= H ) y_end = H-1;  
  151.     a2 = Wx*Wx+Hy*Hy;                /* 计算核函数半径平方a^2 */  
  152.     f = 0.0;                         /* 归一化系数 */  
  153.     for ( y = y_begin; y <= y_end; y++ )  
  154.         for ( x = x_begin; x <= x_end; x++ )  
  155.         {  
  156.             r = image[(y*W+x)*3] >> R_SHIFT;   /* 计算直方图 */  
  157.             g = image[(y*W+x)*3+1] >> G_SHIFT; /*移位位数根据R、G、B条数 */  
  158.             b = image[(y*W+x)*3+2] >> B_SHIFT;  
  159.             index = r * G_BIN * B_BIN + g * B_BIN + b;  
  160.             r2 = (float)(((y-y0)*(y-y0)+(x-x0)*(x-x0))*1.0/a2); /* 计算半径平方r^2 */  
  161.             k = 1 - r2;   /* 核函数k(r) = 1-r^2, |r| < 1; 其他值 k(r) = 0 */  
  162.             f = f + k;  
  163.             ColorHist[index] = ColorHist[index] + k;  /* 计算核密度加权彩色直方图 */  
  164.         }  
  165.         for ( i = 0; i < bins; i++ )     /* 归一化直方图 */  
  166.             ColorHist[i] = ColorHist[i]/f;  
  167.   
  168.         return;  
  169. }  
  170.   
  171. /* 
  172. 计算Bhattacharyya系数 
  173. 输入参数: 
  174. float * p, * q:      两个彩色直方图密度估计 
  175. int bins:            直方图条数 
  176. 返回值: 
  177. Bhattacharyya系数 
  178. */  
  179. float CalcuBhattacharyya( float * p, float * q, int bins )  
  180. {  
  181.     int i;  
  182.     float rho;  
  183.   
  184.     rho = 0.0;  
  185.     for ( i = 0; i < bins; i++ )  
  186.         rho = (float)(rho + sqrt( p[i]*q[i] ));  
  187.   
  188.     return( rho );  
  189. }  
  190.   
  191.   
  192. /*# define RECIP_SIGMA  3.98942280401  / * 1/(sqrt(2*pi)*sigma), 这里sigma = 0.1 * /*/  
  193. # define SIGMA2       0.02           /* 2*sigma^2, 这里sigma = 0.1 */  
  194.   
  195. float CalcuWeightedPi( float rho )  
  196. {  
  197.     float pi_n, d2;  
  198.   
  199.     d2 = 1 - rho;  
  200.     //pi_n = (float)(RECIP_SIGMA * exp( - d2/SIGMA2 ));  
  201.     pi_n = (float)(exp( - d2/SIGMA2 ));  
  202.   
  203.     return( pi_n );  
  204. }  
  205.   
  206. /* 
  207. 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数 
  208. 算法详细描述见: 
  209. [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING. 
  210. Cambridge University Press. 1992. pp.278-279. 
  211. [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,  
  212. vol. 31, pp. 1192–1201. 
  213. */  
  214.   
  215. float ran0(long *idum)  
  216. {  
  217.     long k;  
  218.     float ans;  
  219.   
  220.     /* *idum ^= MASK;*/      /* XORing with MASK allows use of zero and other */  
  221.     k=(*idum)/IQ;            /* simple bit patterns for idum.                 */  
  222.     *idum=IA*(*idum-k*IQ)-IR*k;  /* Compute idum=(IA*idum) % IM without over- */  
  223.     if (*idum < 0) *idum += IM;  /* flows by Schrage’s method.               */  
  224.     ans=AM*(*idum);          /* Convert idum to a floating result.            */  
  225.     /* *idum ^= MASK;*/      /* Unmask before return.                         */  
  226.     return ans;  
  227. }  
  228.   
  229.   
  230. /* 
  231. 获得一个[0,1]之间均匀分布的随机数 
  232. */  
  233. float rand0_1()  
  234. {  
  235.     return( ran0( &ran_seed ) );  
  236. }  
  237.   
  238.   
  239.   
  240. /* 
  241. 获得一个x - N(u,sigma)Gaussian分布的随机数 
  242. */  
  243. float randGaussian( float u, float sigma )  
  244. {  
  245.     float x1, x2, v1, v2;  
  246.     float s = 100.0;  
  247.     float y;  
  248.   
  249.     /* 
  250.     使用筛选法产生正态分布N(0,1)的随机数(Box-Mulles方法) 
  251.     1. 产生[0,1]上均匀随机变量X1,X2 
  252.     2. 计算V1=2*X1-1,V2=2*X2-1,s=V1^2+V2^2 
  253.     3. 若s<=1,转向步骤4,否则转1 
  254.     4. 计算A=(-2ln(s)/s)^(1/2),y1=V1*A, y2=V2*A 
  255.     y1,y2为N(0,1)随机变量 
  256.     */  
  257.     while ( s > 1.0 )  
  258.     {  
  259.         x1 = rand0_1();  
  260.         x2 = rand0_1();  
  261.         v1 = 2 * x1 - 1;  
  262.         v2 = 2 * x2 - 1;  
  263.         s = v1*v1 + v2*v2;  
  264.     }  
  265.     y = (float)(sqrt( -2.0 * log(s)/s ) * v1);  
  266.     /* 
  267.     根据公式 
  268.     z = sigma * y + u 
  269.     将y变量转换成N(u,sigma)分布 
  270.     */  
  271.     return( sigma * y + u );      
  272. }  
  273.   
  274.   
  275.   
  276. /* 
  277. 初始化系统 
  278. int x0, y0:        初始给定的图像目标区域坐标 
  279. int Wx, Hy:        目标的半宽高 
  280. unsigned char * img:图像数据,RGB形式 
  281. int W, H:          图像宽高 
  282. */  
  283. int Initialize( int x0, int y0, int Wx, int Hy,  
  284.                unsigned char * img, int W, int H )  
  285. {  
  286.     int i, j;  
  287.     float rn[7];  
  288.   
  289.     set_seed( 0 ); /* 使用系统时钟作为种子,这个函数在 */  
  290.     /* 系统初始化时候要调用一次,且仅调用1次 */  
  291.     //NParticle = 75; /* 采样粒子个数 */  
  292.     //Pi_Thres = (float)0.90; /* 设置权重阈值 */  
  293.     states = new SPACESTATE [NParticle]; /* 申请状态数组的空间 */  
  294.     if ( states == NULL ) return( -2 );  
  295.     weights = new float [NParticle];     /* 申请粒子权重数组的空间 */  
  296.     if ( weights == NULL ) return( -3 );      
  297.     nbin = R_BIN * G_BIN * B_BIN; /* 确定直方图条数 */  
  298.     ModelHist = new float [nbin]; /* 申请直方图内存 */  
  299.     if ( ModelHist == NULL ) return( -1 );  
  300.   
  301.     /* 计算目标模板直方图 */  
  302.     CalcuColorHistogram( x0, y0, Wx, Hy, img, W, H, ModelHist, nbin );  
  303.   
  304.     /* 初始化粒子状态(以(x0,y0,1,1,Wx,Hy,0.1)为中心呈N(0,0.4)正态分布) */  
  305.     states[0].xt = x0;  
  306.     states[0].yt = y0;  
  307.     states[0].v_xt = (float)0.0; // 1.0  
  308.     states[0].v_yt = (float)0.0; // 1.0  
  309.     states[0].Hxt = Wx;  
  310.     states[0].Hyt = Hy;  
  311.     states[0].at_dot = (float)0.0; // 0.1  
  312.     weights[0] = (float)(1.0/NParticle); /* 0.9; */  
  313.     for ( i = 1; i < NParticle; i++ )  
  314.     {  
  315.         for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */  
  316.         states[i].xt = (int)( states[0].xt + rn[0] * Wx );  
  317.         states[i].yt = (int)( states[0].yt + rn[1] * Hy );  
  318.         states[i].v_xt = (float)( states[0].v_xt + rn[2] * VELOCITY_DISTURB );  
  319.         states[i].v_yt = (float)( states[0].v_yt + rn[3] * VELOCITY_DISTURB );  
  320.         states[i].Hxt = (int)( states[0].Hxt + rn[4] * SCALE_DISTURB );  
  321.         states[i].Hyt = (int)( states[0].Hyt + rn[5] * SCALE_DISTURB );  
  322.         states[i].at_dot = (float)( states[0].at_dot + rn[6] * SCALE_CHANGE_D );  
  323.         /* 权重统一为1/N,让每个粒子有相等的机会 */  
  324.         weights[i] = (float)(1.0/NParticle);  
  325.     }  
  326.   
  327.     return( 1 );  
  328. }  
  329.   
  330.   
  331.   
  332. /* 
  333. 计算归一化累计概率c'_i 
  334. 输入参数: 
  335. float * weight:    为一个有N个权重(概率)的数组 
  336. int N:             数组元素个数 
  337. 输出参数: 
  338. float * cumulateWeight: 为一个有N+1个累计权重的数组, 
  339. cumulateWeight[0] = 0; 
  340. */  
  341. void NormalizeCumulatedWeight( float * weight, float * cumulateWeight, int N )  
  342. {  
  343.     int i;  
  344.   
  345.     for ( i = 0; i < N+1; i++ )   
  346.         cumulateWeight[i] = 0;  
  347.     for ( i = 0; i < N; i++ )  
  348.         cumulateWeight[i+1] = cumulateWeight[i] + weight[i];  
  349.     for ( i = 0; i < N+1; i++ )  
  350.         cumulateWeight[i] = cumulateWeight[i]/ cumulateWeight[N];  
  351.   
  352.     return;  
  353. }  
  354.   
  355. /* 
  356. 折半查找,在数组NCumuWeight[N]中寻找一个最小的j,使得 
  357. NCumuWeight[j] <=v 
  358. float v:              一个给定的随机数 
  359. float * NCumuWeight:  权重数组 
  360. int N:                数组维数 
  361. 返回值: 
  362. 数组下标序号 
  363. */  
  364. int BinearySearch( float v, float * NCumuWeight, int N )  
  365. {  
  366.     int l, r, m;  
  367.   
  368.     l = 0;  r = N-1;   /* extreme left and extreme right components' indexes */  
  369.     while ( r >= l)  
  370.     {  
  371.         m = (l+r)/2;  
  372.         if ( v >= NCumuWeight[m] && v < NCumuWeight[m+1] ) return( m );  
  373.         if ( v < NCumuWeight[m] ) r = m - 1;  
  374.         else l = m + 1;  
  375.     }  
  376.     return( 0 );  
  377. }  
  378.   
  379. /* 
  380. 重新进行重要性采样 
  381. 输入参数: 
  382. float * c:          对应样本权重数组pi(n) 
  383. int N:              权重数组、重采样索引数组元素个数 
  384. 输出参数: 
  385. int * ResampleIndex:重采样索引数组 
  386. */  
  387. void ImportanceSampling( float * c, int * ResampleIndex, int N )  
  388. {  
  389.     float rnum, * cumulateWeight;  
  390.     int i, j;  
  391.   
  392.     cumulateWeight = new float [N+1]; /* 申请累计权重数组内存,大小为N+1 */  
  393.     NormalizeCumulatedWeight( c, cumulateWeight, N ); /* 计算累计权重 */  
  394.     for ( i = 0; i < N; i++ )  
  395.     {  
  396.         rnum = rand0_1();       /* 随机产生一个[0,1]间均匀分布的数 */   
  397.         j = BinearySearch( rnum, cumulateWeight, N+1 ); /* 搜索<=rnum的最小索引j */  
  398.         if ( j == N ) j--;  
  399.         ResampleIndex[i] = j;   /* 放入重采样索引数组 */       
  400.     }  
  401.   
  402.     delete cumulateWeight;  
  403.   
  404.     return;   
  405. }  
  406.   
  407. /* 
  408. 样本选择,从N个输入样本中根据权重重新挑选出N个 
  409. 输入参数: 
  410. SPACESTATE * state:     原始样本集合(共N个) 
  411. float * weight:         N个原始样本对应的权重 
  412. int N:                  样本个数 
  413. 输出参数: 
  414. SPACESTATE * state:     更新过的样本集 
  415. */  
  416. void ReSelect( SPACESTATE * state, float * weight, int N )  
  417. {  
  418.     SPACESTATE * tmpState;  
  419.     int i, * rsIdx;  
  420.   
  421.     tmpState = new SPACESTATE[N];  
  422.     rsIdx = new int[N];  
  423.   
  424.     ImportanceSampling( weight, rsIdx, N ); /* 根据权重重新采样 */  
  425.     for ( i = 0; i < N; i++ )  
  426.         tmpState[i] = state[rsIdx[i]];//temState为临时变量,其中state[i]用state[rsIdx[i]]来代替  
  427.     for ( i = 0; i < N; i++ )  
  428.         state[i] = tmpState[i];  
  429.   
  430.     delete[] tmpState;  
  431.     delete[] rsIdx;  
  432.   
  433.     return;  
  434. }  
  435.   
  436. /* 
  437. 传播:根据系统状态方程求取状态预测量 
  438. 状态方程为: S(t) = A S(t-1) + W(t-1) 
  439. W(t-1)为高斯噪声 
  440. 输入参数: 
  441. SPACESTATE * state:      待求的状态量数组 
  442. int N:                   待求状态个数 
  443. 输出参数: 
  444. SPACESTATE * state:      更新后的预测状态量数组 
  445. */  
  446. void Propagate( SPACESTATE * state, int N)  
  447. {  
  448.     int i;  
  449.     int j;  
  450.     float rn[7];  
  451.   
  452.     /* 对每一个状态向量state[i](共N个)进行更新 */  
  453.     for ( i = 0; i < N; i++ )  /* 加入均值为0的随机高斯噪声 */  
  454.     {  
  455.         for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */  
  456.         state[i].xt = (int)(state[i].xt + state[i].v_xt * DELTA_T + rn[0] * state[i].Hxt + 0.5);  
  457.         state[i].yt = (int)(state[i].yt + state[i].v_yt * DELTA_T + rn[1] * state[i].Hyt + 0.5);  
  458.         state[i].v_xt = (float)(state[i].v_xt + rn[2] * VELOCITY_DISTURB);  
  459.         state[i].v_yt = (float)(state[i].v_yt + rn[3] * VELOCITY_DISTURB);  
  460.         state[i].Hxt = (int)(state[i].Hxt+state[i].Hxt*state[i].at_dot + rn[4] * SCALE_DISTURB + 0.5);  
  461.         state[i].Hyt = (int)(state[i].Hyt+state[i].Hyt*state[i].at_dot + rn[5] * SCALE_DISTURB + 0.5);  
  462.         state[i].at_dot = (float)(state[i].at_dot + rn[6] * SCALE_CHANGE_D);  
  463.         cvCircle(pTrackImg,cvPoint(state[i].xt,state[i].yt),3, CV_RGB(0,255,0),-1);  
  464.     }  
  465.     return;  
  466. }  
  467.   
  468. /* 
  469. 观测,根据状态集合St中的每一个采样,观测直方图,然后 
  470. 更新估计量,获得新的权重概率 
  471. 输入参数: 
  472. SPACESTATE * state:      状态量数组 
  473. int N:                   状态量数组维数 
  474. unsigned char * image:   图像数据,按从左至右,从上至下的顺序扫描, 
  475. 颜色排列次序:RGB, RGB, ...                          
  476. int W, H:                图像的宽和高 
  477. float * ObjectHist:      目标直方图 
  478. int hbins:               目标直方图条数 
  479. 输出参数: 
  480. float * weight:          更新后的权重 
  481. */  
  482. void Observe( SPACESTATE * state, float * weight, int N,  
  483.              unsigned char * image, int W, int H,  
  484.              float * ObjectHist, int hbins )  
  485. {  
  486.     int i;  
  487.     float * ColorHist;  
  488.     float rho;  
  489.   
  490.     ColorHist = new float[hbins];  
  491.   
  492.     for ( i = 0; i < N; i++ )  
  493.     {  
  494.         /* (1) 计算彩色直方图分布 */  
  495.         CalcuColorHistogram( state[i].xt, state[i].yt,state[i].Hxt, state[i].Hyt,  
  496.             image, W, H, ColorHist, hbins );  
  497.         /* (2) Bhattacharyya系数 */  
  498.         rho = CalcuBhattacharyya( ColorHist, ObjectHist, hbins );  
  499.         /* (3) 根据计算得的Bhattacharyya系数计算各个权重值 */  
  500.         weight[i] = CalcuWeightedPi( rho );       
  501.     }  
  502.   
  503.     delete ColorHist;  
  504.   
  505.     return;   
  506. }  
  507.   
  508. /* 
  509. 估计,根据权重,估计一个状态量作为跟踪输出 
  510. 输入参数: 
  511. SPACESTATE * state:      状态量数组 
  512. float * weight:          对应权重 
  513. int N:                   状态量数组维数 
  514. 输出参数: 
  515. SPACESTATE * EstState:   估计出的状态量 
  516. */  
  517. void Estimation( SPACESTATE * state, float * weight, int N,   
  518.                 SPACESTATE & EstState )  
  519. {  
  520.     int i;  
  521.     float at_dot, Hxt, Hyt, v_xt, v_yt, xt, yt;