粒子滤波算法

文档下载链接：https://download.csdn.net/download/OEMT_301/12069538
https://download.csdn.net/download/OEMT_301/12104738

粒子滤波算法是一种非线性的滤波方法。

其大致思路如下（这里以图像目标（人）跟踪为例）：
1、 首先在整个图像中随机初始化一些粒子点，并对每个粒子点分配权值
2、 在视频中框出待跟踪目标
3、 更新权值，增加靠近框出的目标粒子权值
4、 根据状态转移矩阵和测量数据，对粒子权重，对粒子进行重采样

粒子滤波示过程示意图
初始化图像粒子点和权重

框出待跟踪目标

更新权重，其中权重较小的直接舍弃，权值较大的粒子点进行复制，权值越大复制的粒子点越多。

经过几次权值更新，可以使得大多数粒子点处于目标框附近

根据状态转移矩阵获取新粒子点坐标（箭头相当于状态转移矩阵）
A、经过目标移动后，理想状态为如下图

B、但实际上存在一些噪声，预测结果与实际结果有偏差

因此继续重采样，利用测量结果对其进行重采样，便可得到较为准确的跟踪效果

以上为粒子滤波过程示意图

重采样

MATLAB重采样代码示意

function w_new=resample_particles(w)
w_new=w;
Neff=1/sum(w.*w);
N=length(w);
if Neff<75 %75为预先设定阈值
    for i = 1 : N
        u = rand; 
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + w(j);
            if qtempsum >= u
                w_new(i)=w(j);
                break;
            end
        end
    end
end

一维SIR粒子滤波实例

%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 系统过程噪声的协方差  (由于是一维的，这里就是方差)
x_R = 1; % 测量的协方差
T = 75;  % 共进行75次
N = 100; % 粒子数，越大效果越好，计算量也越大

%initilize our initial, prior particle distribution as a gaussian around
%the true initial value

V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
    x_P(i) = x + sqrt(V) * randn;
end

z_out = [x^2 / 20 + sqrt(x_R) * randn];  %实际测量值
x_out = [x];  %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.

for t = 1:T
    x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) +  sqrt(x_N)*randn;
    z = x^2/20 + sqrt(x_R)*randn;
    for i = 1:N
        %从先验p(x(k)|x(k-1))中采样
        x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
        %计算采样粒子的值，为后面根据似然去计算权重做铺垫
        z_update(i) = x_P_update(i)^2/20;
        %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
        P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
    end
    % 归一化.
    P_w = P_w./sum(P_w);
  
    %% Resampling
    %这里没有用博客里之前说的histc函数，不过目的和效果是一样的
    for i = 1 : N
        x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代
    end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标
    
    %状态估计，重采样以后，每个粒子的权重都变成了1/N
    x_est = mean(x_P);
    
    % Save data in arrays for later plotting
    x_out = [x_out x];
    z_out = [z_out z];
    x_est_out = [x_est_out x_est];
    
end

t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('flight position');
legend('True flight position', 'Particle filter estimate');

图像粒子跟踪实例
main.m

% Parameters
F_update = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];    % 状态转移矩阵

Npop_particles = 4000;

Xstd_rgb = 50;  % 方差
Xstd_pos = 25;  % 位置倍率
Xstd_vec = 5;   % 速度倍率

Xrgb_trgt = [255; 0; 0];    % 检测颜色

% Loading Movie
vr = VideoReader('Person.wmv');

Npix_resolution = [vr.Width vr.Height];
Nfrm_movie = floor(vr.Duration * vr.FrameRate);

% Object Tracking by Particle Filter

X = create_particles(Npix_resolution, Npop_particles); % 粒子初始化，在画面中产生均匀分布的随机粒子

for k = 1:Nfrm_movie
    
    % Getting Image
    Y_k = read(vr, k);
    
    % Forecasting
    %通过状态模型预测  这里采用的是在上一时刻基础上叠加噪声
    X = update_particles(F_update, Xstd_pos, Xstd_vec, X); 
    
    % Calculating Log Likelihood
    L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X(1:2, :), Y_k);
    
    % Resampling
    X = resample_particles(X, L);

    % Showing Image
    show_particles(X, Y_k); 
%    show_state_estimated(X, Y_k);

end

create_particles.m

function X = create_particles(Npix_resolution, Npop_particles)

X1 = randi(Npix_resolution(2), 1, Npop_particles);  % 产生一个 1 x Npop_particles 的行向量，各元素值为 1：Npix_resolution(2)之间的产生的均匀分布的随机整数 
X2 = randi(Npix_resolution(1), 1, Npop_particles);
X3 = zeros(2, Npop_particles);

X = [X1; X2; X3];

update_particles.m

