粒子滤波算法理解及实现

文档下载链接：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 : Nu = rand; qtempsum = 0;for j = 1 : Nqtempsum = qtempsum + w(j);if qtempsum >= uw_new(i)=w(j);break;endendend
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 valueV = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:Nx_P(i) = x + sqrt(V) * randn;
endz_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:Tx = 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 : Nx_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标%状态估计，重采样以后，每个粒子的权重都变成了1/Nx_est = mean(x_P);% Save data in arrays for later plottingx_out = [x_out x];z_out = [z_out z];x_est_out = [x_est_out x_est];endt = 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 FilterX = create_particles(Npix_resolution, Npop_particles); % 粒子初始化，在画面中产生均匀分布的随机粒子for k = 1:Nfrm_movie% Getting ImageY_k = read(vr, k);% Forecasting%通过状态模型预测  这里采用的是在上一时刻基础上叠加噪声X = update_particles(F_update, Xstd_pos, Xstd_vec, X); % Calculating Log LikelihoodL = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X(1:2, :), Y_k);% ResamplingX = resample_particles(X, L);% Showing Imageshow_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

function L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X, Y) %#codegenNpix_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 && JC = double(Y(:, m, n));D = C - Xrgb_trgt;D2 = D' * D;   % 欧氏距离L(k) =  A + B * D2;  %高斯似然   D2 越小  L越大  注意B为负数elseL(k) = -Inf;end
end

resample_particles.m

function X = resample_particles(X, L_log)% Calculating Cumulative DistributionL = exp(L_log - max(L_log));
Q = L / sum(L, 2);  % a = sum(L,2)  a中元素为各行向量累加值
R = cumsum(Q, 2);   % 权值累加% Generating Random NumbersN = size(X, 2);
T = rand(1, N); % 随机阈值% Resampling
X_temp = zeros(size(X));
%这里没有用博客里之前说的histc函数，不过目的和效果是一样的
for i = 1 : NX_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 binsq_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;//权重初始为0particles.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 binsq_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

粒子滤波算法理解及实现相关推荐

目标跟踪之粒子滤波---Opencv实现粒子滤波算法
目标跟踪学习笔记_2(particle filter初探1) 目标跟踪学习笔记_3(particle filter初探2) 前面2篇博客已经提到当粒子数增加时会内存报错,后面又仔细查了下程序,是代码方 ...
目标跟踪-粒子滤波算法
http://blog.csdn.net/hujingshuang/article/details/45535423 前言: 粒子滤波广泛的应用于目标跟踪,粒子滤波器是一种序列蒙特卡罗滤波方法,其实质 ...
粒子滤波算法处理非线性噪声，程序简单好用
粒子滤波算法处理非线性噪声,程序简单好用 4115632794284179嘟嘟--囔囔
优化算法笔记|粒子群算法理解及Python实现
粒子群算法的理解及Python实现 1.粒子群算法概述 2 基本PSO算法流程图 3 粒子群算法的Python实现 1.粒子群算法概述粒子群算法来源于对鸟群捕食模型的修正. 假设在一个n维空间中, ...
粒子群算法理解+求解01背包问题
最近在学群体优化算法,做个学习笔记吧,本人蒟蒻,有不对的地方还情多多包涵. 1.粒子群算法的理解. 粒子群算法是一种智能优化算法,模拟的是鸟内捕食行为.假设有一群鸟,在一个区域内觅食,这个区域内只有一 ...
粒子滤波算法（Matlab代码实现）
非线性非高斯模型的改进粒子滤波算法（Matlab代码实现）
改进的粒子滤波算法及其应用研究（Matlab代码实现）
视频跟踪算法之粒子滤波
1. 写在前面最近在看视频跟踪方面的一些硕博士毕业论文,几乎看到的每一篇都会涉及到粒子滤波算法,所以这段时间花了很多时间在看相关的内容. 浏览了大量的博客和文章,跟着不停推导公式,感觉还是无法完全掌 ...
蒙特卡洛粒子滤波定位算法_基于粒子滤波的TBD算法仿真—MATLAB仿真
目标跟踪的最终目的是在最小的误差下确定目标的位置,而在无线传感器网络中要实现这个目的需要很多相关技术的支持,如定位技术.目标检测技术.估计技术.节能技术等.目标跟踪问题的求解有很多方法, 从算法的考虑 ...

粒子滤波算法理解及实现

粒子滤波算法理解及实现相关推荐

最新文章

热门文章