pdaf算法matlab程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序功能 :采用数据关联算法实现单个匀速运动目标的点迹与航迹的关联
% 输入变量 :
% -target_position: 目标的初始位置 target_position=[1500 500 1500 400];
% - n: 采样次数 50
% - T: 采样间隔 1
% -MC_number:仿真次数 5
%状态方程 ： X(n)=A*X(n-1)+G*Q
%观测方程 ： Z(n)=C*X(n)+R
% 输出变量 :
% 无
% 作者 : 冯洋
% 日期 : 05-28-2007
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function PDAF(target_position,n,T,MC_number)
tic
clear all
close all
n=50; %采样次数
T=1; %T为采样间隔
MC_number=5; %monte carlos run times
target_position=[1500 500 1500 400]; %目标的起始位置和速度(km,km/s) （初始状态向量）

Pd=0.95; %检测概率
Pg=0.99; %正确量测落入跟踪门内得概率
g_sigma=9.21; %门限
gamma=7*10^(-6); %每一个单位面积(km^2)内产生2个杂波 ????????（虚假量测的空间密度lamda）



A = [1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %状态转移矩阵 (F)
C = [1 0 0 0;0 0 1 0]; %观测矩阵
target_delta=100; %目标对应的观测标准差
R=[target_delta^2 0;0 target_delta^2]; %观测协方差矩阵 (Q2)
Q=[4 0;0 4]; %过程噪声协方差 (Q1)
G=[T^2/2 0;T 0;0 T^2/2;0 T]; %系统过程噪声矩阵 (H)
P=[target_delta^2 target_delta^2/T 0 0;target_delta^2/T 2*target_delta^2/T^2 0 0;0 0 target_delta^2 target_delta^2/T;0 0 target_delta^2/T 2*target_delta^2/T^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采样次数 (观测向量[x;y],目标坐标）
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); %错误回波数 ？为什么要乘以10--对应的正方形区域的面积是椭圆的10倍，回波数目也是椭圆的10倍
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); %第t次测量的x坐标
b(2)=data_measurement(2,t);

%第t次测量的y坐标
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 （b(k))
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)=exp(-a/2); %求ei(k)
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)'; % 为后面求滤波误差（状态估计误差）协方差矩阵P做准备
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; %滤波值作平均
% sum(x_filter1,3) 把MC_humber个矩阵的对应元素相加 ，x_filter1是一个三维矩阵，第三维的长度是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 %画出目标Y轴的估计位置，测量位置
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)