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

matlab科研社

已于 2024-12-24 21:47:53 修改

阅读量317

点赞数 1

文章标签： matlab 开发语言

于 2023-06-29 07:14:58 首次发布

本文链接：https://blog.csdn.net/Matlab_dashi/article/details/131459004

版权

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

⛄ 内容介绍

基于频差定位(Differential Doppler Positioning)一种利用卫星导航系统测量接收机位置的技术。在四卫星导航定位系统中，通过同时接收来自至少四颗卫星的信号，并测量其频率差异来实现定位。以下是基于频差定位实现四卫星导航定位系统的一般步骤：

接收卫星信号：使用接收机接收来自至少四颗卫星的导航信号。这些信号包含了卫星的导航信息和时间标记。
频差测量：将接收到的卫星信号与本地参考信号进行比较，测量不同卫星之间的频率差异。通常，这可以通过对信号进行频谱分析或相关运算来实现。
绝对范围计算：根据接收到的频差以及每颗卫星的导航信息，计算接收机与每颗卫星之间的绝对距离。这需要使用多普勒效应等相关公式计算。
定位解算：利用已知卫星的位置信息和接收机与卫星之间的距离，使用三。常用的定位算法包括最小二乘(Least Squares)解算、加权解算等。

输出接收机的位置坐标和相关的误差估计。这通常以地理坐标系(如经度、纬度、海拔)或其他参考坐标系统表示。

需要注意的是，基于频差定位的精度受到多种因素的影响，如信号传播路径、多径效应、接收机性能、卫星几何配置等。为了提高精度和可靠性，可以采取一些增强策略，例如采用差分定位、使用更多的卫星进行定位、采集更高频率的测量数据等。

对于实际的导航定位应用系统，还需要考虑数据处理和误差补偿技术、姿态信息、考虑，以获得更精确的导航位置信息。

⛄ 部分代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%多普勒频率变化率/频差(多普勒频率)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度转化为2pi
x=(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
 end
Xk(:,i)=X_hat+[0;0;5.43];
% Xk(i+1,:)=Xk(i,:)+((inv(H'*H))*H'*(Fd(:,i)-Fd0))';
end
dt=(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 on
plot3(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)');