并行计算

开发平台：
MultiPlatform

JPDA.txt：源码内容
							%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%多目标跟踪    
%概率数据关联滤波器   standard_jpdaf      
%两个目标两个目标交叉运动场景  Kalman滤波进行状态估计
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m=2;
Pd=1;
Pg=0.99;
g_sigma=9.21;                                             %门限
gamma=1;                                                  %杂波个数
Target_measurement=zeros(c,2,n);                          %目标观测互联存储矩阵
target_delta=[0.150 0.150];                               %目标对应的观测标准差   
data_measurement=zeros(c,n);                              %观测存储矩阵,n采样次数    行---代表目标 列---代表传感器                   
P=zeros(4,4,c);                                           %滤波时协方差更新
P1=[target_delta(1)^2 0 0 0;0 0.01 0 0;0 0 target_delta(1)^2 0;0 0 0 0.01];
P(:,:,1)=P1;
P(:,:,2)=P1;
x_filter=zeros(4,c,n);                                    %存储所有六个目标的各时刻的滤波值
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]; 
G=[T^2/2 0;T 0;0 T^2/2;0 T];        
x_filter1=zeros(4,c,n,MC_number);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%主程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
tic
for M=1:MC_number
%产生路径
Noise=[];
for i=1:n
    for j=1:c
            data_measurement(j,1,i)=data_measurement1(j,1,i)+randn(1)*target_delta(j);
            data_measurement(j,2,i)=data_measurement1(j,2,i)+randn(1)*target_delta(j);                %各传感器观测的位置
    end
