📄 mainsimulation.m
字号:
% function weixing(t1,t)
%输入卫星轨道参数
clear;
d=(60/180)*pi; %升交点赤径
i9=1;
%输入接受机位子大地坐标
B0=input('longitude(经度):'); %输入经度
L=input('latitude(纬度):'); %输入纬度
% H=input('high:');
%B0=103.51 ;
%L=1.17 ;
B0=(B0/180)*pi;
L=(L/180)*pi;
%卫星经过近地点的时刻（单位:秒）
%t1=input('nowtime:'); %卫星观测时刻（单位:秒）
t1=0;
H=0;
R=6378000; %地球半径（单位：m)
n0 = (2*pi)/(14*3600+240); %卫星运行平均角速度（单位:弧度/秒）
Gt=(14*3600+4*60)/9; %卫星运行时的时间间隔（单位:秒）
%将大地坐标转化为空间直角坐标（单位：km）
XJ=(R+H)*cos(B0)*cos(L);
YJ=(R+H)*cos(B0)*sin(L);
ZJ=(R+H)*sin(B0);
J=[XJ,YJ,ZJ];
%计算卫星的坐标
while t1 <= 86400
a=29993707; %轨道椭圆的长半径(单位：m)
e=0; %轨道椭圆的偏心率
i=(56/180)*pi; %轨道倾角
p=0; %近地点角距
i2=0;
c=0;
t=0;
r=0;
for i0 = 0:2
if i0 == 0 %第一个轨道
d = (60/180)*pi;
elseif i0 == 1 %第二个轨道
d = pi;
else %第三个轨道
d = (300/180)*pi;
end
for i1 = 1:9
M=n0*(t1-t); %平近点角计算
E=M + (e-1/8*e^3+1/192*e^5-1/9216*e^7)*sin(M) + (1/2*e^2-1/6*e^4+1/98*e^6)*sin(2*M) + (3/8*e^3-27/128*e^5+243/5120*e^7)*sin(3*M) + (1/3*e^4 - 4/15*e^6)*sin(4*M)+ (125/384*e^5 - 3125/9216*e^7)*sin(5*M) + 27/80*e^6*sin(6*M) + 16807/46080*e^7*sin(7*M); %偏近点角的计算
Rd=[cos(d),-sin(d),0;sin(d),cos(d),0;0,0,1]; %升交点赤径的旋转矩阵
Ri=[1,0,0;0,cos(i),-sin(i);0,sin(i),cos(i)]; %轨道倾角的旋转矩阵
Rp=[cos(p),-sin(p),0;sin(p),cos(p),0;0,0,1]; %近地点角距的旋转矩阵
W=[a*(cos(E)-e),a*sqrt(1-e^2)*sin(E),0];
U = Rd*Ri*Rp*W'; %由开普勒六参数计算卫星在天球坐标系中卫星位置
RG=[cos(0),sin(0),0;-sin(0),cos(0),0;0,0,1]; %坐标旋转矩阵
i2=i2+1; %计算卫星数目
X(:,i2)= RG*U; %计算27颗卫星在地球坐标系中的位置（单位：m）
t=t+Gt;
end
end
%计算接受机上空满足条件的卫星
x1=2*XJ; %计算接收机位置为切点的地球椭球的外切面的法向量
y1=2*YJ;
z1=2*ZJ;
for j=1:27
x2=X(1,j)-XJ; %计算卫星位置和接受机位置向量方向
y2=X(2,j)-YJ;
z2=X(3,j)-ZJ;
B1=(x1*x2+y1*y2+z1*z2)/(sqrt(x1^2+y1^2+z1^2)*sqrt(x2^2+y2^2+z2^2)); %计算切面法向量与卫星和接收机向量方向的夹角
a=acos(B1);
if a <= (75/180)*pi %判断卫星是否满足要求
c=c+1;
g(c)=j; %记录卫星编号
end
end
if c < 4
error('观测卫星数目少于四颗,定位失败!');
end
C(i9)=c;
for i3=1:c %计算卫星到接收机的距离
x3=X(1,g(i3))-XJ;
y3=X(2,g(i3))-YJ;
z3=X(3,g(i3))-ZJ;
x4=x3^2;
y4=y3^2;
z4=z3^2;
S(i3)=sqrt(x4+y4+z4);
end
switch c %计算卫星的三维位置几何因子
case 4
w1=[g(1);g(2);g(3);g(4)];
A=[(X(1,g(1))-XJ)/ S(1),(X(2,g(1))-YJ)/ S(1),(X(3,g(1))-ZJ)/ S(1),-1;
(X(1,g(2))-XJ)/ S(2),(X(2,g(2))-YJ)/ S(2),(X(3,g(2))-ZJ)/ S(2),-1;
(X(1,g(3))-XJ)/ S(3),(X(2,g(3))-YJ)/ S(3),(X(3,g(3))-ZJ)/ S(3),-1;
(X(1,g(4))-XJ)/ S(4),(X(2,g(4))-YJ)/ S(4),(X(3,g(4))-ZJ)/ S(4),-1];
Q=inv(A'*A);
r=r+1;
PDOP(r)=sqrt(Q(1,1)+Q(2,2)+Q(3,3));
TDOP(r)=sqrt(Q(4,4));
GDOP(r)=sqrt(PDOP(r)^2+TDOP(r)^2);
case 5
for i4=1:c
a(i4)=i4;
end
for n=4:5
k=combinesingle(sa,n);
[a b]=size(k);
if n == 4
p1=k;
[a1 b1]=size(p1);
for i7=1:a1
for i8=1:b1
w1(i7,i8)=g(p1(i7,i8));
end
end
else
w2=[g(1);g(2);g(3);g(4);g(5)];
end
for i5=1:b
for i6=1:a
x5=X(1,g(k(i6,i5)))-XJ;
y5=X(2,g(k(i6,i5)))-YJ;
z5=X(3,g(k(i6,i5)))-ZJ;
x6=x5/ S(k(i6,i5));
y6=y5/ S(k(i6,i5));
z6=z5/ S(k(i6,i5));
B(:,i6)=[x6,y6,z6,-1]';
end
A=B';
Q=inv(A'*A);
r=r+1;
x7=Q(1,1);
y7=Q(2,2);
z7=Q(3,3);
PDOP(r)=sqrt(x7+y7+z7);
TDOP(r)=sqrt(Q(4,4));
GDOP(r)=sqrt(PDOP(r)^2+TDOP(r)^2);
end
end
case 6
for i4=1:c
sa(i4)=i4;
end
for n=4:6
k = combinesingle(sa,n);
[a b]=size(k);
if n == 4
p1=k;
[a1 b1]=size(p1);
for i7=1:a1
for i8=1:b1
w1(i7,i8)=g(p1(i7,i8));
end
end
elseif n == 5
p2=k;
[a2 b2]=size(p2);
for i7=1:a2
for i8=1:b2
w2(i7,i8)=g(p2(i7,i8));
end
end
elseif n == 6
w3=[g(1);g(2);g(3);g(4);g(5);g(6)];
end
for i5=1:b
for i6=1:a
x5=X(1,g(k(i6,i5)))-XJ;
y5=X(2,g(k(i6,i5)))-YJ;
z5=X(3,g(k(i6,i5)))-ZJ;
x6=x5/ S(k(i6,i5));
y6=y5/ S(k(i6,i5));
z6=z5/ S(k(i6,i5));
B(:,i6)=[x6,y6,z6,-1]';
end
A=B';
Q=inv(A'*A);
r=r+1;
x7=Q(1,1);
y7=Q(2,2);
z7=Q(3,3);
PDOP(r)=sqrt(x7+y7+z7);
TDOP(r)=sqrt(Q(4,4));
GDOP(r)=sqrt(PDOP(r)^2+TDOP(r)^2);
end
end
case 7
for i4=1:c
sa(i4)=i4;
end
for n=4:7
k=combinesingle(sa,n);
[a b]=size(k);
if n == 4
p1=k;
[a1 b1]=size(p1);
for i7=1:a1
for i8=1:b1
w1(i7,i8)=g(p1(i7,i8));
end
end
elseif n == 5
p2=k;
[a2 b2]=size(p2);
for i7=1:a2
for i8=1:b2
w2(i7,i8)=g(p2(i7,i8));
end
end
elseif n == 6
p3=k;
[a3 b3]=size(p3);
for i7=1:a3
for i8=1:b3
w3(i7,i8)=g(p3(i7,i8));
end
end
elseif n == 7
w4=[g(1);g(2);g(3);g(4);g(5);g(6);g(7)];
end
for i5=1:b
for i6=1:a
x5=X(1,g(k(i6,i5)))-XJ;
y5=X(2,g(k(i6,i5)))-YJ;
z5=X(3,g(k(i6,i5)))-ZJ;
x6=x5/ S(k(i6,i5));
y6=y5/ S(k(i6,i5));
z6=z5/ S(k(i6,i5));
B(:,i6)=[x6,y6,z6,-1]';
end
A=B';
Q=inv(A'*A);
r=r+1;
x7=Q(1,1);
y7=Q(2,2);
z7=Q(3,3);
PDOP(r)=sqrt(x7+y7+z7);
TDOP(r)=sqrt(Q(4,4));
GDOP(r)=sqrt(PDOP(r)^2+TDOP(r)^2);
end
end
case 8
for i4=1:c
sa(i4)=i4;
end
for n=4:8
k=combinesingle(sa,n);
[a b]=size(k);
if n == 4
p1=k;
[a1 b1]=size(p1);
for i7=1:a1
for i8=1:b1
w1(i7,i8)=g(p1(i7,i8));
end
end
elseif n == 5
p2=k;
[a2 b2]=size(p2);
for i7=1:a2
for i8=1:b2
w2(i7,i8)=g(p2(i7,i8));
end
end
elseif n == 6
p3=k;
[a3 b3]=size(p3);
for i7=1:a3
for i8=1:b3
w3(i7,i8)=g(p3(i7,i8));
end
end
elseif n == 7
p4=k;
[a4 b4]=size(p4);
for i7=1:a4
for i8=1:b4
w4(i7,i8)=g(p4(i7,i8));
end
end
elseif n == 8
w5=[g(1);g(2);g(3);g(4);g(5);g(6);g(7);g(8)];
end
for i5=1:b
for i6=1:a
x5=X(1,g(k(i6,i5)))-XJ;
y5=X(2,g(k(i6,i5)))-YJ;
z5=X(3,g(k(i6,i5)))-ZJ;
x6=x5/ S(k(i6,i5));
y6=y5/ S(k(i6,i5));
z6=z5/ S(k(i6,i5));
B(:,i6)=[x6,y6,z6,-1]';
end
A=B';
Q=inv(A'*A);
r=r+1;
x7=Q(1,1);
y7=Q(2,2);
z7=Q(3,3);
PDOP(r)=sqrt(x7+y7+z7);
TDOP(r)=sqrt(Q(4,4));
GDOP(r)=sqrt(PDOP(r)^2+TDOP(r)^2);
end
end
case 9
for i4=1:c
sa(i4)=i4;
end
for n=4:9
k=combinesingle(sa,n);
[a b]=size(k);
if n == 4
p1=k;
[a1 b1]=size(p1);
for i7=1:a1
for i8=1:b1
w1(i7,i8)=g(p1(i7,i8));
end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -