版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领
文档简介
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. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。
最新文档
- GB/T 45088-2024林木采伐技术规程
- 2024年物流园区入驻服务合同范本参考3篇
- 酒吧KTV音响系统设备合约
- 医疗卫生研究专项资金管理办法
- 商业综合体装修合同样本
- 机场周边房产买卖附加协议
- 药品处方滥用防控措施
- 2025版绿色环保市场摊位租赁服务协议3篇
- 水利工程招投标流程详解
- 金融区车辆通行办法
- 期末复习提升测试(试题)(含答案)2024-2025学年四年级上册数学人教版
- 生和码头港口设施维护管理制度(3篇)
- 黑龙江省哈尔滨市第六中学2025届高考数学三模试卷含解析
- 伤口治疗师进修汇报
- 研学活动协议书合同范本
- ISBAR辅助工具在交班中应用
- AIGC行业报告:国内外大模型和AI应用梳理
- 换热器的原理及构造
- 校园安全形势会商研判制度(4篇)
- 湖北省十堰市2023-2024学年高二上学期期末调研考试 地理 含答案
- 重庆市2023-2024学年六年级上册语文期末测试试卷(含答案)3
评论
0/150
提交评论