function X = update_particles(F_update, Xstd_pos, Xstd_vec, X)
% X 所有粒子组成的矩阵  
% X(1:2, :) 各粒子在画面中的位置
N = size(X, 2);

X = F_update * X;       % 状态转移矩阵

X(1:2,:) = X(1:2,:) + Xstd_pos * randn(2, N);
X(3:4,:) = X(3:4,:) + Xstd_vec * randn(2, N);

calc_log_likelihood.m

扫描二维码关注公众号，回复： 9064119 查看本文章

function L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X, Y) %#codegen

Npix_h = size(Y, 1);
Npix_w = size(Y, 2);

N = size(X,2);

L = zeros(1,N);
Y = permute(Y, [3 1 2]);  %重新安排矩阵

% 高斯函数
A = -log(sqrt(2 * pi) * Xstd_rgb);
B = - 0.5 / (Xstd_rgb.^2);

X = round(X);

for k = 1:N
    % m,n 粒子k在画面中的位置
    m = X(1,k);
    n = X(2,k);
    
    % 越界判断
    I = (m >= 1 & m <= Npix_h);
    J = (n >= 1 & n <= Npix_w);
    
    if I && J
        
        C = double(Y(:, m, n));
        
        D = C - Xrgb_trgt;
        
        D2 = D' * D;   % 欧氏距离
        
        L(k) =  A + B * D2;  %高斯似然   D2 越小  L越大  注意B为负数
    else
        
        L(k) = -Inf;
    end
end

resample_particles.m

function X = resample_particles(X, L_log)

% Calculating Cumulative Distribution

L = exp(L_log - max(L_log));
Q = L / sum(L, 2);  % a = sum(L,2)  a中元素为各行向量累加值
R = cumsum(Q, 2);   % 权值累加

% Generating Random Numbers

N = size(X, 2);
T = rand(1, N); % 随机阈值

% Resampling
X_temp = zeros(size(X));
%这里没有用博客里之前说的histc函数，不过目的和效果是一样的
for i = 1 : N
    X_temp(:,i) = X(:,find(T(i) <= R,1));               % 粒子权重大的将多得到后代
end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标
X = X_temp;
% [~, I] = histc(T, R);
% X = X(:, I + 1);

C++代码：

#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <assert.h>

using namespace cv;
using namespace std;

//****************************定义粒子数目**********************************//
#define PARTICLE_NUMBER 300
#define HIST_SIZE       16

//*******************************定义粒子结构体类型************************//
typedef struct particle//关于typedef struct和struct见下文补充
{
    int orix, oriy;         //原始粒子坐标
    int x, y;               //当前粒子的坐标
    double scale;           //当前粒子窗口的尺寸
    int prex, prey;         //上一帧粒子的坐标
    double prescale;        //上一帧粒子窗口的尺寸
    Rect rect;              //当前粒子矩形窗口
    double weight;          //当前粒子权值
}PARTICLE;

bool leftButtonDownFlag=false;  //左键单击后的标志位
bool leftButtonUpFlag=false;    //左键单击后松开的标志位
Point Point_s;                  //矩形框起点
Point Point_e;                  //矩形框鼠标左键弹起来的终点
Point processPoint;             //矩形框移动的终点
bool  tracking = false;

//****有关粒子窗口变化用到的相关变量****// 目标运行预测值
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;

double *m_hist, *hist;
int p_num = 0;

vector<PARTICLE> newParticle;//定义一个新的粒子数组

/****粒子权重值的降序排列****/
// 排序法则，其中：< 升序    >降序
bool comparison(PARTICLE p1,PARTICLE p2)
{
 return p1.weight > p2.weight ;
}

void onMouse( int event, int x, int y, int flags, void *param )
{
    if(event==CV_EVENT_LBUTTONDOWN)
    {
        tracking = false;
        leftButtonDownFlag = true; //标志位
        leftButtonUpFlag = false;
        processPoint=Point(x,y);  //设置左键按下点的矩形起点
        Point_s=processPoint;
    }
    else if(event == CV_EVENT_MOUSEMOVE && leftButtonDownFlag)
    {
        processPoint=Point(x,y);
    }
    else if(event==CV_EVENT_LBUTTONUP && leftButtonDownFlag)
    {
        leftButtonDownFlag=false;
        processPoint=Point(x,y);
        Point_e=processPoint;
        leftButtonUpFlag = true;
        tracking = true;
    }
}

