📄 position_gg.m
字号:
%------------------------gps/galileo双系统定位程序-------by zhang zhirong----------------%
clear
%------------- 参数定义 -----------%
pi=3.1415926;
C=3.0e8; %光速
a_1=26609e3; %gps轨道长半轴长，单位已经换算为 m
a_2=29994e3; %galileo轨道长半轴长
e_1=0.006; %轨道的偏心率
e_2=0; %轨道的偏心率
i_0_gps=55*pi/180; %gps基准时间t_0的轨道倾角
i_0_galileo=56*pi/180; %galileo基准时间t_0的轨道倾角
%-- gps 采用的椭球参数 --%此程序中galileo也用这个椭球参数
a_e=6378137; %地球椭球的长半径
e_e=1/298.257223563; %地球椭球的第一偏心率
E0=10; %定义的仰角比较值
mu=3.986008e14; %开普勒常数，单位为m3/s2
w_ie=7.292115147e-5; %地球自转平均角速率，单位rad/s
% 卫星轨道参数矩阵，第一列卫星标号1~24，第二列升交点赤经W_0，第三列平近点角距M_0 %
% gps 卫星轨道参数矩阵，第一列卫星标号1~24，第二列升交点赤经W_0，第三列平近点角距M_0 %
sate_1=[
1 325.73 190.96;2 325.73 220.48;3 325.73 330.17;4 325.73 83.58;
5 25.73 249.90;6 25.73 352.12;7 25.73 25.25; 8 25.73 124.10;
9 85.73 286.20;10 85.73 48.94; 11 85.73 155.08;12 85.73 183.71;
13 145.73 312.30;14 145.73 340.93;15 145.73 87.06; 16 145.73 209.81;
17 205.73 11.90; 18 205.73 110.76;19 205.73 143.88;20 205.73 246.11;
21 265.73 52.42; 22 265.73 165.83;23 265.73 275.52;24 265.73 305.04];
% 卫星轨道参数矩阵，第一列卫星标号1~24，第二列升交点赤经W_0，第三列平近点角距M_0 %
%sate_2=[
% 1 120 0 ;2 120 36 ;3 120 72 ;4 120 108;5 120 144;
% 6 120 180;7 120 216;8 120 252;9 120 288;10 120 324;
% 11 0 12 ;12 0 48 ;13 0 84 ;14 0 120;15 0 156;
% 16 0 192;17 0 228;18 0 264;19 0 300;20 0 336;
% 21 240 24 ;22 240 60 ;23 240 96 ;24 240 132;25 240 168;
% 26 240 204;27 240 240;28 240 276;29 240 312;30 240 348
% ];
sate_2=[
1 18 6 ; 2 18 42 ; 3 18 78 ; 4 18 114 ; 5 18 150 ;
6 18 -30; 7 18 -66; 8 18 -102; 9 18 -138; 10 18 -174;
11 138 18.2 ; 12 138 54.2 ; 13 138 90.2 ; 14 138 126.2 ; 15 138 162.2 ;
16 138 -17.8; 17 138 -53.8; 18 138 -89.8; 19 138 -125.8; 20 138 -161.8;
21 258 10.4 ; 22 258 46.4 ; 23 258 82.4 ; 24 258 118.4 ; 25 258 154.4 ;
26 258 -25.6; 27 258 -61.6; 28 258 -97.6; 29 258 -133.6; 30 258 -169.6];
t_0=0; %星历的参考历元
a3_1=a_1^3;
a3_2=a_2^3;
n_1=sqrt(mu/a3_1); % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
n_2=sqrt(mu/a3_2); % n=(2*pi)/T=sqrt(mu/a3),应用了开普勒第三定律
k=1;
r=1;
%
t_u=0;
t_uu0=500; % 用户相对卫星的运行起始时间
fid = fopen('E:\工作\课题工作\所有源程序\position_gg\user_route.txt','r');
while 1
linestring = fgets(fid);
if linestring < 0
break;
end
if strncmp(linestring, '$GPRMC', 6)==1
[gps_inf,inf_num]=sscanf(linestring,'$GPRMC,%f,%1c,%2d%f,%*1c,%3d%f,%*1c,%f,%f\n');
while inf_num < 8
continue;
end
gps_lati=gps_inf(3)+gps_inf(4)/60.0; %如何将数据换算成的经纬度数据？
gps_longi=gps_inf(5)+gps_inf(6)/60.0;
t_u = t_u+1 % 实时显示用户运行时间 ,仿真结果为724
user(1,t_u)=gps_longi;%用户经纬高信息
user(2,t_u)=gps_lati;
user(3,t_u)=15;
% 用户在大地坐标系中的经纬度数据,经度L,纬度B,高度H %
user1(1,t_u)=user(1,t_u)*pi/180;%弧度转换
user1(2,t_u)=user(2,t_u)*pi/180;
L=user1(1,t_u);
B=user1(2,t_u);
H=user(3,t_u);
% 计算椭球的卯酉圈曲率半径N
W=sqrt(1-e_e^2*sin(B)^2);
N=a_e/W;
% 将用户在大地坐标系下的坐标转换为地球坐标系的空间直角坐标[xp,yp,zp]
xp(1,t_u)=(N+H)*cos(B)*cos(L);
yp(1,t_u)=(N+H)*cos(B)*sin(L);
zp(1,t_u)=(N*(1-e_e^2)+H)*sin(B);
% 求系数阵h
h(1,1)=-sin(B)*cos(L);h(1,2)=-sin(B)*sin(L);h(1,3)=cos(B);
h(2,1)=-sin(L); h(2,2)=cos(L); h(2,3)=0;
h(3,1)=cos(B)*cos(L); h(3,2)=cos(B)*sin(L); h(3,3)=sin(B);
t_k=t_uu0+t_u; % 找到对应于用户运行的时刻的卫星所在的位置，用户打第t_u个点时，时间为t_uu0+t_u
q=t_k; % 各个矩阵的行数表示量
t(t_u)=t_u;
%
i=1;
j=1; % 卫星标号
sum_s_1(q,1)=0; % 计算某时某地可见星和的矩阵
while j<=24 % 各个矩阵的列数表示量
M_k(q,j)=sate_1(j,3)+n_1*(t_k-t_0);
% ----- 求偏近点角 E_k ---- %
Et_1(q,j)=M_k(q,j);
t_end=1;
while(t_end)
Et(q,j)=M_k(q,j)+e_1*sin(Et_1(q,j));
delta_E(q,j)=Et(q,j)-Et_1(q,j);
Et_1(q,j)=Et(q,j);
if abs(delta_E(q,j))<=1.0e-6
E_k(q,j)=Et(q,j);
t_end=0;
end
end
%-- 求真近点角 f 的值 --%
%A=(cos(E_k(q,j))-e)/(1-e*cos(E_k(q,j))); %% f 的余弦
A=cos(E_k(q,j))-e_1; %分母一定是是大于0的数,所以只取分子来做判断
%B=(sin(E_k(q,j))*sqrt(1-e^2))/(1-e*cos(E_k(q,j))); %% f 的正弦
B=sqrt(1-e_1^2)*sin(E_k(q,j));
% 象限判断 %
if (A==0)
f(q,j)=pi/2;
elseif (B==0)
f(q,j)=pi;
else
f(q,j)=atan(abs(B/A));
if ((B>0)&(A<0))
f(q,j)=pi-f(q,j);
elseif ((B<0)&(A<0))
f(q,j)=pi+f(q,j);
elseif ((B<0)&(A>0))
f(q,j)=2*pi-f(q,j);
end
end
% 计算升交点角距u_k,地心距r_k,和轨道倾角i_k ,升交点的经度L_k %
% u_k(q,j)=w+f(q,j)+c_uc*cos(2*(w+f(q,j)))+c_us*sin(2*(w+f(q,j)));
u_k(q,j)=f(q,j);
% r_k(q,j)=a*(1-e*cos(E_k(q,j)))+c_rc*cos(2*(w+f(q,j)))+c_rs*sin(2*(w+f(q,j)));
r_k(q,j)=a_1*(1-e_1*cos(E_k(q,j)));
% i_k(q,j)=i_0+di_k+c_ic*cos(2*(w+f(q,j)))+c_is*sin(2*(w+f(q,j)));
i_k(q,j)=i_0_gps;
% L_k(q,j)=sate(j,2)+(dW-w_ie)*(t_k-t_0)-w_ie*t_0;
L_k(q,j)=sate_1(j,2)+w_ie*(t_k);%%%%%%%%%%% jia 还是 jian ？？
% 计算导航星的位置,在地心坐标系中,ECEF(Earth-Centered Earth-Fixed)坐标系 %
xk_1(q,j)=r_k(q,j)*cos(u_k(q,j))*cos(L_k(q,j))-r_k(q,j)*sin(u_k(q,j))*sin(L_k(q,j))*cos(i_k(q,j));
yk_1(q,j)=r_k(q,j)*cos(u_k(q,j))*sin(L_k(q,j))+r_k(q,j)*sin(u_k(q,j))*cos(L_k(q,j))*cos(i_k(q,j));
zk_1(q,j)=r_k(q,j)*sin(u_k(q,j))*sin(i_k(q,j));
%---计算仰角 E=arctan(Z/sqrt(X^2+Y^2)) ,E_rad单位rad ,E_deg单位度 --%
delta_x(q,j)=xk_1(q,j)-xp(1,t_u);
delta_y(q,j)=yk_1(q,j)-yp(1,t_u);
delta_z(q,j)=zk_1(q,j)-zp(1,t_u);
%--求卫星在 %%% 站心坐标系下 %%%% 的坐标---%
X_sta(q,j)=h(1,1)*delta_x(q,j)+h(1,2)*delta_y(q,j)+h(1,3)*delta_z(q,j);
Y_sta(q,j)=h(2,1)*delta_x(q,j)+h(2,2)*delta_y(q,j)+h(2,3)*delta_z(q,j);
Z_sta(q,j)=h(3,1)*delta_x(q,j)+h(3,2)*delta_y(q,j)+h(3,3)*delta_z(q,j);
%---------------------给出对应各颗卫星的星历误差---------------------%
%d_star(j)=0;
d_star(j)=50+randn(1);
%--------------------------------------------------------------------%
E_deno(q,j)=X_sta(q,j)^2+Y_sta(q,j)^2;
E_deno(q,j)=sqrt(E_deno(q,j));
if E_deno(q,j)==0
E_rad(q,j)=pi/2;
E_deg(q,j)=90;
else
E_rad(q,j)=atan(Z_sta(q,j)/E_deno(q,j));
E_deg(q,j)=E_rad(q,j)*180/pi;
end
%-- 开始高度角比较 ,E0 为给定的高度角,判断可见星 --%
ele_1(q,j)=E_deg(q,j);
if ele_1(q,j)>=E0
ele_1(q,j)=1;
if r~=q
i=1;
r=q;
end
s_n_1(q,i)=j; % j :可见星标号
%-----------%
if s_n_1(q,i)~=0 % 将可见星提取出来
sd(i)=s_n_1(q,i);% sd ;可见星标号阵
% -地心坐标系下站星的几何距离 rou - %
rou(q,i)=(xk_1(q,sd(i))-xp(1,t_u))^2 + (yk_1(q,sd(i))-yp(1,t_u))^2 + (zk_1(q,sd(i))-zp(1,t_u))^2;
rou(q,i)=sqrt(rou(q,i));
%------------------------------以下要产生伪距R------------------------------%
%d_T(t_u)=0;
d_T(t_u)=100000+randn(1);%-仿真给出接收机在各个时刻的钟差,即折合的距离误差-%
%---------------------------------------%
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -