?? 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 + -