void init_target(Mat mould, vector<PARTICLE> &particles, Rect &rect)
{
    int q_r, q_g, q_b, q_temp;
    particles.clear();

    //初始化权值矩阵和目标直方图
    for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
    {
        m_hist[i] = 0.0;
    }
    //计算目标权值直方
    for (int i = 0;i < mould.rows; i++)
    {
        for (int j = 0;j < mould.cols; j++)
        {
            //rgb颜色空间量化为16*16*16 bins
            q_r = mould.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
            q_g = mould.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
            q_b = mould.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
            q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
            m_hist[q_temp]++;          // 颜色权重直方图
        }
    }

    // 归一化
    double m_hist_sum = 0.0;
    for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
    {
        m_hist_sum += pow(m_hist[i], 2);
    }
    m_hist_sum = sqrt(m_hist_sum);
    for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
    {
        m_hist[i] = m_hist[i]/m_hist_sum;
    }

    // 目标粒子初始化
    PARTICLE pParticle;
    for (int k=0; k<PARTICLE_NUMBER; k++)               //对于每个粒子
    {
        pParticle.x = cvRound(rect.x + 0.5*rect.width);//当前粒子的x坐标
        pParticle.y = cvRound(rect.y + 0.5*rect.height);//当前粒子的y坐标

        //粒子的原始坐标为选定矩形框(即目标)的中心
        pParticle.orix = pParticle.x;
        pParticle.oriy = pParticle.y;
        //当前粒子窗口的尺寸
        pParticle.scale = 1;//初始化为1，然后后面粒子到搜索的时候才通过计算更新

        //更新上一帧粒子的坐标
        pParticle.prex = pParticle.x;
        pParticle.prey = pParticle.y;
        //上一帧粒子窗口的尺寸
        pParticle.prescale = 1;

        //当前粒子矩形窗口
        pParticle.rect = rect;

        //当前粒子权值
        pParticle.weight = 0;//权重初始为0

        particles.push_back(pParticle);
    }
}


void My_Tracking(Mat img, vector<PARTICLE> &particles)
{
    int xpre, ypre;
    double prescale, scale;
    int x, y;
    double sum = 0.0;

    RNG rng;                        //随机数产生器
    int q_r, q_g, q_b, q_temp;
    /*计算粒子区域的直方图*/
    //初始化目标直方图
    for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
    {
        hist[i] = 0.0;
    }

    for (int k=0; k<PARTICLE_NUMBER; k++)
    {
        //当前粒子的坐标
        xpre = particles.at(k).x;
        ypre = particles.at(k).y;

        //当前粒子窗口的尺寸
        prescale = particles.at(k).scale;

        /*更新跟踪矩形框中心，即粒子中心*///使用二阶动态回归来自动更新粒子状态
        x = cvRound(A1*(particles.at(k).x - particles.at(k).orix) + A2*(particles.at(k).prex - particles.at(k).orix) +
            B0*rng.gaussian(sigmax) + particles.at(k).orix);
        particles.at(k).x = max(0, min(x, img.cols - 1));

        y = cvRound(A1*(particles.at(k).y - particles.at(k).oriy) + A2*(particles.at(k).prey - particles.at(k).oriy) +
            B0*rng.gaussian(sigmay) + particles.at(k).oriy);
        particles.at(k).y = max(0, min(y, img.rows - 1));

        scale = A1*(particles.at(k).scale - 1) + A2*(particles.at(k).prescale - 1) + B0*(rng.gaussian(sigmas)) + 1.0;
        particles.at(k).scale = max(1.0, min(scale, 3.0));//此处参数设置存疑

        particles.at(k).prex = xpre;
        particles.at(k).prey = ypre;
        particles.at(k).prescale = prescale;

        /*计算更新得到矩形框数据*/
        particles.at(k).rect.x = max(0, min(cvRound(particles.at(k).x - 0.5*particles.at(k).scale*particles.at(k).rect.width), img.cols));
        particles.at(k).rect.y = max(0, min(cvRound(particles.at(k).y - 0.5*particles.at(k).scale*particles.at(k).rect.height), img.rows));
        particles.at(k).rect.width = min(cvRound(particles.at(k).rect.width), img.cols - particles.at(k).rect.x);
        particles.at(k).rect.height = min(cvRound(particles.at(k).rect.height), img.rows - particles.at(k).rect.y);

        //计算粒子权值直方图
        for (int i = particles.at(k).rect.y; i < particles.at(k).rect.y + particles.at(k).rect.height; i++)
        {
            for (int j = particles.at(k).rect.x; j < particles.at(k).rect.x + particles.at(k).rect.width; j++)
            {
                //rgb颜色空间量化为16*16*16 bins
                q_r = img.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
                q_g = img.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
                q_b = img.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
                q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
                hist[q_temp]++;                         // 颜色权重直方图
            }
        }

        // 直方图归一化
        double hist_sum = 0.0;
        double sim_sum = 0.0;
        for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
        {
            hist_sum += pow(hist[i], 2);
        }
        hist_sum = sqrt(hist_sum);
        for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
        {
            hist[i] = hist[i]/hist_sum;
            if(m_hist[i] > 0.0 && hist[i] > 0.0)
            {
                sim_sum += sqrt(m_hist[i]*hist[i]);         // 计算巴氏距离
            }
        }

        particles.at(k).weight =  sim_sum;
        /*粒子权重累加*/
        sum += particles.at(k).weight;
    }

    // 赋值每个粒子权重
    for (int k=0; k<PARTICLE_NUMBER; k++)
    {
        particles.at(k).weight /= sum;
    }

    sort(particles.begin(), particles.end(), comparison);           // 权重排序

    //*********************重采样，根据粒子权重重采样********************//
    int T = 0;                  // 阈值，只要T个粒子
    bool flag = false;          // 是否对每个粒子重新赋值跳出标志位
    newParticle.clear();

    for (int k = 0;k < PARTICLE_NUMBER;k++)
    {
        if(flag)            // 完成粒子赋值，跳出循环
        {
            break;
        }
        T = cvRound(particles.at(k).weight*PARTICLE_NUMBER);    //将权重较弱的粒子淘汰掉，保留权重在阈值以上的
        if(T > 0)
        {
            for (int i = 0;i < T;i++)           // 权重越大，该点赋值数越多
            {
                newParticle.push_back(particles.at(k));
                if (newParticle.size() >= PARTICLE_NUMBER)
                {
                    flag = true;
                    break;
                }
            }
        }
        else            // 没有可以满足条件的权值，跳出循环
        {
            break;
        }
    }

    if(!flag)           // 点未全部赋值，将剩下的用最大值进行赋值
    {
        while (newParticle.size() < PARTICLE_NUMBER)
        {
            newParticle.push_back(particles.at(0));//复制大的权值的样本填满空间
        }
    }

    // 将粒子点替换为更新后的粒子点
    for (int k = 0;k < PARTICLE_NUMBER;k++)
    {
        particles.at(k) = newParticle.at(k);

    }

    //***********计算最大权重目标的期望位置，采用权值最大的1/4个粒子数作为跟踪结果************//
    Rect rectTracking;              //初始化一个Rect作为跟踪的临时
    double weight_temp = 0.0;
    for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
    {
        weight_temp += particles.at(k).weight;
    }
    for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
    {
        particles.at(k).weight /= weight_temp;
    }

    // 更新检测框
    for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
    {
        rectTracking.x += particles.at(k).rect.x*particles.at(k).weight;
        rectTracking.y += particles.at(k).rect.y*particles.at(k).weight;
        rectTracking.width += particles.at(k).rect.width*particles.at(k).weight;
        rectTracking.height += particles.at(k).rect.height*particles.at(k).weight;
    }

    rectangle(img, rectTracking, Scalar(0, 255, 0), 3, 8, 0);//显示跟踪结果，框出
}

