JPDA的matlab程序(最新整理)_第1页
JPDA的matlab程序(最新整理)_第2页
JPDA的matlab程序(最新整理)_第3页
已阅读5页,还剩7页未读 继续免费阅读

下载本文档

版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领

文档简介

1、functionjpdaf(target_position,n,t,mc_number,c)% 程序功能:采用 jpda 数据关联算法实现两个个匀速运动目标的点迹与航迹的关联% 输入变量:%-target_position:目标的初始位置%- n:采样次数%- t:采样间隔%-mc_number:仿真次数%- c:目标个数% 输出变量:%无% 参考文献:%黄玲,数据挖掘及融合技术研究与应用,西北工业大学硕士学位论文,2004 年% 声明:%该代码为作者毕业设计内容,鉴于学术交流的角度,现在公开发布该代码%该代码非本人原创,修改自网上另一位作者的 jpda 代码%该代码仅用于学术交流,请勿用于任

2、何其它商业用途,请大家自觉遵守%如果有人用该代码进行不合适的用途,该代码作者不承担任何责任%请遵守作者的劳动成果,转载请标明% 作者邮箱:%%参数定义% pd=1;%检测概率pg=0.99;%正确量测落入跟踪门内得概率g_sigma=9.21;%门限lambda=2;gamma=lambda*10(-6);%每一个单位面积(km2)内产生 lambda 个杂波target_measurement=zeros(c,2,n);%目标观测互联存储矩阵target_delta=100100;%目标对应的观测标准差p=zeros(4,4,c);%协方差矩阵p1=tar

3、get_delta(1)2 0 0 0;0 0.01 0 0;0 0 target_delta(1)2 0;0 0 0 0.01;%初始协方差矩阵p(:,:,1)=p1;p(:,:,2)=p1;a = 1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1;%状态转移矩阵c=1000;0010;%观测矩阵r=target_delta(1)2 0;0 target_delta(1)2;%观测协方差矩阵q=4%系统过程噪声协方差g=t2/20;t%过程噪声矩阵x_filter=zeros(4,c,n);0;00;0t2/2;04;t;%存储目标的各时刻的滤波值x_filter1=zeros

