www.gusucode.com > 是自己编写的GM-PHD多目标跟踪软件matlab源码程序 > code/GM_PHD1.m
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(2,100); X1(:,1)=H*m1; X2=zeros(2,100); 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 on,plot(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<w_k(i) 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(4,1))*w_k(L).*m_k(:,L),2)/w(l); pt(:,:,l)=zeros(4,4); for i=1:length(L) pt(:,:,l)=pt(:,:,l)+w_k(L(i))*(p_k(:,:,L(i))+(m(:,l)-m_k(:,L(i)))*(m(:,l)-m_k(:,L(i)))'); end; pt(:,:,l)=pt(:,:,l)/w(l); L=find(lik>=4); w_k=w_k(L); m_k=m_k(:,L); p_k=p_k(:,:,L); I=length(w_k); end; w_k=w; m_k=m; p_k=pt; Jk=length(w_k); %extracting target state target=[]; l=0; w=w_k; for i=1:Num_Target temp=0; for j=1:Jk if temp<w_k(j) temp=w_k(j); l=j; end; end; w(l)=-1; target(:,i)=m_k(:,l); end; if Num_Target>0 for i=1:Num_Target figure(1),plot(target(1,l),target(2,l),'ko'); end; end; end;