PDA.rar_PDA关联算法CSDN_PDA跟踪_tracking matlab_数据关联PDA_跟踪

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序功能 :采用数据关联算法实现单个匀速运动目标的点迹与航迹的关联 % 输入变量 : % -target_position: 目标的初始位置 % - n: 采样次数 % - T: 采样间隔 % -MC_number:仿真次数 % 输出变量 : % 无 % 作者 : 冯洋 % 日期 : 05-28-2007 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function PDAF(target_position,n,T,MC_number) tic Pd=1; %检测概率 Pg=0.99; %正确量测落入跟踪门内得概率 g_sigma=9.21; %门限 gamma=1*10^(-6); %每一个单位面积(km^2)内产生2个杂波 A = [1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %状态转移矩阵 C = [1 0 0 0;0 0 1 0]; %观测矩阵 target_delta=100; %目标对应的观测标准差 R=[target_delta^2 0;0 target_delta^2]; %观测协方差矩阵 Q=[4 0;0 4]; %过程噪声协方差 G=[T^2/2 0;T 0;0 T^2/2;0 T]; %过程噪声矩阵 P=[target_delta^2 target_delta^2 0 0;target_delta^2 2*target_delta^2 0 0;0 0 target_delta^2 target_delta^2;0 0 target_delta^2 2*target_delta^2]; %初始协方差矩阵 x_filter=zeros(4,n); %存储目标的各时刻的滤波值 x_filter1=zeros(4,n,MC_number); %MC_number次montle carlo仿真所得全部结果存储矩阵 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %产生目标的实际位置 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% data_measurement1=zeros(4,n); %data_measurement1实际位置矩阵 data_measurement1(:,1)=target_position'; for i=2:n data_measurement1(:,i)=A*data_measurement1(:,i-1)+G*sqrt(Q)*(randn(2,1)); %实际位置 end plot(data_measurement1(1,:),data_measurement1(3,:),'-'); xlabel('x(m)'),ylabel('y(m)'); legend('目标的实际位置',4) %axis([0 30 1 25]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %主程序 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for M=1:MC_number %产生路径 Noise=[]; data_measurement=zeros(2,n); %观测存储矩阵,n采样次数 for i=1:n data_measurement(1,i)=data_measurement1(1,i)+randn(1)*target_delta; data_measurement(2,i)=data_measurement1(3,i)+randn(1)*target_delta; %data_measurement观测值矩阵,传感器观测的位置 end NOISE=[]; %滤波开始 for t=1:n if t~=1 x_predic = A*x_filter(:,t-1); %用前一时刻的滤波值来预测当前的值 else x_predic = target_position'; %第一次采样我们用真实位置当预测值 end P_predic = A*P*A'+G*Q*G'; Z_predic = C*x_predic; S = C*P_predic*C'+ R; K = P_predic*C'*inv(S); %增益 ellipse_Volume=pi*g_sigma*sqrt(det(S)); %计算椭球体积，这里算的是面积 number_returns=floor(10*ellipse_Volume*gamma+1); %错误回波数 side=sqrt((10*ellipse_Volume*gamma+1)/gamma)/2; %求出正方行边长的二分之一 Noise_x=x_predic(1)+side-2*rand(1,number_returns)*side; %在预测值周围产生多余回波 Noise_y=x_predic(3)+side-2*rand(1,number_returns)*side; Noise=[Noise_x ;Noise_y]; NOISE=[NOISE Noise]; % b=zeros(1,2); b(1)=data_measurement(1,t); b(2)=data_measurement(2,t); y1=[Noise b']; %将接收到的所有的回波存在y1中 y=[]; d=[]; m=0; for j=1:(number_returns+1) d=y1(:,j)-Z_predic; D=d'*inv(S)*d; if D<=g_sigma y=[y y1(:,j)]; %把落入跟踪门中的所有回波放入y中 m=m+1; %记录观测个数 end end %m=0表示无有效回波 Bk=gamma*2*pi*sqrt(det(S))*(1-Pd*Pg)/Pd; %算b0 if m==0 x_filter(:,t)= x_predic; P=P_predic; %无回波的情况 else E=zeros(1,m); belta=zeros(1,m); for i=1:m a=(y(:,i)-Z_predic)'*inv(S)*(y(:,i)-Z_predic); E(i)=E(i)+exp(-a/2); end belta0=Bk/(Bk+sum(E)); %无回波时的关联概率 v=zeros(2,1); v1=zeros(2,2); for i=1:m belta(i)=E(i)/(Bk+sum(E)); %算关联概率 v=v+belta(i)*(y(:,i)-Z_predic); v1=v1+belta(i)*(y(:,i)-Z_predic)*(y(:,i)-Z_predic)'; end x_filter(:,t)= x_predic + K*v; %算协方差 Pc=(eye(4)-K*C)*P_predic; PP=K*(v1-v*v')*K'; P=belta0*P_predic+(1-belta0)*Pc+PP; end x_filter1(:,t,M)=x_filter(:,t); end end toc % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %画图 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%R=sum(U1,4)/MC_number; x_filter=sum(x_filter1,3)/MC_number; %滤波值作平均 figure %画出目标的估计位置，测量位置，杂波位置 plot(x_filter(1,:),x_filter(3,:),'r-'),hold on %plot(data_measurement1(1,:),data_measurement1(3,:),'-') plot(data_measurement(1,:),data_measurement(2,:),'k-') plot(NOISE(1,:),NOISE(2,:),'.') %杂波位置 %axis([0 30 1 25]) xlabel('x(m)'),ylabel('y(m)'); legend('估计的位置','测量的位置','杂波位置',4) figure %画出目标X轴的估计位置，测量位置 plot(1:n,x_filter(1,:),'r-'),hold on %plot(1:n,data_measurement1(1,:),'-') plot(data_measurement(1,:),'k-') %axis([0 50 1 30]) xlabel('t(s)'),ylabel('x(m)'); legend('X方向估计位置','X方向测量位置',4) figure %画出目标X轴的估计位置，测量位置 plot(1:n,x_filter(3,:),'r-'),hold on %plot(1:n,data_measurement1(3,:),'-') plot(data_measurement(2,:),'k-') %axis([0 50 1 25]) xlabel('t(s)'),ylabel('y(m)'); legend('Y方向估计位置','Y方向测量位置',4) figure a=zeros(1,n); %最小均方误差 for j=1:n a(1,j)=sqrt((x_filter(1,j)-data_measurement1(1,j))^2+(x_filter(3,j)-data_measurement1(3,j))^2); end plot(1:n,a(1,:),'r:') xlabel('t(s)'),ylabel('预测误差(m)'); figure plot(1:n,x_filter(2,:),'r-'),hold on plot(1:n,x_filter(4,:),'-'); xlabel('t(s)'),ylabel('速度(m/s)'); legend('X方向速度','Y方向速度',4)