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=zero
s(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=zer
os(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)]; …… 此处隐藏：3917字，全部文档内容请下载后查看。喜欢就下载吧 ……