时差和频差无源定位方法研究matlab程序

【实例简介】一.dwmain.m为定位程序，1.调用了gj.m（轨迹方程）2.SC.m（时差定位程序）3.pc.m（频差定位程序） 二.GDOP_main.m为时差定位GDOP图 三.pcgdop_main.m为频差定位GDOP图

【程序说明】

一.dwmain.m为定位程序，1.调用了gj.m（轨迹方程）2.SC.m（时差定位程序）3.pc.m（频差定位程序）
二.GDOP_main.m为时差定位GDOP图
三.pcgdop_main.m为频差定位GDOP图

【实例截图】

【核心代码】

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%多普勒频率变化率/频差（多普勒频率）s、kHz、km、km/
%%%%%%%%%%%%%%%注意：载机位置和目标位置,载机和目标的速度差值愈大越好
%%%%%%%%%%%%%%%固定的比较好，因为速度差值比较大                                                      
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xk,m]=pc(xii,x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4,N)
tic;
v_x0=vx*10^3;v_y0=vy*10^3;v_z0=vz*10^3;
v_x1=vxo1*10^3;v_y1=vyo1*10^3;v_z1=vzo1*10^3;
v_x2=vxo2*10^3;v_y2=vyo2*10^3;v_z2=vzo2*10^3;
v_x3=vxo3*10^3;v_y3=vyo3*10^3;v_z3=vzo3*10^3;
v_x4=vxo4*10^3;v_y4=vyo4*10^3;v_z4=vzo4*10^3;
x=x*10^3;y=y*10^3;z=z*10^3;
x_10=xo1*10^3; y_10=yo1*10^3;z_10=zo1*10^3;
x_20=xo2*10^3; y_20=yo2*10^3;z_20=zo2*10^3;
x_30=xo3*10^3; y_30=yo3*10^3;z_30=zo3*10^3;
x_40=xo4*10^3; y_40=yo4*10^3;z_40=zo4*10^3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        仿真
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%计算多普勒频率%%%%%%估计值？
%%%%%%%%%%%%%%%%初值%%%%%%%%%%%%%%%
Xk=zeros(3,N);
Xk(:,1)=xii*10^3;%参考位置（初值很重要）！！！！！！！！！！！！！！！！！！！！
f0=6*10^9;
c=3*10^8;
TT=0.1;%阈值
w=0.01;
w1=w*rand(N,1);
w2=w*rand(N,1);
w3=w*rand(N,1);
w4=w*rand(N,1); 
dvx1=v_x0-v_x1;dvy1=v_y0-v_y1;dvz1=v_z0-v_z1;            %相对速度矢量（载机速度一致的基础上）
dvx2=v_x0-v_x2;dvy2=v_y0-v_y2;dvz2=v_z0-v_z2; 
dvx3=v_x0-v_x3;dvy3=v_y0-v_y3;dvz3=v_z0-v_z3; 
dvx4=v_x0-v_x4;dvy4=v_y0-v_y4;dvz4=v_z0-v_z4; 
v_x=[dvx1,dvx2,dvx3,dvx4];
v_y=[dvy1,dvy2,dvy3,dvy4];
v_z=[dvz1,dvz2,dvz3,dvz4];
%预测接收频率
f1=f0 (f0/c).*(((dvx1).*(x-x_10) (dvy1).*(y-y_10) (dvz1).*(z-z_10))./(sqrt((x-x_10).^2 (y-y_10).^2 (z-z_10).^2))) w1;
f2=f0 (f0/c).*(((dvx2).*(x-x_20) (dvy2).*(y-y_20) (dvz2).*(z-z_20))./(sqrt((x-x_20).^2 (y-y_20).^2 (z-z_20).^2))) w2;
f3=f0 (f0/c).*(((dvx3).*(x-x_30) (dvy3).*(y-y_30) (dvz3).*(z-z_30))./(sqrt((x-x_30).^2 (y-y_30).^2 (z-z_30).^2))) w3;
f4=f0 (f0/c).*(((dvx4).*(x-x_40) (dvy4).*(y-y_40) (dvz4).*(z-z_40))./(sqrt((x-x_40).^2 (y-y_40).^2 (z-z_40).^2))) w4;
dfd12=f2-f1;
dfd13=f3-f1;
dfd14=f4-f1;
Fd=[dfd12,dfd13,dfd14]';%频差Fd
f0=(mean(f1) mean(f2) mean(f3) mean(f4))/4;
H=zeros(3,3);
for i=2:N
 X_hat=Xk(:,i-1);  
 num=0;
 MM=200;
 while num<MM
