資源簡(jiǎn)介
GM-PHD濾波算法實(shí)現(xiàn)

代碼片段和文件信息
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=[];
%獲取由量測(cè)
????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
?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件???????3754??2010-09-09?09:21??GM_PHD.m
-----------?---------??----------?-----??----
?????????????????3754????????????????????1
評(píng)論
共有 條評(píng)論