OpenCV中實現了粒子濾波的代碼，位置在c:\program files\opencv\cv\src\cvcondens.cpp文件，通過分析這個文件，可以知道庫函數中如何實現粒子濾波過程的。
首先是從手冊上拷貝的粒子濾波跟蹤器的數據結構：
typedef struct CvConDensation
{
int MP; // 測量向量的維數： Dimension of measurement vector
int DP; // 狀態向量的維數： Dimension of state vector
float* DynamMatr; // 線性動態系統矩陣：Matrix of the linear Dynamics system
float* State; // 狀態向量： Vector of State
int SamplesNum; // 粒子數： Number of the Samples
float** flSamples; // 粒子向量數組： array of the Sample Vectors
float** flNewSamples; // 粒子向量臨時數組： temporary array of the Sample Vectors
float* flConfidence; // 每個粒子的置信度(譯者注：也就是粒子的權值)：Confidence
for each Sample
float* flCumulative; // 權值的累計： Cumulative confidence
float* Temp; // 臨時向量：Temporary vector
float* RandomSample; // 用來更新粒子集的隨機向量： RandomVector to update sample set
CvRandState* RandS; // 產生隨機向量的結構數組： Array of structures to generate random vectors
} CvConDensation;
與粒子濾波相關的幾個函數：
cvCreateConDensation：用於構造上述濾波器數據結構
cvReleaseConDensation：釋放濾波器
cvConDensInitSampleSet：初始化粒子集
cvConDensUpdateByTime：更新粒子集
下面着重對這幾個函數進行分析。
CV_IMPLCvConDensation*cvCreateConDensation(intDP,intMP,intSamplesNum )
{
inti;
CvConDensation *CD = 0;
CV_FUNCNAME("cvCreateConDensation" );
__BEGIN__;
if(DP < 0 ||MP < 0 ||SamplesNum < 0 )
CV_ERROR(CV_StsOutOfRange,"" );
/* allocating memory for the structure */
CV_CALL(CD = (CvConDensation *)cvAlloc(sizeof(CvConDensation )));
/* setting structure params */
CD->SamplesNum =SamplesNum;
CD->DP =DP;
CD->MP =MP;
/* allocating memory for structure fields */
CV_CALL(CD->flSamples = (float **)cvAlloc(sizeof(float * ) *SamplesNum ));
CV_CALL(CD->flNewSamples = (float **)cvAlloc(sizeof(float * ) *SamplesNum ));
CV_CALL(CD->flSamples[0] = (float *)cvAlloc(sizeof(float ) *SamplesNum *DP ));
CV_CALL(CD->flNewSamples[0] = (float *)cvAlloc(sizeof(float ) *SamplesNum *DP ));
/* setting pointers in pointer's arrays */
for(i = 1;i <SamplesNum;i++ )
{
CD->flSamples[i] =CD->flSamples[i - 1] +DP;
CD->flNewSamples[i] =CD->flNewSamples[i - 1] +DP;
}
CV_CALL(CD->State = (float *)cvAlloc(sizeof(float ) *DP ));
CV_CALL(CD->DynamMatr = (float *)cvAlloc(sizeof(float ) *DP *DP ));
CV_CALL(CD->flConfidence = (float *)cvAlloc(sizeof(float ) *SamplesNum ));
CV_CALL(CD->flCumulative = (float *)cvAlloc(sizeof(float ) *SamplesNum ));
CV_CALL(CD->RandS = (CvRandState *)cvAlloc(sizeof(CvRandState ) *DP ));
CV_CALL(CD->Temp = (float *)cvAlloc(sizeof(float ) *DP ));
CV_CALL(CD->RandomSample = (float *)cvAlloc(sizeof(float ) *DP ));
/* Returning created structure */
__END__;
returnCD;
}
輸入參數分別是系統狀態向量維數、測量向量維數以及粒子個數，然後根據這些參數爲濾波器相應結構分配空間。
CV_IMPLvoid
cvReleaseConDensation(CvConDensation ** ConDensation )
{
CV_FUNCNAME("cvReleaseConDensation" );
__BEGIN__;
CvConDensation *CD = *ConDensation;
if( !ConDensation )
CV_ERROR(CV_StsNullPtr,"" );
if( !CD )
EXIT;
/* freeing the memory */
cvFree( &CD->State );
cvFree( &CD->DynamMatr);
cvFree( &CD->flConfidence );
cvFree( &CD->flCumulative );
cvFree( &CD->flSamples[0] );
cvFree( &CD->flNewSamples[0] );
cvFree( &CD->flSamples );
cvFree( &CD->flNewSamples );
cvFree( &CD->Temp );
cvFree( &CD->RandS );
cvFree( &CD->RandomSample );
/* release structure */
cvFree(ConDensation );
__END__;
}
釋放濾波器佔用的空間，沒什麼好說的
CV_IMPLvoid
cvConDensInitSampleSet(CvConDensation * conDens,CvMat *lowerBound,CvMat *upperBound )
{
inti,j;
float *LBound;
float *UBound;
floatProb = 1.f /conDens->SamplesNum;
CV_FUNCNAME("cvConDensInitSampleSet" );
__BEGIN__;
if( !conDens || !lowerBound || !upperBound )
CV_ERROR(CV_StsNullPtr,"" );
if(CV_MAT_TYPE(lowerBound->type) !=CV_32FC1 ||
!CV_ARE_TYPES_EQ(lowerBound,upperBound) )
CV_ERROR(CV_StsBadArg,"source has not appropriate format" );
if( (lowerBound->cols != 1) || (upperBound->cols != 1) )
CV_ERROR(CV_StsBadArg,"source has not appropriate size" );
if( (lowerBound->rows !=conDens->DP) || (upperBound->rows !=conDens->DP) )
CV_ERROR(CV_StsBadArg,"source has not appropriate size" );
LBound =lowerBound->data.fl;
UBound =upperBound->data.fl;
/* Initializing the structures to create initial Sample set */
這裏根據輸入的動態範圍給每個系統狀態分配一個產生隨即數的結構
for(i = 0;i <conDens->DP;i++ )
{
cvRandInit( &(conDens->RandS[i]),
LBound[i],
UBound[i],
i );
}
/* Generating the samples */
根據產生的隨即數，爲每個粒子的每個系統狀態分配初始值，並將每個粒子的置信度設置爲相同的1/n
for(j = 0;j <conDens->SamplesNum;j++ )
{
for(i = 0;i <conDens->DP;i++ )
{
cvbRand(conDens->RandS +i,conDens->flSamples[j] +i, 1 );
}
conDens->flConfidence[j] =Prob;
}
/* Reinitializes the structures to update samples randomly */
產生以後更新粒子系統狀態的隨即結構，採樣範圍爲原來初始範圍的-1/5到1/5
for(i = 0;i <conDens->DP;i++ )
{
cvRandInit( &(conDens->RandS[i]),
(LBound[i] -UBound[i]) / 5,
(UBound[i] -LBound[i]) / 5,
i);
}
__END__;
}
CV_IMPLvoid
cvConDensUpdateByTime(CvConDensation * ConDens )
{
inti,j;
floatSum = 0;
CV_FUNCNAME("cvConDensUpdateByTime" );
__BEGIN__;
if( !ConDens )
CV_ERROR(CV_StsNullPtr,"" );
/* Sets Temp to Zero */
icvSetZero_32f(ConDens->Temp,ConDens->DP, 1 );
/* Calculating the Mean */
icvScaleVector_32f是內聯函數，表示flConfidence乘flSamples，放到State裏，即求系統狀態的過程，系統狀態保存在temp中
for(i = 0;i <ConDens->SamplesNum;i++ )
{
icvScaleVector_32f(ConDens->flSamples[i],ConDens->State,ConDens->DP,
ConDens->flConfidence[i] );
icvAddVector_32f(ConDens->Temp,ConDens->State,ConDens->Temp,ConDens->DP );
Sum +=ConDens->flConfidence[i];
ConDens->flCumulative[i] =Sum;
}
/* Taking the new vector from transformation of mean by dynamics matrix */
icvScaleVector_32f(ConDens->Temp,ConDens->Temp,ConDens->DP, 1.f /Sum );
icvTransformVector_32f(ConDens->DynamMatr,ConDens->Temp,ConDens->State,ConDens->DP,
ConDens->DP );
Sum =Sum /ConDens->SamplesNum;
/* Updating the set of random samples */
重採樣，將新採樣出來的粒子保存在flNewSamples中
for(i = 0;i <ConDens->SamplesNum;i++ )
{
j = 0;
while( (ConDens->flCumulative[j] <= (float)i *Sum)&&(j<ConDens->SamplesNum-1))
{
j++;
}
icvCopyVector_32f(ConDens->flSamples[j],ConDens->DP,ConDens->flNewSamples[i] );
}
/* Adding the random-generated vector to every vector in sample set */
for(i = 0;i <ConDens->SamplesNum;i++ )
{
爲每個新粒子產生一個隨即量
for(j = 0;j <ConDens->DP;j++ )
{
cvbRand(ConDens->RandS +j,ConDens->RandomSample +j, 1 );
}
將flNewSamples加一個隨即量，保存入flSamples中
icvTransformVector_32f(ConDens->DynamMatr,ConDens->flNewSamples[i],
ConDens->flSamples[i],ConDens->DP,ConDens->DP );
icvAddVector_32f(ConDens->flSamples[i],ConDens->RandomSample,ConDens->flSamples[i],
ConDens->DP );
}
__END__;
}
更新中用到的函數如下：
（1）
CV_INLINE void icvScaleVector_32f( const float* src, float* dst,
int len, double scale )
{
int i;
for( i = 0; i < len; i++ )
dst[i] = (float)(src[i]*scale);
icvCheckVector_32f( dst, len );
}
（2）
#define icvAddMatrix_32f( src1, src2, dst, w, h ) \
icvAddVector_32f( (src1), (src2), (dst), (w)*(h))
CV_INLINE void icvAddVector_32f( const float* src1, const float* src2,
float* dst, int len )
{
int i;
for( i = 0; i < len; i++ )
dst[i] = src1[i] + src2[i];
icvCheckVector_32f( dst, len );
}
（3）
#define icvTransformVector_32f( matr, src, dst, w, h ) \
icvMulMatrix_32f( matr, w, h, src, 1, w, dst )
CV_INLINE void icvMulMatrix_32f( const float* src1, int w1, int h1,
const float* src2, int w2, int h2,
float* dst )
{
int i, j, k;
if( w1 != h2 )
{
assert(0);
return;
}
for( i = 0; i < h1; i++, src1 += w1, dst += w2 )
for( j = 0; j < w2; j++ )
{
double s = 0;
for( k = 0; k < w1; k++ )
s += src1[k]*src2[j + k*w2];
dst[j] = (float)s;
}
icvCheckVector_32f( dst, h1*w2 );
}
（4）
#define icvCopyVector_32f( src, len, dst ) memcpy((dst),(src),(len)*sizeof(float))
（5）
CV_INLINE void cvbRand( CvRandState* state, float* dst, int len )
{
CvMat mat = cvMat( 1, len, CV_32F, (void*)dst );
cvRand( state, &mat );
}
CV_INLINE void cvRand( CvRandState* state, CvArr* arr )
{
if( !state )
{
cvError( CV_StsNullPtr, "cvRand", "Null pointer to RNG state", "cvcompat.h", 0 );
return;
}
cvRandArr( &state->state, arr, state->disttype, state->param[0], state->param[1] );
}
運行完上述函數之後，經重採樣後的粒子保存在flSamples中，同時上次迭代的加權系統狀態保存在State中