int main()
{
    Mat img_mould, frame, mould;
    Rect rect;
    vector<PARTICLE> particles;                 // 粒子参数


    //打开摄像头或者特定视频
    VideoCapture cap;
    cap.open(0);//或cap.open("文件名")
    //读入视频是否为空
    if (!cap.isOpened())
    {
        return -1;
    }
    namedWindow("输出视频", 1);
    setMouseCallback("输出视频", onMouse, 0);//鼠标回调函数，响应鼠标以选择跟踪区域

    m_hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE);         // 目标直方图
    hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE);         // 目标直方图

    while(1)
    {
        cap >> frame;
        if (frame.empty())
        {
            return -1;
        }
        blur(frame, frame, Size(2, 2));//先对原图进行均值滤波处理

        if(tracking && leftButtonUpFlag)
        {
            leftButtonUpFlag = false;
            rect.x = Point_s.x;
            rect.y = Point_s.y;
            rect.width = Point_e.x - Point_s.x;
            rect.height = Point_e.y - Point_s.y;
            img_mould = frame.clone();
            mould = Mat(img_mould, rect);   //目标图像

            init_target(mould, particles, rect);
            p_num = 0;
        }
        if(leftButtonDownFlag)                                      // 绘制截取目标窗口
        {
            rect.x = Point_s.x;
            rect.y = Point_s.y;
            rect.width = processPoint.x - Point_s.x;
            rect.height = processPoint.y - Point_s.y;
            rectangle(frame, rect, Scalar(0, 255, 0), 3, 8, 0);
        }

        if(tracking)
        {
            My_Tracking(frame, particles);
            cout << p_num++ << endl;            // 输出检测帧数
        }

        imshow("输出视频", frame);
        waitKey(1);
    }


    return 0;
}

注：C++代码是直接调动摄像头，效率比较低，识别准确率也有待提高，以后有时间了再优化。

自我感觉自己理解可能有误，请谨慎参考

参考：
http://blog.csdn.net/heyijia0327/article/details/40899819
https://blog.csdn.net/u011624019/article/details/80559397