是自己编写的GM-PHD多目标跟踪软件matlab源码程序 - matlab算法设计

下载频道> 资源分类> matlab源码> 算法设计> 是自己编写的GM-PHD多目标跟踪软件matlab源码程序

标题：是自己编写的GM-PHD多目标跟踪软件matlab源码程序
分享到：

所属分类: 算法设计资源类型：程序源码文件大小: 1.36 KB 上传时间: 2019-06-16 15:50:52 下载次数: 2 资源积分：1分提供者: zhangsan456 code

内容：
是自己编写的GM-PHD多目标跟踪软件matlab源码程序

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;

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;

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;

文件列表（点击上边下载按钮，如果是垃圾文件请在下面评价差评或者投诉）:

code/
code/GM_PHD1.m

关键词： GM-PHD 多目标跟踪

matlab源码下载排行

回到顶部

友情链接

matlab源码网站源码编程语言网络编程手机通讯编程 KUNAG 官网 KUNAG 电商更多

: 联系方式| 版权声明| 招聘信息| 广告服务| 银行汇款| 法律顾问| 兼职技术| 付款方式| 关于我们|; 网站客服程序员兼职招聘 411241577@qq.com; 沪ICP备19040327号-3; 公安备案号：沪公网安备 31011802003874号; 库纳格流体控制系统（上海）有限公司版权所有; Copyright © 1999-2014, GUSUCODE.COM, All Rights Reserved

关键词： GM-PHD 多目标跟踪

相关推荐

matlab源码下载排行

友情链接