end
% for i=1:c
%     a=zeros(1,n);
%     b=zeros(1,n);
%     for j=1:n
%         a(j)=data_measurement1(i,1,j);b(j)=data_measurement1(i,2,j);
%     end
%     plot(a(:),b(:),'o'),hold on
% end
% for i=1:c
%     a=zeros(1,n);
%     b=zeros(1,n);
%     for j=1:n
%         a(j)=data_measurement(i,1,j);b(j)=data_measurement(i,2,j);
%     end
%     plot(a(:),b(:),'*'),hold on
% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%产生杂波，并确定有效观测
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
S=zeros(2,2,c);
Z_predic=zeros(2,2);                                                     %存储两个目标的观测预测值
x_predic=zeros(4,2);                                                     %存储两个目标的状态预测值 
ellipse_Volume=zeros(1,2);
for t=1:n
    y1=[];
    y=[];
    NOISE=[];
    for i=1:c                                                              %产生在每个椭圆域中的杂波数
        Noise=[];
    if t~=1
        x_predic(:,i) = A*x_filter(:,i,t-1);                               %用前一时刻的滤波值来预测当前的值 
    else
        x_predic(:,i)=target_position(:,i);                                %第一次采样我们用真实位置当预测值 
    end
    P_predic = A*P(:,:,i)*A'+G*Q*G';                                    
    Z_predic(:,i) = C*x_predic(:,i);
    R=[target_delta(i)^2  0;0  target_delta(i)^2];
    S(:,:,i)= C*P_predic*C'+ R;
    %============??????????????????????????????????????/
    ellipse_Volume(i)=pi*g_sigma*sqrt(det(S(:,:,i)));                     %计算椭球体积，这里算的是面积   
    number_returns=floor(10*ellipse_Volume(i)*gamma+1);                   %错误回波数
    side=sqrt((10*ellipse_Volume(i)*gamma+1)/gamma)/2;                    %求出正方行边长的二分之一
    Noise_x=x_predic(1,i)+side-2*rand(1,number_returns)*side;               %在预测值周围产生多余回波
    Noise_y=x_predic(3,i)+side-2*rand(1,number_returns)*side;    
    Noise=[Noise_x ;Noise_y];
    NOISE=[NOISE Noise];
    %==============????????????????????????????????????=
    end    
    b=zeros(1,2);
    b(1)=data_measurement(1,1,t);
    b(2)=data_measurement(1,2,t);
    y1=[NOISE b'];                                                      %将接收到的所有的回波存在y1中%杂波、观测都放在y中
    b(1)=data_measurement(2,1,t);
    b(2)=data_measurement(2,2,t);
    y1=[y1 b'];        %当有一个杂波回波时，y1[2*282]282=(2+2(2个回波)+2+2(2个杂波))*35+2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%产生观测确认矩阵Q2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    m1=0;                                                                 %记录有效观测个数
    [n1,n2]=size(y1);
    Q1=zeros(100,3);
    for j=1:n2 
        flag=0;
        for i=1:c
            d=y1(:,j)-Z_predic(:,i);
            D=d'*inv(S(:,:,i))*d;                       
            if D<=g_sigma                                                    
               flag=1;
               Q1(m1+1,1)=1;Q1(m1+1,i+1)=1;
            end
        end
            if flag==1   
               y=[y y1(:,j)];                                              %把落入跟踪门中的所有回波放入y中
               m1=m1+1;                                                    %记录有效观测个数
            end
    end
    Q2=Q1(1:m1,1:3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%产生互联矩阵A_matrix   num表示可行联合事件个数                  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A_matrix=zeros(m1,3,10000);
A_matrix(:,1,1:10000)=1;
if m1~=0
    num=1;                                  %num＝1表示两个目标都没有观测时
%当目标1有有效观测
    for i=1:m1
        if Q2(i,2)==1
            A_matrix(i,2,num)=1;A_matrix(i,1,num)=0;
            num=num+1;
            for j=1:m1
                if (i~=j)&(Q2(j,3)==1)
                    A_matrix(i,2,num)=1;A_matrix(i,1,num)=0;
                    A_matrix(j,3,num)=1;A_matrix(j,1,num)=0;
                    num=num+1;
                end
            end
        end
    end                                   
%当目标2无观测时
    for i=1:m1
        if Q2(i,3)==1
            A_matrix(i,3,num)=1;A_matrix(i,1,num)=0;
            num=num+1;
        end
    end
else
    flag=1;
end
A_matrix=A_matrix(:,:,1:num);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 计算后验概率Pr,  False_num表示假量测，mea_indicator表示观测指示器，target_indicator表示目标指示器
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
Pr=zeros(1,num);
for i=1:num
    False_num=m1;
    N=1;
    for j=1:m1
        mea_indicator=sum(A_matrix(j,2:3,i));
        if mea_indicator==1
            False_num=False_num-1;
            if A_matrix(j,2,i)==1
                b=(y(:,j)-Z_predic(:,1))'*inv(S(:,:,1))*(y(:,j)-Z_predic(:,1));
                N=N/sqrt(det(2*pi*S(:,:,1)))*exp(-1/2*b);                  %如果观测与目标1关联
            else
                b=(y(:,j)-Z_predic(:,2))'*inv(S(:,:,2))*(y(:,j)-Z_predic(:,2));
                N=N/sqrt(det(2*pi*S(:,:,2)))*exp(-1/2*b);                  %如果观测与目标2关联
            end                                                          %计算正态分布函数    
        end
    end
    if Pd==1
        a=1;
    else
        a=1;
        for j=1:c
            target_indicator=sum(A_matrix(:,j+1,i));
            a=a*Pd^target_indicator*(1-Pd)^(1-target_indicator);
        end
    end                                                                 %计算检测概率
    V=ellipse_Volume(1)+ellipse_Volume(2);                            %表示整个空域的体积
    a1=1;
    for j=1:False_num
        a1=a1*j;
    end
    Pr(i)=N*a*a1/(V^False_num);
end
Pr=Pr/sum(Pr);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%计算关联概率U
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
U=zeros(m1+1,c);
for i=1:c
    for j=1:m1
        for k=1:num
            U(j,i)=U(j,i)+Pr(k)*A_matrix(j,i+1,k);
        end
    end
end
U(m1+1,:)=1-sum(U(1:m1,1:c));                          %无量测与目标T互联的关联概率存入U（m1+1,:),规一化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%滤波开始  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for i=1:c                                                                            % 更新协方差矩阵
    P_predic = A*P(:,:,i)*A'+G*Q*G';
    K (:,:,i)= P_predic*C'*inv(S(:,:,i));
    P(:,:,i)= P_predic-(1-U(m1+1,i))*K(:,:,i)*S(:,:,i)*K(:,:,i)';
end
for i=1:c
    a=0;         
    b=0;
    x_filter2=0;%随便设置的中间参数
    for j=1:m1
        x_filter2=x_filter2+U(j,i)*(x_predic(:,i)+ K (:,:,i)*(y(:,j)- Z_predic(:,i)));
    end
    x_filter2=U(j+1,i)*x_predic(:,i)+x_filter2;
    x_filter(:,i,t)=x_filter2;
    for j=1:m1+1
        if j==m1+1
            a=x_predic(:,i);
        else
           a=x_predic(:,i)+ K (:,:,i)*(y(:,j)- Z_predic(:,i));
        end
        b=b+U(j,i)*(a*a'-x_filter2*x_filter2');
    end
    P(:,:,i)=P(:,:,i)+b; 
    x_filter1(:,i,t,M)=x_filter(:,i,t);   
end
end
end
toc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%画图
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
x_filter=sum(x_filter1,4)/MC_number;                                %滤波值作平均
a=zeros(1,n);
b=zeros(1,n);
figure(1)
for i=1:c
    a=zeros(1,n);
    b=zeros(1,n);
    for j=1:n
        a(j)=data_measurement1(i,1,j);b(j)=data_measurement1(i,2,j);
    end
    plot(a(:),b(:)),hold on
end
% axis([5900 8500 5900 6200 ])
for i=1:c
    a=zeros(1,n);
    b=zeros(1,n);
    for j=1:n
        a(j)=x_filter(1,i,j);b(j)=x_filter(3,i,j);
    end
if i==1
    plot(a(:),b(:),'m+')
else 
    plot(a(:),b(:),'+')
end
end
xlabel('x/km'),ylabel('y/km');
%估计误差
% a=zeros(c,n);b=zeros(c,n);c1=zeros(1,n);MEASURE=zeros(2,c,n);
% 
% for j=1:n
%     a(1,j)=sqrt((x_filter(1,1,j)-data_measurement1(1,1,j))^2+(x_filter(3,1,j)-data_measurement1(1,2,j))^2);%最小均方误差
%     c1(1,j)=c1(1,j)+a(1,j);
% end
% figure(3)
% plot(1:n,c1(1,:),'r:') 
% hold on
% c1=zeros(1,n);
% for j=1:n
%         a(1,j)=sqrt((x_filter(1,2,j)-data_measurement1(2,1,j))^2+(x_filter(3,2,j)-data_measurement1(2,2,j))^2);%最小均方误差
%         c1(1,j)=c1(1,j)+a(1,j);
% end
% plot(1:n,c1(1,:),'r-') 
% % axis([0 20 0 11 ])
% xlabel('times'),ylabel('Mean Estimated Errors/km');
% legend('target1','target2')
a=0;b=zeros(c,n);c1=zeros(c,n);
for j=1:n
    for i=1:MC_number
        a=(x_filter1(1,1,j,i)-data_measurement1(1,1,j))^2+(x_filter1(3,1,j,i)-data_measurement1(1,2,j))^2;%最小均方误差
        c1(1,j)=c1(1,j)+a;
    end
        c1(1,j)=sqrt(c1(1,j)/MC_number);
end
figure(3)
plot(1:n,c1(1,:),'r:') 
hold on
for j=1:n
    for i=1:MC_number
        a=(x_filter1(1,2,j,i)-data_measurement1(2,1,j))^2+(x_filter1(3,2,j,i)-data_measurement1(2,2,j))^2;%最小均方误差
        c1(2,j)=c1(2,j)+a;
    end
        c1(2,j)=sqrt(c1(2,j)/MC_number);
end
plot(1:n,c1(2,:),'r-') 
% axis([0 20 0 11 ])
xlabel('times'),ylabel('RMS Position  Errors /km');
legend('Target1','Target2')
 %单独画出每个目标的曲线
figure(4)
c1=zeros(c,n);
for j=1:n
    for i=1:MC_number
        a=(x_filter1(1,1,j,i)-data_measurement1(1,1,j))^2+(x_filter1(3,1,j,i)-data_measurement1(1,2,j))^2;%最小均方误差
        c1(1,j)=c1(1,j)+a;                                                                              %目标一
    end
        c1(1,j)=sqrt(c1(1,j)/MC_number);
end
plot(1:n,c1(1,:),'r:')
xlabel('times'),ylabel('RMS Position  Errors /km');
legend('Target1')
figure(5)
for j=1:n
    for i=1:MC_number
        a=(x_filter1(1,2,j,i)-data_measurement1(2,1,j))^2+(x_filter1(3,2,j,i)-data_measurement1(2,2,j))^2;%最小均方误差
        c1(2,j)=c1(2,j)+a;
    end                                                                                        % 目标2
        c1(2,j)=sqrt(c1(2,j)/MC_number);
end
plot(1:n,c1(2,:),'r-') 
% axis([0 20 0 11 ])
xlabel('times'),ylabel('RMS Position  Errors /km');
legend('Target2')
%
% c1=zeros(1,n);
% for j=1:n
%     a(1,j)=sqrt((x_filter(2,1,j)-target_position(2,1))^2+(x_filter(4,1,j)-target_position(4,1))^2);%最小均方误差
%     c1(1,j)=c1(1,j)+a(1,j);
% end
% figure(4)
% plot(1:n,c1(1,:),'r:') 
% hold on
% c1=zeros(1,n);
% for j=1:n
%     a(1,j)=sqrt((x_filter(2,2,j)-target_position(2,2))^2+(x_filter(4,2,j)-target_position(4,2))^2);%最小均方误差
%     c1(1,j)=c1(1,j)+a(1,j);
% end
% plot(1:n,c1(1,:),'r-') 
% xlabel('times'),ylabel('Mean Estimated Errors km/s');
%给出速度的RMS曲线]
a=0;b=zeros(c,n);c1=zeros(c,n);
for j=1:n
    for i=1:MC_number
        a=(x_filter1(2,1,j,i)-target_position(2,1))^2+(x_filter1(4,1,j,i)-target_position(4,1))^2;%最小均方误差
        c1(1,j)=c1(1,j)+a;
    end
        c1(1,j)=sqrt(c1(1,j)/MC_number);
end
figure(6)
plot(1:n,c1(1,:),'r:') 
hold on
for j=1:n
    for i=1:MC_number
        a=(x_filter1(2,2,j,i)-target_position(2,2))^2+(x_filter1(4,2,j,i)-target_position(4,2))^2;%最小均方误差
        c1(2,j)=c1(2,j)+a;
    end
        c1(2,j)=sqrt(c1(2,j)/MC_number);
end
plot(1:n,c1(2,:),'r-') 
% axis([0 20 0 11 ])
xlabel('times'),ylabel('RMS Position  Errors /km');
legend('Target1','Target2')
% legend('target1','target2')

						