基于粒子濾波的目标跟蹤基本算法（Matlab）

說明：粒子濾波基本算法的代碼，以目标跟蹤的Meanshift算法和粒子濾波的架構為參考，用了兩天時間才寫好，。調試效果很好，感覺粒子濾波比Meanshift的基本方法效果要好，拿出來供大家參考。此程式需要圖像序列，我是用C++連續讀取視訊儲存每一幀圖像得到的。請自己修改圖檔路徑，調節參數R及RR。

%%改進粒子濾波  function []=pf()
clc;
clear;
N=100;%%粒子數
T=150;%%幀數

%%選取目标
Paths='D:\work\KK';%%檔案路徑
I=imread('D:\work\KK\img_274.jpg');%%讀入圖檔
figure(1);%%繪圖
% imshow(I);
[aa,bb,cc]=size(I)

[temp,rect]=imcrop(I); %%目标選取
[a,b,c]=size(temp);

v1=rect(1)
v2=rect(2)
v3=rect(3)
v4=rect(4)

tic_x=v1+v3/2;
tic_y=v2+v4/2;

X0=v1 %%初始化
Y0=v2

dX=[0;0];
dL=[0;0];

%%粒子初始化
xpart=zeros(2,N);
xhatPart=zeros(2,N);
R=3;%%R=5;
RR=10;%%RR=30;
for i=1:N  %%長度為200
    xpart(:,i) = [X0;Y0] + RR* randn(2,1);  %%粒子正态随機分布
end
Hist0=FHist(temp,8);

%% 粒子濾波
for i=1:T  %%長度為200
    if i==1
        Hist1=Hist0;%特征更新
    else
        Hist1=Hist2;
    end
    Im=imread(['D:\work\KK\img_',int2str(274+i),'.jpg']);%%讀入一幀圖像
    
    for j=1:N
        if i~=1
            xpart(:,j)=xpart(:,j)+dX+R*randn(2,1);%%狀态預測 X（k|k-1）
        end
        rect=[xpart(1,j),xpart(2,j),v3,v4];
        temp=imcrop(Im,rect);
        Hist2= FHist(temp,8);% 對預測值觀察，
        bc(j)=Bhatt(Hist0,Hist2);
        db=1-bc(j);
        q(j) = exp(-20*db); %粒子權值配置設定
    end
    aaaa=bc;
    q=q/sum(q);
    xxxx=xpart.*[q;q];
    yyyy=sum(xxxx,2);
    
    %%重采樣
    for ii = 1 : N
        u = rand; % uniform random number between 0 and 1
        qs = 0;
        for jj = 1 : N
            qs = qs + q(jj);
            if qs >= u
                xpart(:,ii) = xpart(:,jj);
                break;
            end
        end
    end
    
    %%取均值
    bbbb=mean(xpart,2);
    xhatPart(:,i) = yyyy;
    rect=[xhatPart(1,i),xpart(2,j),v3,v4];
    temp=imcrop(Im,rect);
    Hist2= FHist(temp,8); % 對預測值觀察，
    cccc=Bhatt(Hist1,Hist2);
    Xcb(i)=cccc;
    
    if i==1
        dX=xhatPart(:,i)-[X0;Y0]
    else
        dX=xhatPart(:,i)-xhatPart(:,i-1)
    end
    
    %%更新
    rect=[xhatPart(1,i),xhatPart(2,i),v3,v4];
    temp=imcrop(Im,rect);
    
    v1=rect(1);
    v2=rect(2);
    v3=rect(3);
    v4=rect(4);
    
    tic_x=[tic_x;v1+v3/2];
    tic_y=[tic_y;v2+v4/2];

    if isnan(dX(1))
        break;
    end
    
    if ( v1<0 || v2<0 || v1+v3>bb || v2+v4>aa )
        break;
    end
    
    %%顯示跟蹤結果
    figure(1)
    clf
    imshow(uint8(Im))
    hold on;
    plot([v1,v1+v3],[v2,v2],[v1,v1],[v2,v2+v4],[v1,v1+v3],...
        [v2+v4,v2+v4],[v1+v3,v1+v3],[v2,v2+v4],'LineWidth',1,'Color','r')
    plot(tic_x,tic_y,'LineWidth',2,'Color','b');
end

           

function [hist]=FHist(temp,D) 


% 此函數為提取區域目标色彩直方圖
% temp-目标區域
% D-顔色空間位數
% m_wei(i,j)-像素點權值貢獻
% hist-色彩直方圖分布


E=256/D;
hist=zeros(1,D^3);
[a,b,c]=size(temp);


y(1)=a/2;
y(2)=b/2;
m_wei=zeros(a,b);%權值矩陣
h=y(1)^2+y(2)^2 ;%帶寬


for i=1:a
    for j=1:b
        dist=(i-y(1))^2+(j-y(2))^2;
        m_wei(i,j)=1-dist/h; %像素點權值配置設定
    end
end
C=sum(sum(m_wei));%歸一化系數
for i=1:a
    for j=1:b
        %rgb顔色空間量化
        q_r=fix(double(temp(i,j,1))/E);  %fix為趨近0取整函數
        q_g=fix(double(temp(i,j,2))/E);
        q_b=fix(double(temp(i,j,3))/E);
        q_temp=q_r*D^2+q_g*D+q_b;  %設定顔色分量
        hist(q_temp+1)= hist(q_temp+1)+m_wei(i,j);  %像素點權重累積
    end
end
hist=hist/C;
           

function C=Bhatt( R , Q)
C=sum(sqrt(R.*Q));