MIMO tbd 记录

%% 反射幅度测量函数% 生成零均值方差为固定值的随机数clc;clear all;tic;num_transmit=2;num_receive=3;transmit=[0,0;0,1500]; %两个发射基站位置坐标receive=[1500,0;1500,750;1500,1500]; %三个接受基站位置坐标%% 状态参数T=1; % 帧间间隔Total_time=30; % 总帧数start1=7; % 目标1出现时间start2=9; % 目标2出现的时间disappear=23; % 目标消失时间F=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1];q1=1.5;Q=[q1*T^3/3 q1*T^2/2 0 0 q1*T^2/2 q1*T 0 0 0 0 q1*T^3/3 q1*T^2/2 0 0 q1*T^2/2 q1*T ];delta_m=2; % 量测噪声标准差%% 模型参数monte=1; % 蒙特卡洛次数likeli_bound=1; % 似然区域SNR=5;A=sqrt(10.^(SNR/10)*2*delta_m.^2); % 信号复幅度模值，2-35中的~Ak，利用2-42求Lr=0.01;c=3*10^8;Rc=5; %% 目标真实运动状态x1_true=zeros(4,Total_time); %目标1真实状态x2_true=zeros(4,Total_time); %目标2真实状态initx_1 =[100;10;200;11];initx_2 =[100;10;200;-11];x1_true(:,start1)=initx; %在目标出现的时刻“start”初始化目标状态x2_true(:,start2)=initx; %在目标出现的时刻“start”初始化目标状态for t=start+1:disappear-1; x1_true(:,t)=F*x_true(:,t-1)+Q*[randn;0;randn;0]; %单一静止目标 x1_true(:,t)=F*x_true(:,t-1)+Q*[randn;0;randn;0];end% figure(1); %观察真实轨迹% plot(x_true(1,7:22),x_true(3,7:22),'-ro');%% 状态空间min_x=0; max_x=600; %二维平面上的观测空间范围min_y=0; max_y=800;v_delta=6;min_xv=initx(2,1)-v_delta; max_xv=initx(2,1)+v_delta;min_yv=initx(4,1)-v_delta; max_yv=initx(4,1)+v_delta;%% 观测区域 min_distance=1000;size_distance_cell=15; % 距离单元的尺寸num_distance_cell=200; % 距离单元的个数dx=1;dy=1;fc=100000; %10MHZ,发射频率对目标表面的复幅度测量有影响mu=0;sigma=sqrt(1/(dx*dy)); %方差转标准差R=zeros(num_transmit,num_receive); %初始化reflect(:,:,:)=zeros(num_transmit,num_receive,Total_time);distance(:,:,:)=zeros(num_transmit,num_receive,Total_time);%% 粒子参数N=4000;N_birth_Particle=N; %新生粒子数N_continue_Particle=N; %继续粒子数N_particle=N; %固定粒子数Pd=0.05; %目标死亡概率Pb=0.05; %目标新生概率%% 计算不同的随机RCS的反射率，乘以幅值A就是反射的幅度大小，再乘以一个类似扩散的衰减函数，就是TBD的概念？？要不要乘以呢？for t= 1:Total_time %%t=1; %第一帧观测 for i= 1:num_transmit for j= 1:num_receive R(i,j)= normrnd(mu,sigma); %反射率，零均值，方差为sigma fun_c=@(dx1,dy1)exp(-sqrt(-1)*2*pi*fc*( sqrt( ( transmit(i,1)-(x_true(1,t)+dx1) ).^2+ ( transmit(i,2)-(x_true(3,t)+dy1) ).^2 ) /c... - sqrt( ( transmit(i,1)-x_true(1,t) ).^2+ ( transmit(i,2)-x_true(3,t) ).^2 )./c... + sqrt( ( receive(j,1)-(x_true(1,t)+dx1) ).^2+ ( receive(j,2)-(x_true(3,t)+dy1) ).^2 ) /c... - sqrt( ( receive(j,1)-x_true(1,t) ).^2+ ( receive(j,2)-x_true(3,t) ).^2 )./c)* R(i,j));%积分变量是dx1，dy1 reflect(i,j,t) =quad2d(fun_c,-dx/2,dx/2,-dy/2,dy/2); end endend%% 获得观测数据（只有目标2*3*30，每一帧只有一个数据） z_distance(:,:,:)=zeros(num_transmit,num_receive,Total_time);for t=start:disappear-1 for i= 1:num_transmit for j= 1:num_receive z_distance(i,j,t) = sqrt( ( transmit(i,1)-x_true(1,t) ).^2+ ( transmit(i,2)-x_true(3,t) ).^2 )... +sqrt( ( receive(j,1)-x_true(1,t) ).^2+ ( receive(j,2)-x_true(3,t) ).^2 )... +delta_m*randn(1,1); % 观测量是距离和，加上测量噪声 end endend %% 滤波过程xp=zeros(4,N_particle,Total_time); %重采样后的粒子x_estimate_final=zeros(4,Total_time); %状态估计location_error=zeros(1,Total_time); %位置误差RMS=zeros(1,Total_time); %均方根误差% Pe=zeros(1,Total_time); %目标检测概率% Mb=zeros(1,Total_time); %新生粒子未归一化权值和% Mc=zeros(1,Total_time); %继续粒子未归一化权值和x_par_birth=zeros(4,N_birth_Particle,Total_time); %新生粒子x_par_continue=zeros(4,N_continue_Particle,Total_time); %继续粒子final_q_mu_birth=zeros(N_birth_Particle,Total_time); %似然比、新生粒子权重final_q_mu_continue=zeros(N_continue_Particle,Total_time); %似然比、继续粒子权重t_data=zeros(num_transmit,num_receive,num_distance_cell); %每帧数据,每一个收发对h=zeros(num_transmit,num_receive,num_distance_cell); h_birth=zeros(num_transmit,num_receive,num_distance_cell); % h函数 h_continue=zeros(num_transmit,num_receive,num_distance_cell); z_par_birth_distance(:,:)=zeros(num_transmit,num_receive); % 距离、量化坐标z_par_continue_distance(:,:)=zeros(num_transmit,num_receive);z_par_birth_coord(:,:)=zeros(num_transmit,num_receive);z_par_continue_coord(:,:)=zeros(num_transmit,num_receive);% 第一帧，只有新生粒子，没有继续粒子 for m=1:monte m t=8; % ============ 第一帧观测空间（每一帧是一个观测平面）============ t_data(:,:,:)=delta_m*randn(num_transmit,num_receive,num_distance_cell)... +delta_m*randn(num_transmit,num_receive,num_distance_cell).*sqrt(-1) ;%产生幅度服从瑞利分布的高斯白噪声 %比较简单，就是产生实部和虚部相互独立的高斯序列。 if (t>=start)&&(t<=disappear-1) for i= 1:num_transmit for j= 1:num_receive for k=1:num_distance_cell h(i,j,k)=reflect(i,j,t)*exp(-(z_distance(i,j,t) -(k*size_distance_cell+min_distance-size_distance_cell/2)).^2*Lr/(2*Rc)); %k融入到h中，借助于第几个距离单元 end end end t_data(:,:,:)=(abs(t_data(:,:,:)+(A*exp(sqrt(-1)*2*pi*rand)*h(:,:,:)))).^2; %2-38 将反射写在h(i,j,k)里面 else t_data(:,:,:)=(abs(t_data(:,:,:))).^2 ; end %============新生粒子============= for n=1:N_birth_Particle x_par_birth(1,n,t)=(max_x-min_x)*rand+min_x; %新生粒子均匀分布 x_par_birth(2,n,t)=(max_xv-min_xv)*rand+min_xv; x_par_birth(3,n,t)=(max_y-min_y)*rand+min_y; x_par_birth(4,n,t)=(max_yv-min_yv)*rand+min_yv; %============计算出每个新生粒子的距离============ for i= 1:num_transmit for j= 1:num_receive z_par_birth_distance(i,j) = sqrt( ( transmit(i,1)-x_par_birth(1,n,t) ).^2+ ( transmit(i,2)-x_par_birth(3,n,t) ).^2 )... +sqrt( ( receive(j,1)-x_par_birth(1,n,t) ).^2+ ( receive(j,2)-x_par_birth(3,n,t) ).^2 ); %n在外部循环，t也在因此都不用在z_par_distance中 end end %============求出每个新生粒子落在哪个距离单元格,仿写程序============ for i= 1:num_transmit for j= 1:num_receive z_par_birth_coord(i,j)=ceil(( z_par_birth_distance(i,j) -min_distance)./size_distance_cell); if z_par_birth_coord(i,j)<1 z_par_birth_coord="" i="" j="" 1="" end="" if="" z_par_birth_coord="" i="" j="">num_distance_cell z_par_birth_coord(i,j)=num_distance_cell; end end end final_q_mu_birth(i,j,n,t)=1; % 没有目标时候 for i= 1:num_transmit for j= 1:num_receive for k=(z_par_birth_coord-likeli_bound):(z_par_birth_coord+likeli_bound) if((k>=1)&&(k<=num_distance_cell)) h_birth=A.^2*exp(-(z_par_birth_distance(i,j)-(k*size_distance_cell+min_distance-size_distance_cell/2)).^2*Lr./(Rc) ) ;% final_q_mu_birth(n,t)=final_q_mu_birth(n,t)*... (2*delta_m^2/h_birth).*exp(t_data(i,j,k)... /(2*delta_m.^2)-t_data(i,j,k)./h_birth); %似然比（有目标的时候） end end end end final_q_mu_birth(n,t)=(1/N_birth_Particle).*final_q_mu_birth(n,t); %公式2-49 end % Mb(t)=Pb*sum(final_q_mu_birth(:,t)); %新生粒子未归一化权值加权和，公式2-52 % Mc(t)=0; % Pe(t)=(Mb(t)+Mc(t))/( Mb(t)+Mc(t)+(1-Pb)/(Mb(t)*sum(final_q_mu_birth(:,t)*Pb)) ); % final_q_mu_birth(:,t)=final_q_mu_birth(:,t)/sum(final_q_mu_birth(:,t)); %============对第一帧新生的粒子做重采样============ q0(1:N_birth_Particle)=final_q_mu_birth(:,t); q0 = q0/sum(q0); P = cumsum(q0); ut(1)=rand(1)/N_particle; k = 1; ii = zeros(1,N_particle); for j = 1:N_particle ut(j)=ut(1)+(j-1)/N_particle; while(P(k)<ut(j)); k = k + 1; end; ii(j) = k; end xp(:,:,t) = x_par_birth(:,ii,t); %重采样过后的新粒子 xx = xp(:,:,t)'; x_estimate_final(:,t) = mean(xx); %计算状态估计 %===========第2帧以后的观测============ for t=2:Total_time %===========第t帧的观测============ t_data(:,:,:)=delta_m*randn(num_transmit,num_receive,num_distance_cell)... +delta_m*randn(num_transmit,num_receive,num_distance_cell).*sqrt(-1) ; if (t>=start)&&(t<=disappear-1) for i= 1:num_transmit for j= 1:num_receive for k=1:num_distance_cell h(i,j,k)=reflect(i,j,t)*exp(-(z_distance(i,j,t) -(k*size_distance_cell+min_distance-size_distance_cell/2)).^2*Lr/(2*Rc)); %k融入到h中，借助于第几个距离单元 end end end t_data(:,:,:)=(abs(t_data(:,:,:)+(A*exp(sqrt(-1)*2*pi*rand)*h(:,:,:)))).^2; %2-38 将反射写在h(i,j,k)里面 else t_data(:,:,:)=(abs(t_data(:,:,:))).^2 ; end %%===========新生粒子============ for n=1:N_birth_Particle x_par_birth(1,n,t)=(max_x-min_x)*rand+min_x; %新生粒子均匀分布 x_par_birth(2,n,t)=(max_xv-min_xv)*rand+min_xv; x_par_birth(3,n,t)=(max_y-min_y)*rand+min_y; x_par_birth(4,n,t)=(max_yv-min_yv)*rand+min_yv; %=======计算出每个新生粒子的距离======= for i= 1:num_transmit for j= 1:num_receive z_par_birth_distance(i,j) = sqrt( ( transmit(i,1)-x_par_birth(1,n,t) ).^2+ ( transmit(i,2)-x_par_birth(3,n,t) ).^2 )... +sqrt( ( receive(j,1)-x_par_birth(1,n,t) ).^2+ ( receive(j,2)-x_par_birth(3,n,t) ).^2 ); %n在外部循环，t也在因此都不用在z_par_distance中 end end %====求出每个新生粒子落在哪个距离单元格,仿写程序===== for i= 1:num_transmit for j= 1:num_receive z_par_birth_coord(i,j)=ceil(( z_par_birth_distance(i,j) -min_distance)./size_distance_cell); if z_par_birth_coord(i,j)<1 z_par_birth_coord="" i="" j="" 1="" end="" if="" z_par_birth_coord="" i="" j="">num_distance_cell z_par_birth_coord(i,j)=num_distance_cell; end end end final_q_mu_birth(i,j,n,t)=1; % 没有目标时候 for i= 1:num_transmit for j= 1:num_receive for k=(z_par_birth_coord-likeli_bound):(z_par_birth_coord+likeli_bound) if((k>=1)&&(k<=num_distance_cell)) h_birth=A.^2*exp(-(z_par_birth_distance(i,j)-(k*size_distance_cell+min_distance-size_distance_cell/2)).^2*Lr./(Rc) ) ;% final_q_mu_birth(n,t)=final_q_mu_birth(n,t)*... (2*delta_m^2/h_birth).*exp(t_data(i,j,k)... /(2*delta_m.^2)-t_data(i,j,k)./h_birth); %似然比（有目标的时候） end end end end final_q_mu_birth(n,t)=(1/N_birth_Particle).*final_q_mu_birth(n,t); %公式2-49 end %%==============继续粒子============= for n=1:N_continue_Particle x_par_continue(:,n,t)=F*xp(:,n,t-1)+Q*randn(4,1);%delta_s*randn(2,1); %继续粒子通过状态转移函数生成 %x_par_continue(:,n,t)=F*xp(:,n,t-1)+Q*[randn;0;randn;0]; %单一静止目标 %=====计算出每个继续粒子的距离======= for i= 1:num_transmit for j= 1:num_receive z_par_continue_distance(i,j) = sqrt( ( transmit(i,1)-x_par_continue(1,n,t) ).^2+ ( transmit(i,2)-x_par_continue(3,n,t) ).^2 )... +sqrt( ( receive(j,1)-x_par_continue(1,n,t) ).^2+ ( receive(j,2)-x_par_continue(3,n,t) ).^2 ); %n在外部循环，t也在因此都不用在z_par_distance中 end end %求出每个继续粒子落在哪个距离单元格 for i= 1:num_transmit for j= 1:num_receive z_par_continue_coord(i,j)=ceil(( z_par_continue_distance(i,j) -min_distance)./size_distance_cell); if z_par_continue_coord(i,j)<1 z_par_continue_coord="" i="" j="" 1="" end="" if="" z_par_continue_coord="" i="" j="">num_distance_cell z_par_continue_coord(i,j)=num_distance_cell; end end end final_q_mu_continue(i,j,n,t)=1; % 没有目标时候 for i= 1:num_transmit for j= 1:num_receive for k=(z_par_continue_coord-likeli_bound):(z_par_continue_coord+likeli_bound) if((k>=1)&&(k<=num_distance_cell)) h_continue=A.^2*exp(-(z_par_continue_distance(i,j)-(k*size_distance_cell+min_distance-size_distance_cell/2)).^2*Lr./(Rc) ) ;% final_q_mu_continue(n,t)=final_q_mu_continue(n,t)*... (2*delta_m^2/h_continue).*exp(t_data(i,j,k)... /(2*delta_m.^2)-t_data(i,j,k)./h_continue); %似然比（有目标的时候） end end end end final_q_mu_continue(n,t)=(1/N_continue_Particle).*final_q_mu_continue(n,t); %公式2-49 end %% Mb(t)=Pb*(1-Pe(t-1))*sum(final_q_mu_birth(:,t)); Mc(t)=(1-Pd)*Pe(t-1)*sum(final_q_mu_continue(:,t));% Pe(t)=(Mb(t)+Mc(t))/(Mb(t)+Mc(t)+(Pd)*Pe(t-1)+(1-Pb)*(1-Pe(t-1)));% final_q_mu_birth(:,t)=(Pb*(1-Pe(t-1))/(Mb(t)+Mc(t))).*final_q_mu_birth(:,t);% final_q_mu_continue(:,t)=((1-Pd)*Pe(t-1)/(Mb(t)+Mc(t))).*final_q_mu_continue(:,t); Pe(t)=( Mb(t)+Mc(t) )/( Mb(t)+Mc(t) + ( 1 - ((1-Pd)*Pe(t-1) + Pb*(1-Pe(t-1))) ) / ( (Mc(t)*sum( final_q_mu_continue(:,t) )+Mb(t)*sum( final_q_mu_birth(:,t) ))*((1-Pd)*Pe(t-1) + Pb*(1-Pe(t-1))) ) ); final_q_mu_birth(:,t)=final_q_mu_birth(:,t)/sum(final_q_mu_birth(:,t)); final_q_mu_continue(:,t)=final_q_mu_continue(:,t)/sum(final_q_mu_continue(:,t)); figure(t); %查看粒子分布% plot(x_par_birth(1,:,t),x_par_birth(3,:,t),'.r');% hold on ; plot(x_par_continue(1,:,t),x_par_continue(3,:,t),'.b'); axis([-100,700,-100,900]); hold on; if (t>6)&&(t<23 plot="" x_true="" 1="" t="" x_true="" 3="" t="" or="" end="" hold="" off="" if="" t="">=5)&&(t<=10) figureceive(t+40); % 查看每帧数据 x=1:num_z21_cell; y=1:num_z31_cell; [xx yy] = meshgrid(y, x); surf(xx, yy, t_data(:,:)); colorbar; axis([-inf, inf, -inf, inf, -inf, inf]); view(0, 90); colormap pink; end for n=1:N_continue_Particle if ((x_par_continue(1,n,t)>max_x)||(x_par_continue(3,n,t)>max_y)||(x_par_continue(1,n,t)<0)||(x_par_continue(3,n,t)<0)) final_q_mu_continue(n,t)=0; end end %================混合重采样============== x_estimate(:,1:N_birth_Particle)=x_par_birth(:,:,t); x_estimate(:,N_birth_Particle+1:N_birth_Particle+N_continue_Particle)=x_par_continue(:,:,t); %将两种粒子结合起来 q1(1:N_birth_Particle)=final_q_mu_birth(:,t); q1(N_birth_Particle+1:N_birth_Particle+N_continue_Particle)=final_q_mu_continue(:,t); q1 = q1/sum(q1); P = cumsum(q1); ut(1)=rand(1)/N_particle; k = 1; iii = zeros(1,N_particle); for j=1:N_particle ut(j)=ut(1)+(j-1)/N_particle; while(P(k)<ut(j)); k = k + 1; end iii(j) = k; end xp(:,:,t) = x_estimate(:,iii); %新的粒子 xx=xp(:,:,t)'; x_estimate_final(:,t) = mean(xx); %计算状态估计 end monte_x_estimate1(m,:)=x_estimate_final(1,:); %保存x方向位置的蒙特卡洛结果 monte_x_estimate3(m,:)=x_estimate_final(3,:); %保存y方向位置的蒙特卡洛结果 monte_Pe(m,:)=Pe; %保存检测概率的蒙特卡洛结果 location_error(1,:)=(x_estimate_final(1,:)-x_true(1,:)).^2+(x_estimate_final(3,:)-x_true(3,:)).^2;% RMS=sqrt(location_error); %均方根误差 monte_RMS(m,:)=location_error;endif m==1 monte_x_ex=monte_x_estimate1(:,:); %x方向蒙特卡洛平均估计值 monte_x_ey=monte_x_estimate3(:,:); %y方向蒙特卡洛平均估计值 monte_Pefinal=monte_Pe(:,:); monte_RMSfinal=sqrt(monte_RMS(m,:));else monte_x_ex=mean(monte_x_estimate1(:,:)); %x方向蒙特卡洛平均估计值 monte_x_ey=mean(monte_x_estimate3(:,:)); %y方向蒙特卡洛平均估计值 monte_Pefinal=mean(monte_Pe(:,:)); monte_RMSfinal=sqrt(mean(monte_RMS(:,:)));end time=toc%% 蒙特卡洛仿真结果figureceive(31);plot(x_true(1,start:disappear-1),x_true(3,start:disappear-1),'r-o',monte_x_ex(1,start:disappear-1),monte_x_ey(1,start:disappear-1),'b-h');title('目标真实运动轨迹和跟踪轨迹');% legend('真实轨迹','估计轨迹');xlabel('x/m');ylabel('y/m');hold on;plot(s1(1,1),s1(2,1),'pb',s2(1,1),s2(2,1),'pr',s3(1,1),s3(2,1),'pg');legend('真实轨迹','估计轨迹','观测站s1','观测站s2','观测站s3');hold off;figureceive(32);plot(1:Total_time,monte_Pefinal(1,1:Total_time),'k-*',[0 Total_time],[0.4,0.4],'b--');title('基于RPF-TBD方法的存在概率');ylabel('存在概率');xlabel('帧数');set(gca,'xtick',1:1:Total_time);set(gca,'ytick',0:0.1:1);axis([0,30,0,1]);grid on;figureceive(33)plot(start:disappear-1,monte_RMSfinal(start:disappear-1),'k-*');hold on;title('基于RPF-TBD方法的位置误差');ylabel('RMSE');xlabel('帧数');grid on;set(gca,'xtick',1:Total_time);hold off;