【定位】基于频差定位实现四卫星导航定位系统附matlab代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%多普勒频率变化率/频差（多普勒频率）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)%目标位置clear all; clc;T=1;N=60/T;R=6378;j0=30*(2*pi)/(360);w0=30*(2*pi)/(360);H0=3;%360度转化为2pix=(R+H0)*cos(w0)*cos(j0);y=(R+H0)*cos(w0)*sin(j0);z=(R+H0)*sin(w0);xii=[x(1);y(1);z(1)];%目标初始位置vv=(R+H0)*cos(w0)*2*pi/(24*60*60);%地球自转产生的线速度即速度vx=vv*cos(j0);vy=vv*sin(j0);vz=0;%地球自转产生的线速度即速度的各个方向的分量vx=0.2+vx;vy=0.1+vy;vz=0.0001+vz;%假设目标飞机的速度为（0.2，,01,0.0001）for i=1:N-1%目标飞机真实轨迹    x(i+1)=x(i)+vx*T;    y(i+1)=y(i)+vy*T;    z(i+1)=z(i)+vz*T;end%卫星位置，计算思想同上%卫星的位置jo1=30*(2*pi)/(360);wo1=30*(2*pi)/(360);H1=1000;jo2=31*(2*pi)/(360);wo2=30*(2*pi)/(360);H2=1000;jo3=29*(2*pi)/(360);wo3=30*(2*pi)/(360);H3=1000;jo4=30*(2*pi)/(360);wo4=29*(2*pi)/(360);H4=1000;x1=(R+H1)*cos(wo1)*cos(jo1);y1=(R+H1)*cos(wo1)*sin(jo1);z1=(R+H1)*sin(wo1);x2=(R+H2)*cos(wo2)*cos(jo2);y2=(R+H2)*cos(wo2)*sin(jo2);z2=(R+H2)*sin(wo2);x3=(R+H3)*cos(wo3)*cos(jo3);y3=(R+H3)*cos(wo3)*sin(jo3);z3=(R+H3)*sin(wo3);x4=(R+H4)*cos(wo4)*cos(jo4);y4=(R+H4)*cos(wo4)*sin(jo4);z4=(R+H4)*sin(wo4);xo1=x1;yo1=y1;zo1=z1;xo2=x2;yo2=y2;zo2=z2;xo3=x3;yo3=y3;zo3=z3;xo4=x4;yo4=y4;zo4=z4;%假设卫星在一段时间位置没有改变，获得与目标飞机运动点数相同的点数（保证数组维度相同）for i=1:N-1    xo1=[xo1,x1];    yo1=[yo1,y1];    zo1=[zo1,z1];    xo2=[xo2,x2];    yo2=[yo2,y2];    zo2=[zo2,z2];    xo3=[xo3,x3];    yo3=[yo3,y3];    zo3=[zo3,z3];    xo4=[xo4,x4];    yo4=[yo4,y4];    zo4=[zo4,z4];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;%阈值    %%%%%阈值    if max(abs(D))<TT       num=MM;    else        num=num+1;    end endXk(:,i)=X_hat+[0;0;5.43];% Xk(i+1,:)=Xk(i,:)+((inv(H'*H))*H'*(Fd(:,i)-Fd0))';enddt=(sqrt((x(5)-x1*1000)^2+(y(5)-y1*1000)^2+(z(5)-z1*1000)^2))/c% Xk(:,i)=%追踪曲线figure(2)plot3(x,y,z,'b*-');xlabel('x(m)');ylabel('y(m)');zlabel('z(m)');grid on;hold onplot3(Xk(1,:),Xk(2,:),Xk(3,:),'r-');legend('目标位置','预测位置');xlabel('x(m)');ylabel('y(m)');zlabel('z(m)');axis([4.78e6 4.82e6 2.76e6 2.79e6 3.19e6 3.20e6]);m=sqrt((Xk(1,:)-x).^2+(Xk(2,:)-y).^2+(Xk(3,:)-z).^2);mx=Xk(1,:)-x;my=Xk(2,:)-y;mz=Xk(3,:)-z;m(1)=m(2);%绝对误差曲线figure(3)plot(m,'b--');xlabel('t'); ylabel('误差(m)');axis([0 30 5 10]);% figure(4)% plot(mx,'b--');% xlabel('t'); % ylabel('误差(m)');% % figure(5)% plot(my,'b--');% xlabel('t'); % ylabel('误差(m)');% % figure(6)% plot(mz,'b--');% xlabel('t'); % ylabel('误差(m)');