GM-PHD滤波算法实现

GM-PHD滤波算法实现

close all;
clc;
m1=[250 250 0 0]‘;
m2=[-250 -250 0 0]‘;
f=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
p=diag（[10 10 5 5]）;
r=[10 0;0 10];
del=5;
q=del*[1/4 0 1/2 0;0 1/4 0 1/2;1/2 0 1 0;0 1/2 0 1];
pd=0.9;
ps=0.9;
w0=0.1;
H=[1 0 0 0;0 1 0 0];
X1=zeros（2100）;
X1（:1）=H*m1;
X2=zeros（2100）;
X2（:1）=H*m2;
for i=2:100
    X1（:i）=X1（:i-1）+[2.5 -12]‘+[randn randn]‘.*[2 5]‘;
    X2（:i）=X2（:i-1）+[12 -2]‘+[randn randn]‘.*[5 2]‘;
end;
figure（1）plot（X1（1:）X1（2:）‘.‘）;
hold onplot（X2（1:）X2（2:）‘r.‘）;
xlabel（‘x coordinate（in m）‘）;
ylabel（‘y coordinate（in m）‘）;
wk=[];
mk=[];
pk=[];
Jk=0;
for iter=1:100
    w_k_k=[];
    m_k_k=[];
    p_k_k=[];
%获取由量测
    i=0;
    i=i+1;
    w_k_k（i）=0.1;
    m_k_k（:i）=[X1（:iter）‘ 2.5 -12]‘;
    p_k_k（::i）=p;
    i=i+1;
    w_k_k（i）=0.1;
    m_k_k（:i）=[X2（:iter）‘ 12 -2]‘;
    p_k_k（::i）=p;
    for j=1:Jk
        i=i+1;
        w_k_k（i）=ps*w_k（j）;
        m_k_k（:i）=f*m_k（:j）;
        p_k_k（::i）=q+f*p_k（::j）*f‘;
    end;
    J_k_k=i;
    n_k_k=[];
    s_k=[];
    k_k=[];
    p_kk=[];
    for j=1:J_k_k
        n_k_k（:j）=H*m_k_k（:j）;
        s_k（::j）=r+H*p_k_k（::j）*H‘;
        k_k（::j）=p_k_k（::j）*H‘*inv（s_k（::j））;
        p_kk（::j）=（diag（[1 1 1 1]）-k_k（::j）*H）*p_k_k（::j）;
    end;
    w_k=[];
    p_k=[];
    m_k=[];
    %update
    for j=1:J_k_k
        w_k（j）=（1-pd）*w_k_k（j）;
        m_k（:j）=m_k_k（:j）;
        p_k（::j）=p_k_k（::j）;
    end;
    l=1;
    for j=1:J_k_k
        w_k（l*J_k_k+j）=pd*w_k_k（j）*exp（-（X1（:iter）-n_k_k（:j））‘*inv（s_k（::j））*（X1（:iter）-n_k_k（:j））/2）/sqrt（2*pi*det（s_k（::j）））;
        m_k（:l*J_k_k+j）=m_k_k（:j）+k_k（::j）*（X1（:iter）-n_k_k（:j））;
        p_k（::l*J_k_k+j）=p_k_k（::j）;
    end;
    w_k（l*J_k_k+1:l*J_k_k+J_k_k）=w_k（l*J_k_k+1:l*j+J_k_k）./（0.0005+sum（w_k（l*J_k_k+1:l*J_k_k+J_k_k）））;
    l=2;
    for j=1:J_k_k
        w_k（l*J_k_k+j）=pd*w_k_k（j）*exp（-（X2（:iter）-n_k_k（:j））‘*inv（s_k（::j））*（X2（:iter）-n_k_k（:j））/2）/sqrt（2*pi*det（s_k（::j）））;
        m_k（:l*J_k_k+j）=m_k_k（:j）+k_k（::j）*（X2（:iter）-n_k_k（:j））;
        p_k（::l*J_k_k+j）=p_k_k（::j）;
    end;
    w_k（l*J_k_k+1:l*J_k_k+J_k_k）=w_k（l*J_k_k+1:l*j+J_k_k）./（0.0005+sum（w_k（l*J_k_k+1:l*J_k_k+J_k_k）））;
    Jk=l*J_k_k+J_k_k;
    Num_Target=round（sum（w_k））;
    %pruning
    L=find（w_k>0.00001）;
    w_k=w_k（L）;
    m_k=m_k（:L）;
    p_k=p_k（::L）;
    Jk=length（w_k）;
    l=0;
	I=Jk;
    m=[];
    w=[];
    pt=[];
	while I>0
        temp=0;
        for i=1:I
            if temp                temp=w_k（i）;
                j=i;
            end;
        end;
        l=l+1;
        lik=[];
        for i=1:I
            lik（i）=（m_k（:i）-m_k（:j））‘*inv（p_k（::i））*（m_k（:i）-m_k（:j））;
        end;
        L=find（lik<4）;
        w（l）=sum（w_k（L））;
        m（:l）=sum（（ones（41））*w_k（L）.*m_k（:L）2）/w（l）;
        pt（::l）=zeros（44）;
        for i=1:length（L）
            pt（::l）=pt

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3754  2010-09-09 09:21  GM_PHD.m

----------- ---------  ---------- -----  ----

                 3754                    1