www.pudn.com > PDA.rar > PDAF.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序功能 :采用数据关联算法实现单个匀速运动目标的点迹与航迹的关联
% 输入变量 :
% -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)