精华内容
下载资源
问答
  • 粒子滤波代码

    千次下载 热门讨论 2014-11-15 12:00:39
    压缩包中有三个粒子滤波的演示程序,一个滤波,一个目标跟踪,一个机器人定位。关于效果,大家可以先看看http://blog.csdn.net/heyijia0327/article/details/41142679。再决定是否下载。
  • Matlab关于粒子滤波代码与卡尔曼做比较-粒子滤波代码与卡尔曼做比较.rar 部分程序如下: function ParticleEx1 % Particle filter example, adapted from Gordon, Salmond, and Smith paper. x = 0.1; % ...
  • 粒子滤波代码与卡尔曼做比较

    热门讨论 2009-05-09 20:41:56
    这是用于目标跟踪的粒子滤波代码, 用matlab编写的,很有借鉴性,一维情况下, 非高斯非线性,其中将扩展卡尔曼滤波与粒子滤波进行比较,更好的说明了粒子滤波的优越性
  • 最基本的粒子滤波代码 初学有用
  • 包括基本粒子滤波,扩展卡尔曼滤波,无忌卡尔曼粒子滤波等算法
  • Matlab关于粒子滤波代码与卡尔曼做比较-untitled3.fig 部分程序如下: function ParticleEx1 % Particle filter example, adapted from Gordon, Salmond, and Smith paper. x = 0.1; % initial state Q = 1; %...
  • Matlab关于粒子滤波代码与卡尔曼做比较-untitled2.fig 部分程序如下: function ParticleEx1 % Particle filter example, adapted from Gordon, Salmond, and Smith paper. x = 0.1; % initial state Q = 1; %...
  • Matlab关于粒子滤波代码与卡尔曼做比较-untitled1.fig 部分程序如下: function ParticleEx1 % Particle filter example, adapted from Gordon, Salmond, and Smith paper. x = 0.1; % initial state Q = 1; %...
  • 粒子滤波的matlab代码
  • OpenCV中实现了粒子滤波代码,位置在c:\program files\opencv\cv\src\cvcondens.cpp文件,通过分析这个文件,可以知道库函数中如何实现粒子滤波过程的。 首先是从手册上拷贝的粒子滤波跟踪器的数据结构: ...

    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/51/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是内联函数,表示flConfidenceflSamples,放到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

    展开全文
  • matlab粒子代码粒子过滤器 地形参考导航的标准粒子滤波器、辅助粒子滤波器、混合粒子滤波器、乱序粒子滤波器的MATLAB实现 如何运行: 运行 main_OOSM.m 如果您发现此代码有帮助,请引用以下论文: Youngjoo Kim 等...
  • 粒子滤波代码详解 构建粒子和权重 创建NP个粒子和粒子的权重并初始化 px = np.matrix(np.zeros((4, NP))) # Particle store pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight 更新粒子的权重 def pf...

    粒子滤波代码详解

    构建粒子和权重

    创建NP个粒子和粒子的权重并初始化

     px = np.matrix(np.zeros((4, NP)))  # Particle store
     pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # Particle weight
    

    更新粒子的权重

    def pf_localization(px, pw, xEst, PEst, z, u):
        """
        Localization with Particle filter
        """
        #z 绝对真实位置测量得出信标的位置
        for ip in range(NP):
            #获取该粒子
            x = px[:, ip]
            #获取该粒子的权重
            w = pw[0, ip]
    
            #  Predict with ramdom input sampling
            ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
            ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
            ud = np.matrix([ud1, ud2]).T
            #更新该粒子的位置
            x = motion_model(x, ud)
    
            #  Calc Inportance Weight
            for i in range(len(z[:, 0])):
                dx = x[0, 0] - z[i, 1]
                dy = x[1, 0] - z[i, 2]
                prez = math.sqrt(dx**2 + dy**2)
                #该粒子和信标之间的位置误差 和 真实位置得出的位置误差 
                dz = prez - z[i, 0]
                #使用激光的模型 来计算权重 计算该粒子的权重 直接使用高斯模型来更新 权重做累积求和
                w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
    
            #更新该粒子在粒子群存储变量中的值
            px[:, ip] = x
            pw[0, ip] = w
        #均一化权重
        #print("srg")
        pw = pw / pw.sum()  # normalize
        #print(px)
        #print(pw.T)
        xEst = px * pw.T
        print(xEst)
        #a=input()
        PEst = calc_covariance(xEst, px, pw)
    
        px, pw = resampling(px, pw)
    
        return xEst, PEst, px, pw
    

    重采样

    def resampling(px, pw):
        """
        low variance re-sampling
        """
        #print(pw)
        #print('neff')
        Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
        if Neff < NTh:
            '''
            这里是自己根据AMCL 源代码实现的重采样的写法 下面屏蔽的是原来的写法原理如下
            重采样的核心原理,非常简单
            1,假如有5个粒子,各个粒子权重归一化以后
            [0.05 0.05 0.4 0.4 0.1] 
            2,对粒子累积求和前面并补0值 得到以下数组
            [0.0 0.05 0.1 0.5 0.9 1.0] 
            3,现在在0到1之间生成5次随机数
            0.35,0.56,0.89,0.016,0.28
            4,根据这个规则
            if r>wcum[0,ind] and r <wcum[0,ind+1] 就可以筛选出来所需要的粒子,粒子可能被重复筛选出来,总而言之,权重大的粒子被选出来的概率大
            0.35-->第2个粒子被选出 权重 0.4
            0.56-->第3个粒子被选出 权重 0.4
            0.89-->第3个粒子被选出 权重 0.4
            0.016-->第0个粒子被选出 权重 0.05
            0.35-->第2个粒子被选出 权重 0.4
            '''
            #求累积权重和 对应AMCL 具体代码在AMCL pf.c pf_update_resample 函数里面
            wcum = np.cumsum(pw)
            #前面补一个0值
            zz=np.matrix(np.array([0.0]))
            wcum = np.hstack((zz,wcum))
            
            inds = []
            for ip in range(NP):
    			#因为权重已经归一化 所以随机选择0到1以内的随机数
    			r=np.random.rand(1)
    			#print(r)
    			ind=0
    			while ind < wcum.shape[1]-1:
    				#print (ind) 寻找满足该随机数的粒子
    				if r>wcum[0,ind] and r <wcum[0,ind+1]:
    					inds.append(ind)
    				ind = ind + 1
    		        
            '''
            base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
            resampleid = base + np.random.rand(base.shape[1]) / NP
    
            inds = []
            ind = 0
            for ip in range(NP):
                while resampleid[0, ip] > wcum[0, ind]:
                    ind += 1
                inds.append(ind)
            '''
    
            px = px[:, inds]
            pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # init weight
        return px, pw
    
    

    AMCL具体代码对比如下

    //求累积和步骤
      // Build up cumulative probability table for resampling.得到一个set_a samples 的权重
      // TODO: Replace this with a more efficient procedure
      // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
      c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
      //将权重累积求和
      c[0] = 0.0;
      for(i=0;i<set_a->sample_count;i++)
        c[i+1] = c[i]+set_a->samples[i].weight;
    
    //选取粒子步骤
    // Naive discrete event sampler
          double r;
          //随机生成0到1之间的一个数字 随机采样
          r = drand48();
          for(i=0;i<set_a->sample_count;i++)
          {
            //c[i] 为 sample_a->weight 的weight 
            if((c[i] <= r) && (r < c[i+1]))
              break;
          }
          assert(i<set_a->sample_count);
    
          //随机选择出来该粒子
          sample_a = set_a->samples + i;
    
          assert(sample_a->weight > 0);
    
          // Add sample to list 将sample_a里面的pose 更新给sample_b
          sample_b->pose = sample_a->pose;
    
    #coding = utf8
    """
    
    Particle Filter localization sample
    
    author: Atsushi Sakai (@Atsushi_twi)
    
    """
    
    import numpy as np
    import math
    import matplotlib.pyplot as plt
    
    # Estimation parameter of PF
    Q = np.diag([0.1])**2  # range error
    R = np.diag([1.0, math.radians(40.0)])**2  # input error
    
    #  Simulation parameter
    Qsim = np.diag([0.2])**2
    Rsim = np.diag([1.0, math.radians(30.0)])**2
    
    DT = 0.1  # time tick [s]
    SIM_TIME = 50.0  # simulation time [s]
    MAX_RANGE = 20.0  # maximum observation range
    
    # Particle filter parameter
    NP = 100  # Number of Particle 粒子总数
    NTh = NP / 2.0  # Number of particle for re-sampling
    
    show_animation = True
    
    
    def calc_input():
        v = 1.0  # [m/s]
        yawrate = 0.1  # [rad/s]
        u = np.matrix([v, yawrate]).T
        return u
    
    
    def observation(xTrue, xd, u, RFID):
    
        xTrue = motion_model(xTrue, u)
    
        # add noise to gps x-y
        z = np.matrix(np.zeros((0, 3)))
    
        for i in range(len(RFID[:, 0])):
    
            dx = xTrue[0, 0] - RFID[i, 0]
            dy = xTrue[1, 0] - RFID[i, 1]
            d = math.sqrt(dx**2 + dy**2)
            if d <= MAX_RANGE:
                dn = d + np.random.randn() * Qsim[0, 0]  # add noise
                zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
                z = np.vstack((z, zi))
    
        # add noise to input
        ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
        ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
        ud = np.matrix([ud1, ud2]).T
    
        xd = motion_model(xd, ud)
    
        return xTrue, z, xd, ud
    
    
    def motion_model(x, u):
    
        F = np.matrix([[1.0, 0, 0, 0],
                       [0, 1.0, 0, 0],
                       [0, 0, 1.0, 0],
                       [0, 0, 0, 0]])
    
        B = np.matrix([[DT * math.cos(x[2, 0]), 0],
                       [DT * math.sin(x[2, 0]), 0],
                       [0.0, DT],
                       [1.0, 0.0]])
    
        x = F * x + B * u
    
        return x
    
    
    def gauss_likelihood(x, sigma):
        p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
            math.exp(-x ** 2 / (2 * sigma ** 2))
    
        return p
    
    
    def calc_covariance(xEst, px, pw):
        cov = np.matrix(np.zeros((3, 3)))
    
        for i in range(px.shape[1]):
            dx = (px[:, i] - xEst)[0:3]
            cov += pw[0, i] * dx * dx.T
    
        return cov
    
    
    def pf_localization(px, pw, xEst, PEst, z, u):
        """
        Localization with Particle filter
        """
        #z 绝对真实位置测量得出信标的位置
        for ip in range(NP):
            #获取该粒子
            x = px[:, ip]
            #获取该粒子的权重
            w = pw[0, ip]
    
            #  Predict with ramdom input sampling
            ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
            ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
            ud = np.matrix([ud1, ud2]).T
            #更新该粒子的位置
            x = motion_model(x, ud)
    
            #  Calc Inportance Weight
            for i in range(len(z[:, 0])):
                dx = x[0, 0] - z[i, 1]
                dy = x[1, 0] - z[i, 2]
                prez = math.sqrt(dx**2 + dy**2)
                #该粒子和信标之间的位置误差 和 真实位置得出的位置误差 
                dz = prez - z[i, 0]
                #使用激光的模型 来计算权重 计算该粒子的权重 直接使用高斯模型来更新 权重做累积求和
                w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
    
            #更新该粒子在粒子群存储变量中的值
            px[:, ip] = x
            pw[0, ip] = w
        #均一化权重
        #print("srg")
        pw = pw / pw.sum()  # normalize
        #print(px)
        #print(pw.T)
        xEst = px * pw.T
        print(xEst)
        #a=input()
        PEst = calc_covariance(xEst, px, pw)
    
        px, pw = resampling(px, pw)
    
        return xEst, PEst, px, pw
    
    
    def resampling(px, pw):
        """
        low variance re-sampling 低方差重采样
        """
    
        Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
        if Neff < NTh:
            #这里得到累计概率和
            wcum = np.cumsum(pw)
            print("wcum")
            print(wcum)
            #这里得到一个基准该概率
            base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
            print(base)
            #base.shape[1] 数组的大小 np.random.rand 随机分布 基准概率再加上后验高斯分布概率
            resampleid = base + np.random.rand(base.shape[1]) / NP
            print(resampleid)
    
            inds = []
            ind = 0
            #采样NP个粒子 旋转托盘采样 用累加和来选择大权重的粒子 
            #举个例子 目前 基准base 0.1 0.2 0.3 0.4 0.5 0.6 wcum 0.1,0.5 ,0.55,0.6,0.8,那么抽样更多的则是第二个粒子将被选中
            for ip in range(NP):
                while resampleid[0, ip] > wcum[0, ind]:
                    print(resampleid[0, ip])
                    #如果该采样累计概率大于权重累计概率 则接着寻找
                    ind += 1
                    #如果该采样累计概率小于权重累计概率 则选择该粒子
                inds.append(ind)
            #选择出来新的粒子
            px = px[:, inds]
            print(px)
            input()
            pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # init weight
    
        return px, pw
    
    
    def plot_covariance_ellipse(xEst, PEst):
        Pxy = PEst[0:2, 0:2]
        eigval, eigvec = np.linalg.eig(Pxy)
    
        if eigval[0] >= eigval[1]:
            bigind = 0
            smallind = 1
        else:
            bigind = 1
            smallind = 0
    
        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
    
        #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
        #close to 0 (~10^-20), catch these cases and set the respective variable to 0
        try: a = math.sqrt(eigval[bigind])
        except ValueError: a = 0
    
        try: b = math.sqrt(eigval[smallind])
        except ValueError: b = 0
    
        x = [a * math.cos(it) for it in t]
        y = [b * math.sin(it) for it in t]
        angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
        R = np.matrix([[math.cos(angle), math.sin(angle)],
                       [-math.sin(angle), math.cos(angle)]])
        fx = R * np.matrix([x, y])
        px = np.array(fx[0, :] + xEst[0, 0]).flatten()
        py = np.array(fx[1, :] + xEst[1, 0]).flatten()
        plt.plot(px, py, "--r")
    
    
    def main():
        print(__file__ + " start!!")
    
        time = 0.0
    
        # RFID positions [x, y]
        RFID = np.array([[10.0, 0.0],
                         [10.0, 10.0],
                         [0.0, 15.0],
                         [-5.0, 20.0]])
    
        # State Vector [x y yaw v]'
        xEst = np.matrix(np.zeros((4, 1)))
        xTrue = np.matrix(np.zeros((4, 1)))
        PEst = np.eye(4)
    
        px = np.matrix(np.zeros((4, NP)))  # Particle store
        pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # Particle weight
        print(px)
        print(pw)
        xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning
    
        # history
        hxEst = xEst
        hxTrue = xTrue
        hxDR = xTrue
    
        while SIM_TIME >= time:
            time += DT
            u = calc_input()
    
            xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
    
            xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
    
            # store data history
            hxEst = np.hstack((hxEst, xEst))
            hxDR = np.hstack((hxDR, xDR))
            hxTrue = np.hstack((hxTrue, xTrue))
    
            if show_animation:
                plt.cla()
    
                for i in range(len(z[:, 0])):
                    plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
                plt.plot(RFID[:, 0], RFID[:, 1], "*k")
                plt.plot(px[0, :], px[1, :], ".r")
                plt.plot(np.array(hxTrue[0, :]).flatten(),
                         np.array(hxTrue[1, :]).flatten(), "-b")
                plt.plot(np.array(hxDR[0, :]).flatten(),
                         np.array(hxDR[1, :]).flatten(), "-k")
                plt.plot(np.array(hxEst[0, :]).flatten(),
                         np.array(hxEst[1, :]).flatten(), "-r")
                plot_covariance_ellipse(xEst, PEst)
                plt.axis("equal")
                plt.grid(True)
                plt.pause(0.001)
    
    
    if __name__ == '__main__':
        main()
    
    
    展开全文
  • 粒子滤波代码学习

    2012-07-21 22:25:00
    粒子滤波采用将这个分布重新表示为一组加权值,或称为粒子的方法克服了这个困难。每个粒子表示一个可能的系统状态实例。换句话说,每个粒子描述了被追踪物体可能处于的一个方位。一个粒子集包含了被追踪物体最有可能...

    这个项目是由俄亥俄州立大学(OSU)一位博士生所写,http://web.engr.oregonstate.edu/~hess/,这位博士在其个人主页上对该项目进行了如下描述:
    Object tracking is a tricky problem. A general, all-purpose object tracking algorithm must deal with difficulties like camera motion, erratic object motion, cluttered backgrounds, and other moving objects. Such hurdles render general image processing techniques an inadequate solution to the object tracking problem.

    Particle filtering is a Monte Carlo sampling approach to Bayesian filtering. It has many uses but has become the state of the art in object tracking. Conceptually, a particle filtering algorithm maintains a probability distribution over the state of the system it is monitoring, in this case, the state -- location, scale, etc. -- of the object being tracked. In most cases, non-linearity and non-Gaussianity in the object's motion and likelihood models yields an intractable filtering distribution. Particle filtering overcomes this intractability by representing the distribution as a set of weighted samples, or particles. Each particle represents a possible instantiation of the state of the system. In other words, each particle describes one possible location of the object being tracked. The set of particles contains more weight at locations where the object being tracked is more likely to be. We can thus determine the most probable state of the object by finding the location in the particle filtering distribution with the highest weight.
    大致翻译如下:
    物体追踪是一个棘手的问题。一个普适的,通用的物体追踪算法必须应对诸如摄像头运动、不稳定物体的追踪、复杂的背景、以及存在其他移动物体等困难的状况。粒子滤波算法是一个采用蒙特卡罗采样进行贝叶斯滤波的方法。这种方法有许多的用途,但它已经成为进行物体追踪最好的方法。从概念上讲,一个粒子滤波算法包含一个被监视系统的状态的概率分布。在本项目中,状态就是指被追踪物体的位置、大小等等。在许多情况下,非线性和非高斯型在物体的运动和相似性建模上会得到一个难以处理的滤波分布。粒子滤波采用将这个分布重新表示为一组加权值,或称为粒子的方法克服了这个困难。每个粒子表示一个可能的系统状态实例。换句话说,每个粒子描述了被追踪物体可能处于的一个方位。一个粒子集包含了被追踪物体最有可能处于的方位。因此,我们可以通过寻找在粒子滤波分布中最大的权重来确定物体最有可能处于的状态。

    程序流程:

    1.命令行参数处理 ->
    2.设置随机数生成器环境,创建随机数生成器、并且对其初始化。->
    3.初始化视频句柄 ->
    4.取视频中的一帧进行处理 ->
      1)GRB->HSV
      2)保存当前帧在frames
      3) 判断是否为第一帧,
         若是则,
         (1)忙等用户选定欲跟踪的区域
         (2)计算相关区域直方图
         (3)得到跟踪粒子
        若不是则,
         (1)对每个粒子作变换,并计算每个粒子的权重
         (2)对粒子集合进行归一化
         (3)重新采样粒子
    4)画出粒子所代表的区域
    5.释放图像

    OpenCV学习——物体跟踪的粒子滤波算法实现之命令行参数处理

    void arg_parse( int argc, char** argv )
    {
      int i = 0;
      pname = remove_path( argv[0] );

      while( TRUE )
        {
          char* arg_check;
          int arg = getopt( argc, argv, OPTIONS );
          if( arg == -1 )
        break;

          switch( arg )
        {
        case 'h':
          usage( pname );
          exit(0);
          break;
        case 'a':
          show_all = TRUE;
          break;

        case 'o':
          export = TRUE;
          break;

        case 'p':
          if( ! optarg )
            fatal_error( "error parsing arguments at -%c\n"    \
                 "Try '%s -h' for help.", arg, pname );
          num_particles = strtol( optarg, &arg_check, 10 );
          if( arg_check == optarg  ||  *arg_check != '\0' )
            fatal_error( "-%c option requires an integer argument\n"    \
                 "Try '%s -h' for help.", arg, pname );
          break;     
        default:
          fatal_error( "-%c: invalid option\nTry '%s -h' for help.",
                   optopt, pname );
        }
        }
      if( argc - optind < 1 )
        fatal_error( "no input image specified.\nTry '%s -h' for help.", pname );
      if( argc - optind > 2 )
        fatal_error( "too many arguments.\nTry '%s -h' for help.", pname );
      vid_file = argv[optind];
    }

    作者使用Getopt这个系统函数对命令行进行解析,-h表示显示帮助,-a表示将所有粒子所代表的位置都显示出来,-o表示输出tracking的帧,-p number进行粒子数的设定,然后再最后指定要处理的视频文件。

    OpenCV学习——物体跟踪的粒子滤波算法实现之RGB-&gt;HSV

    IplImage* bgr2hsv( IplImage* bgr )
    {
    IplImage* bgr32f, * hsv;

    bgr32f = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
    hsv = cvCreateImage( cvGetSize(bgr), IPL_DEPTH_32F, 3 );
    cvConvertScale( bgr, bgr32f, 1.0 / 255.0, 0 );
    cvCvtColor( bgr32f, hsv, CV_BGR2HSV );
    cvReleaseImage( &bgr32f );
    return hsv;
    }

    程序现将图像的像素值归一化,然后使用OpenCV中的cvCvtcolor函数将图像从RGB空间转换到HSV空间。

    OpenCV学习——物体跟踪的粒子滤波算法实现之设定随机数

    gsl_rng_env_setup();//setup the enviorment of random number generator
    rng = gsl_rng_alloc( gsl_rng_mt19937 );//create a random number generator
    gsl_rng_set( rng, time(NULL) );//initializes the random number generator.

    作者使用GSL库进行随机数的产生,GSL是GNU的科学计算库,其中手册中random部分所述进行随机数生成有三个步骤:
    随机数生成器环境建立,随机数生成器的创建,随机数生成器的初始化。

    OpenCV学习——物体跟踪的粒子滤波算法实现之计算选定区域直方图

    histogram** compute_ref_histos( IplImage* frame, CvRect* regions, int n )
    {
      histogram** histos = malloc( n * sizeof( histogram* ) );
      IplImage* tmp;
      int i;

      for( i = 0; i < n; i++ )
        {
          cvSetImageROI( frame, regions[i] );//set the region of interest
          tmp = cvCreateImage( cvGetSize( frame ), IPL_DEPTH_32F, 3 );
          cvCopy( frame, tmp, NULL );
          cvResetImageROI( frame );//free the ROI
          histos[i] = calc_histogram( &tmp, 1 );//calculate the hisrogram
          normalize_histogram( histos[i] );//Normalizes a histogram so all bins sum to 1.0
          cvReleaseImage( &tmp );
        }

      return histos;
    }

    程序中先设置了一个类型为histogram的指向指针的指针,是histogram指针数组的指针,这个数组是多个选定区域的直方图数据存放的位置。然后对于每一个用户指定的区域,在第一帧中都进行了ROI区域设置,通过对ROI区域的设置取出选定区域,交给函数calc_histogram计算出直方图,并使用normalize_histogram对直方图进行归一化。
    计算直方图的函数详解如下:
    histogram* calc_histogram( IplImage** imgs, int n )
    {
      IplImage* img;
      histogram* histo;
      IplImage* h, * s, * v;
      float* hist;
      int i, r, c, bin;

      histo = malloc( sizeof(histogram) );
      histo->n = NH*NS + NV;
      hist = histo->histo;
      memset( hist, 0, histo->n * sizeof(float) );

      for( i = 0; i < n; i++ )
        {
          img = imgs[i];
          h = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
          s = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
          v = cvCreateImage( cvGetSize(img), IPL_DEPTH_32F, 1 );
          cvCvtPixToPlane( img, h, s, v, NULL );
          for( r = 0; r < img->height; r++ )
        for( c = 0; c < img->width; c++ )
          {
            bin = histo_bin( pixval32f( h, r, c ),
                     pixval32f( s, r, c ),
                     pixval32f( v, r, c ) );
            hist[bin] += 1;
          }
          cvReleaseImage( &h );
          cvReleaseImage( &s );
          cvReleaseImage( &v );
        }
      return histo;
    }

    这个函数将h、s、 v分别取出,然后以从上到下,从左到右的方式遍历以函数histo_bin的评判规则放入相应的bin中(很形象的)。函数histo_bin的评判规则详见下图:

    |----|----|----|。。。。|----|------|------|。。。。|-------|
          1NH      2NH       3NH                    NS*NH    NS*NH+1    NS*NH+2                     NS*NH+NV

    OpenCV学习——物体跟踪的粒子滤波算法实现之初始化粒子集

    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;

    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;
    }
    }

    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;
    }

    程序中的变量np是指若有多个区域n,则一个区域内的粒子数为p/n,这样粒子的总数为p。然后程序对每个区域(n个)中p/n个粒子进行初始化,三个位置坐标都为选定区域的中点,比例都为1,宽度和高度为选定区域的高度。然后又跑了个循环确定p个粒子被初始化。

    OpenCV学习——物体跟踪的粒子滤波算法实现之粒子集合变换

    particle transition( particle p, int w, int h, gsl_rng* rng )
    {
      float x, y, s;
      particle pn;
      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;
    }

    程序使用动态二阶自回归模型作为基本变换思路,变换的对象有坐标x,坐标y,和比例s。变换的x和y要符合在width和height之内的条件。

    OpenCV学习——物体跟踪的粒子滤波算法实现之粒子集重新采样

    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;
    }

    程序先使用C标准库中的qsort排序函数,按照权重,由大到小将原粒子集排序。然后将权重大的在新的粒子集中分配的多一点。

    OpenCV学习——物体跟踪的粒子滤波算法实现之权重归一化

    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;
    }

    转自:http://blog.csdn.net/gnuhpc/category/549384.aspx?PageNumber=3

     

     

     

    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_IMPL CvConDensation* cvCreateConDensation( int DP, int MP, int SamplesNum )

    {

    int i;

    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__;

    return CD;

    }

    输入参数分别是系统状态向量维数、测量向量维数以及粒子个数,然后根据这些参数为滤波器相应结构分配空间。

    CV_IMPL void

    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_IMPL void

    cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )

    {

    int i, j;

    float *LBound;

    float *UBound;

    float Prob = 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_IMPL void

    cvConDensUpdateByTime( CvConDensation * ConDens )

    {

    int i, j;

    float Sum = 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__;

    }

    运行完上述函数之后,经重采样后的粒子保存在flSamples中,同时上次迭代的加权系统状态保存在State中

    转载于:https://www.cnblogs.com/freedesert/archive/2012/07/21/2603118.html

    展开全文
  • 一、定位粒子位置 你想要对图像中的粒子进行定位和跟踪。在MATLAB里面,图像可以是任何格式的(.jpg , .tif , etc.),但是你要想很好的定位你期望的特征,此处,使用的定位功能 要与使用的算法兼容。根据经验,你...

    如何用MATLAB进行预测跟踪呢?

    一、定位粒子位置

    你想要对图像中的粒子进行定位和跟踪。在MATLAB里面,图像可以是任何格式的(.jpg , .tif , etc.),但是你要想很好的定位你期望的特征,此处,使用的定位功能 要与使用的算法兼容。根据经验,你要能够通过眼睛定位大量的点。

    这里有一副图像,图像上白色区域上有很多胶体粒子。想要读取,显示,查询这幅图像,请键入以下MATLAB命令符:

    >>a=double(imread('test.jpg'));

    >>colormap('gray'),imagesc(a);

    >>whos a

    面的图片对于定位来说是很完美的,然而想要定位的恰当,粒子与背景相比就要更加明亮。如果你的图像和上述图形很像,那就请跟着下面的步骤走。如果那些粒 在黑色背景上已经很明亮,那就跳过这步。

    >>a=255-a;

    >>colormap('gray'),imagesc(a);

    现在,我们要使用一个宏。首先,我们要对这幅图像进行空间滤波。要完成这个,请输入以下命令:

    >>b=bpass(a,1,10);

    >>colormap('gray'),image(b);

    bpass是使图像平滑并减去背景的空间带通滤波器。这两个数字是以像素为单位的空间滤波长截止值。第一个参数通常总设置为“1”。第二个数字应该是你想要在像素 里面找的“blob”直径。尝试几个值,然后使用一个能够给你最好效果的那个值。

    接下来是识别bpass已经找到的blob作为特征。你可能需要先使用宏pkfnd.

    >>pk=pkfnd(b,60,11);

    这里给出的所有峰的位置都是阈值高于此处给出的值60。这个数字将取决于你最终的带通图像的外观。一种大致的估计明显特征的方法如下所示:

    >>max(max(b)

    这里给出的第二个数字11,大致是以像素为单位的平均要素的直径。这个参数对于噪音来说是有帮助的。如果你有噪音数据,请阅读pkfnd里面的前言。对于整个代码 而言,确定你阅读了完整的文档。

    >>cnt=cntrd(b,pk,15)

    >>whos cnt


    这些基本上就是,你成功的定位了,或者预先跟踪了单张图片。既然你会了单张图像的,那你肯定会多张图像的了。你需要了解关于for循环更多的知识来完成这项工 作。

    为了确保你已经找到特征子像素的精度,请查阅IDL教程(点击打开链接)。检查子像素特征位置的命令特别简单,如下所示:

    >>hist(mod(cnt(:,1),1),20);

    这将导致x位置的模数为1的直方图,如果你有足够的特征,并且它们不是单像素偏置,则该直方图应该看起来比较平坦。

    二、将粒子位置和前面的轨迹连接起来

    如果你所使用的数据集包含x、y,甚至z轴,那么你可以根据时间的变化进行粒子位置跟踪。如果你使用IDL进行跟踪,那么转换到MATLAB应该非常简单,因为宏是IDL 版本的副本。

    首先,你应该阅读track的文档。这对于你将你的位置列表转换为要求的格式很重要。一旦你做了这些,你就会非常的清楚。只需要调用适合你的参数轨迹。这里有个 例子:

    >>tr=track(pos_lst,3);

    pos_lst是要被考虑垂直连接的每个帧的粒子位置和时间戳的列表。通过在函数调用中传递一个附加结构、参数,你可以调整在跟踪中的重要参数:

    >>tr=track(pos_lst,3,param);

    阅读track的文档了解更多的参数,打开文档的命令为:

    >>help track

    注:文中所需函数的代码可以在参考链接里面下载到。

    参考链接:

    http://site.physics.georgetown.edu/matlab/

    http://site.physics.georgetown.edu/matlab/tutorial.html

    展开全文
  • matlab粒子代码MATLAB 机器人定位示例 此代码与提交给 EEE 百科全书的论文相关联: 论文题目:机器人定位:简介。 作者:Shoudong Huang 和 Gamini Dissanayake(悉尼科技大学) 对于 EKF 本地化示例,运行Robot_...
  • 粒子滤波代码

    2016-05-08 21:43:15
    粒子滤波代码
  • 粒子滤波matlab代码

    热门讨论 2013-04-15 20:22:40
    粒子滤波matlab代码,能够运行。Pf粒子滤波实现的目标跟踪程序,可实现针对非高斯噪声情况下的跟踪
  • 粒子滤波全套代码

    2016-08-21 21:20:34
    粒子滤波全套代码http://bbs.sciencenet.cn/thread-1306074-1-1.html
  • matlab粒子滤波代码

    2013-04-06 19:51:52
    matlab粒子滤波代码,方面您应用粒子滤波
  • 给出以个标准的粒子滤波的MATLAB代码实现方案,希望能帮助初学者更好的理解粒子滤波,请多多指教

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 2,693
精华内容 1,077
关键字:

粒子滤波代码