%频率初值(我方飞机)
    fd10=(f0/c)*(((dvx1)*(X_hat(1)-x_10(i)) (dvy1)*(X_hat(2)-y_10(i)) (dvz1)*(X_hat(3)-z_10(i)))/(sqrt((X_hat(1)-x_10(i))^2 (X_hat(2)-y_10(i))^2 (X_hat(3)-z_10(i))^2)));
    fd20=(f0/c)*(((dvx2)*(X_hat(1)-x_20(i)) (dvy2)*(X_hat(2)-y_20(i)) (dvz2)*(X_hat(3)-z_20(i)))/(sqrt((X_hat(1)-x_20(i))^2 (X_hat(2)-y_20(i))^2 (X_hat(3)-z_20(i))^2)));
    fd30=(f0/c)*(((dvx3)*(X_hat(1)-x_30(i)) (dvy3)*(X_hat(2)-y_30(i)) (dvz3)*(X_hat(3)-z_30(i)))/(sqrt((X_hat(1)-x_30(i))^2 (X_hat(2)-y_30(i))^2 (X_hat(3)-z_30(i))^2)));
    fd40=(f0/c)*(((dvx4)*(X_hat(1)-x_40(i)) (dvy4)*(X_hat(2)-y_40(i)) (dvz4)*(X_hat(3)-z_40(i)))/(sqrt((X_hat(1)-x_40(i))^2 (X_hat(2)-y_40(i))^2 (X_hat(3)-z_40(i))^2)));
% 频差初值
    fd120=fd20-fd10;
    fd130=fd30-fd10;
    fd140=fd40-fd10;
    Fd0=[fd120;fd130;fd140];
    x_n=[x_20(i),x_30(i),x_40(i)];
    y_n=[y_20(i),y_30(i),y_40(i)];
    z_n=[z_20(i),z_30(i),z_40(i)]; 
%偏导
    xx=X_hat(1);
    yy=X_hat(2);
    zz=X_hat(3);    
%我方飞机   
    for n=1:3
        r0=sqrt( (xx-x_10(i))^2   (yy-y_10(i))^2   (zz-z_10(i))^2);
        r =sqrt( (xx-x_n(n))^2    (yy-y_n(n))^2    (zz-z_n(n))^2);
        H(n,1)=(f0/c)*(v_x(n 1)/r-((xx-x_n(n))*(v_x(n 1)*(xx-x_n(n)) v_y(n 1)*(yy-y_n(n)) v_z(n 1)*(zz-z_n(n)))/(r^3))-v_x(1)/r0 (xx-x_10(i))*((v_x(1)*(xx-x_10(i)) v_y(1)*(yy-y_10(i)) v_z(1)*(zz-z_10(i)))/(r0^3)));
        H(n,2)=(f0/c)*(v_y(n 1)/r-((yy-y_n(n))*(v_x(n 1)*(xx-x_n(n)) v_y(n 1)*(yy-y_n(n)) v_z(n 1)*(zz-z_n(n)))/(r^3))-v_y(1)/r0 (yy-y_10(i))*((v_x(1)*(xx-x_10(i)) v_y(1)*(yy-y_10(i)) v_z(1)*(zz-z_10(i)))/(r0^3)));
        H(n,3)=(f0/c)*(v_z(n 1)/r-((zz-z_n(n))*(v_x(n 1)*(xx-x_n(n)) v_y(n 1)*(yy-y_n(n)) v_z(n 1)*(zz-z_n(n)))/(r^3))-v_z(1)/r0 (zz-z_10(i))*((v_x(1)*(xx-x_10(i)) v_y(1)*(yy-y_10(i)) v_z(1)*(zz-z_10(i)))/(r0^3)));
    end
%频差偏导矩阵H,雅克比矩阵
%%%%%%%频差(化为线性方程)%%%%%%%%%%x0=[0,0,0];
    C=Fd(:,i)-Fd0;
    D=(H.'*H)\H.'*C;
    X_hat=X_hat D;
    %%%%%阈值
    if max(abs(D))<TT
       num=MM;
    else 
       num=num 1;
    end
 end
Xk(:,i)=X_hat;
% Xk(i 1,:)=Xk(i,:) ((inv(H'*H))*H'*(Fd(:,i)-Fd0))';
end
toc

% figure(2)
% plot3(x,y,z,'b--');
% xlabel('x(m)');
% ylabel('y(m)');
% zlabel('z(m)');
% grid on;
% hold on
% plot3(Xk(:,1),Xk(:,2),Xk(:,3),'r-');
% legend('目标位置','预测位置');
% xlabel('x(m)');
% ylabel('y(m)');
% zlabel('z(m)');
m=sqrt((Xk(1,:)-x').^2 (Xk(2,:)-y').^2 (Xk(3,:)-z').^2);
Xk=Xk/1000;
% figure(3)
% plot(m,'b--');
% xlabel('t'); 
% ylabel('误差(m)');
% axis([0,50 0,100])
end