4、(4,c,n,mc_number);%mc_number 次 montle carlo 仿真所得全部结果存储矩阵data_measurement=zeros(c,2,n);%观测存储矩阵data_measurement1=zeros(c,4,n);%实际位置坐标 x,y 矩阵%产生目标的实际位置%data_measurement1(:,:,1)=target_position;%实际位置矩阵初始化for i=1:cforii=2:n%实际位置data_measurement1(i,:,ii)=(a*data_measurement1(i,:,ii-1)+(g*sqrt(q)*(randn(2,

5、1);endenda=zeros(1,n);b=zeros(1,n); for i=1:na(i)=data_measurement1(1,1,i); b(i)=data_measurement1(1,3,i);end plot(a,b,b-); hold on; a=zeros(1,n);b=zeros(1,n); for i=1:na(i)=data_measurement1(2,1,i); b(i)=data_measurement1(2,3,i);end plot(a,b,r-);xlabel(x(m),ylabel(y(m);legend(目标 a 的实际位置,目标 b 的实际位置,

6、1);grid;%程序主体%for m=1:mc_number%1.产生路径%noise=; for i=1:nforj=1:c%各传感器观测的位置data_measurement(j,1,i)=data_measurement1(j,1,i)+rand(1)*target_delta(j); data_measurement(j,2,i)=data_measurement1(j,3,i)+rand(1)*target_delta(j);endend%2.产生杂波,并确定有效观测%s=zeros(2,2,c);z_predic=zeros(2,2);%存储两个目标的观测预测值,即只包括 x,y

7、 坐标x_predic=zeros(4,2);%存储两个目标的状态预测值,即包括 x,y 坐标和 x,y 方向速度ellipse_volume=zeros(1,2);noise_sum_a=;%存储目标 1 的杂波noise_sum_b=;%存储目标 2 的杂波for t=1:ny1=;y=;noise=;noise=;for i=1:c if t=1x_predic(:,i) = a*x_filter(:,i,t-1);%用前一时刻的滤波值来预测当前的值(kalman 滤波的第一个表达式) elsex_predic(:,i)=target_position(i,:);%第一次采样我们用真实位

8、置当预测值end p_predic=a*p(:,:,i)*a+g*q*g;%更新 x_predic 协方差矩阵(kalman 滤波的第二个表达式)z_predic(:,i)=c*x_predic(:,i);%提取预测值的 x,y 坐标,舍弃 x,y 速度r=target_delta(i)2 0; 0 target_delta(i)2; s(:,:,i)=c*p_predic*c+r;% 定 义 中 间 变 量 s ellipse_volume(i)=pi*g_sigma*sqrt(det(s(:,:,i);%计算椭圆跟踪门的面积number_returns=floor(ellipse_volu

9、me(i)*gamma+1);%椭圆跟踪门内的错误回波数side=sqrt(ellipse_volume(i)*gamma+1)/gamma)/2;%将椭圆跟踪门等效为正方形,并求出正方形边长的二分之一noise_x=x_predic(1,i)+side-2*rand(1,number_returns)*side;%在预测值周围产生多余回波。注意:当某一次 number_returns 小于等于 0 时会出错, 再运行一次即可。noise_y=x_predic(3,i)+side-2*rand(1,number_returns)*side; noise=noise_x;noise_y;nois

10、e=noise noise;if i=1noise_sum_a=noise_sum_a noise;elseend endnoise_sum_b=noise_sum_b noise;b=zeros(1,2); b(1)=data_measurement(1,1,t); b(2)=data_measurement(1,2,t);y1=noiseb;%将接收到的所有的回波存在 y1 中,包括杂波和观测b(1)=data_measurement(2,1,t); b(2)=data_measurement(2,2,t);y1=y1b;%当有一个杂波回波时 3.产生观测确认矩阵 q2% m1=0;%记录

11、有效观测个数n1,n2=size(y1); q1=zeros(100,3); for j=1:n2flag=0; for i=1:cd=y1(:,j)-z_predic(:,i);d=d*inv(s(:,:,i)*d; if d=g_sigmaflag=1;q1(m1+1,1)=1;q1(m1+1,i+1)=1;endendif flag=1y=yy1(:,j);%把落入跟踪门中的所有回波放入 y 中m1=m1+1;%记录有效观测个数endend q2=q1(1:m1,1:3);%4.产生互联矩阵 a_matrix,其中 num 表示可行联合事件个数%a_matrix=zeros(m1,3,1

12、0000); a_matrix(:,1,1:10000)=1;ifm1=0%m1=0 表示两个目标都没有观测num=1;for i=1:m1if q2(i,2)=1a_matrix(i,2,num)=1;a_matrix(i,1,num)=0; num=num+1;for j=1:m1if (i=j)&(q2(j,3)=1) a_matrix(i,2,num)=1;a_matrix(i,1,num)=0; a_matrix(j,3,num)=1;a_matrix(j,1,num)=0; num=num+1;endendendendfor i=1:m1if q2(i,3)=1a_matrix(i

13、,3,num)=1;a_matrix(i,1,num)=0; num=num+1;end else endendflag=1;a_matrix=a_matrix(:,:,1:num);%穷举法拆分的结果存在 a_matrix 中%5.计算后验概率 pr,其中 false_num 表示假量测,mea_indicator 表示观测指示器,target_indicator 表示目标指示器%pr=zeros(1,num); for i=1:numfalse_num=m1; n=1;for j=1:m1mea_indicator=sum(a_matrix(j,2:3,i);%参考文献中式 4-48if

14、mea_indicator=1 false_num=false_num-1;ifa_matrix(j,2,i)=1%如果观测与目标 1 关联b=(y(:,j)-z_predic(:,1)*inv(s(:,:,1)*(y(:,j)-z_predic(:,1); n=n/sqrt(det(2*pi*s(:,:,1)*exp(-1/2*b);%计算正态分布函数else%如果观测与目标 2 关联b=(y(:,j)-z_predic(:,2)*inv(s(:,:,2)*(y(:,j)-z_predic(:,2); n=n/sqrt(det(2*pi*s(:,:,2)*exp(-1/2*b);%计算正态分

15、布函数endendendif pd=1a=1;elsea=1;for j=1:ctarget_indicator=sum(a_matrix(:,j+1,i);%参考文献中式 4-49a=a*pdtarget_indicator*(1-pd)(1-target_indicator);%计算检测概率endend v=ellipse_volume(1)+ellipse_volume(2);%表示整个空域的体积a1=1;for j=1:false_num a1=a1*j;end pr(i)=n*a*a1/(vfalse_num);end pr=pr/sum(pr);% 6.计算关联概率 u%u=zer

16、os(m1+1,c); for i=1:cfor j=1:m1for k=1:numu(j,i)=u(j,i)+pr(k)*a_matrix(j,i+1,k);endendendu(m1+1,:)=1-sum(u(1:m1,1:c);%无量测与目标 t 互联的关联概率存入 u(m1+1,:),规一化%7.kalman 滤波开始%fori=1:c%更新协方差矩阵p_predic = a*p(:,:,i)*a+g*q*g; k (:,:,i)= p_predic*c*inv(s(:,:,i);p(:,:,i)= p_predic-(1-u(m1+1,i)*k(:,:,i)*s(:,:,i)*k(:

17、,:,i);endfor i=1:ca=0; b=0;x_filter2=0;%随便设置的中间参数for j=1:m1x_filter2=x_filter2+u(j,i)*(x_predic(:,i)+ k (:,:,i)*(y(:,j)- z_predic(:,i);end x_filter2=u(j+1,i)*x_predic(:,i)+x_filter2; x_filter(:,i,t)=x_filter2;for j=1:m1+1if j=m1+1a=x_predic(:,i);elsea=x_predic(:,i)+ k (:,:,i)*(y(:,j)- z_predic(:,i);

18、endendb=b+u(j,i)*(a*a-x_filter2*x_filter2);endp(:,:,i)=p(:,:,i)+b;x_filter1(:,i,t,m)=x_filter(:,i,t);end end%画图%x_filter=sum(x_filter1,4)/mc_number;%滤波值作平均%1.滤波结果%figure;%目标 a,b 的观测位置for i=1:ca=zeros(1,n);b=zeros(1,n); for j=1:na(j)=data_measurement(i,1,j); b(j)=data_measurement(i,2,j);endif i=1plot

19、(a(:),b(:),b:) elseplot(a(:),b(:),b:)endend hold on;%目标 a,b 的杂波位置for i=1:cif i=1plot(noise_sum_a(1,:),noise_sum_a(2,:),c.); elseplot(noise_sum_b(1,:),noise_sum_b(2,:),c.);endendhold on;%目标 a,b 的估计位置for i=1:ca=zeros(1,n);b=zeros(1,n); for j=1:na(j)=x_filter(1,i,j);b(j)=x_filter(3,i,j);end if i=1plot(

20、a(:),b(:),r-);elseplot(a(:),b(:),r-);endhold on; endxlabel(x/m),ylabel(y/m);legend(目标 a 的观测位置,目标 b 的观测位置,目标 a 的杂波,目标 b 的杂波,目标 a 的估计位置,目标 b 的估计位置,4);grid;% 2.速度误差%figure; a=0;c1=zeros(c,n); for j=1:nfori=1:mc_number%最小均方误差a=(x_filter1(1,1,j,i)-data_measurement1(1,1,j)2+(x_filter1(3,1,j,i)- data_measu

21、rement1(1,3,j)2;c1(1,j)=c1(1,j)+a;endendc1(1,j)=sqrt(c1(1,j)/mc_number);temp=c1(1,:); a_extra=zeros(2,n); b_extra=zeros(1,n); c_extra=zeros(1,n); a_extra(1,:)=temp; a_extra(2,:)=1:1:n; b_extra=a_extra(1,:); c_extra,pos=sort(b_extra);%pos 为排序后的下标,c 为第一行的排序结果; a_extra(2,:)=a_extra(2,pos);%第二行按照第一行排序的下

22、标对应a_extra(1,:)=c_extra;%第一行结果重新赋给 a 的第一行; str1=num2str(a_extra(2,n); str2=num2str(a_extra(1,n); str=strcat(itn=,str1,iterror=,str2,(m); text(a_extra(2,n),0.8*a_extra(1,n),str); hold on;plot(a_extra(2,n) a_extra(2,n),0 a_extra(1,n),r); hold on;plot(1:n,c1(1,:),r:) hold on;a=0;for j=1:nfori=1:mc_numb

23、er%最小均方误差a=(x_filter1(1,2,j,i)-data_measurement1(2,1,j)2+(x_filter1(3,2,j,i)- data_measurement1(2,3,j)2;c1(2,j)=c1(2,j)+a;endendc1(2,j)=sqrt(c1(2,j)/mc_number);temp=c1(2,:); a_extra=zeros(2,n); b_extra=zeros(1,n); c_extra=zeros(1,n); a_extra(1,:)=temp; a_extra(2,:)=1:1:n; b_extra=a_extra(1,:); c_ext

24、ra,pos=sort(b_extra);%pos 为排序后的下标,c 为第一行的排序结果; a_extra(2,:)=a_extra(2,pos);%第二行按照第一行排序的下标对应a_extra(1,:)=c_extra;%第一行结果重新赋给 a 的第一行; str1=num2str(a_extra(2,n); str2=num2str(a_extra(1,n); str=strcat(itn=,str1,iterror=,str2,(m); text(a_extra(2,n),0.8*a_extra(1,n),str); hold on;plot(a_extra(2,n) a_extra(

25、2,n),0 a_extra(1,n),b); hold on;plot(1:n,c1(2,:),b:)xlabel(times),ylabel(测量值与估计值均方差/m);legend(目标 a 的误差最大值,目标 a 的误差,目标 b 的误差最大值,目标 b 的误差);grid;%revised on 26th june 2008 by wangzexun%figure; extra11=zeros(1,n); extra12=zeros(1,n); extra13=zeros(1,n); for j=1:nextra11(1,j)=sqrt(x_filter(1,1,j)-data_me

26、asurement1(1,1,j)2+(x_filter(3,1,j)- data_measurement1(1,3,j)2;extra12(1,j)=sqrt(data_measurement(1,1,j)- data_measurement1(1,1,j)2+(data_measurement(1,2,j)-data_measurement1(1,3,j)2);extra13(1,j)=extra12(1,j)/extra11(1,j);end plot(1:n,extra13(1,:),k:);xlabel(times),ylabel(rmse of a); grid;figure; extra21=zeros(1,n); extra22=zeros(1,n); extra23=zeros(1,n); for j=1:nextra21(1,j)=sqrt(x_filter(1,2,j)-data_measurement1(2,1,j)2+(x_filter(3,2,j)- data_measurement1(2,3,j)2;extra22(1,j)=sqrt(data_measurement(2,1,j)- da

温馨提示

  • 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

评论

0/150

提交评论