利用MATLAB测量几何精度因子GDOP

%地面用户
A=[302536.5663,5772741.575,2695567.787];
z=[-10406472.1542,-21343723.3464,16165873.7271,-16604248.782];

%四颗卫星
B=[-7911594.8957,23011269.5235,z(1,1)];
C=[-502268.6954,15950763.8544,z(1,2)];
D=[-9283543.7249,18906293.7233,z(1,3)];
E=[14425033.2569,14961867.2032,z(1,4)];
sat=[B;C;D;E];
F=[];
J=[];
cosB=[]; %B为方位角
sinB=[];
cosH=[]; %H为仰角
sinH=[];
H=[]; %几何矩阵

for i=1:4
F(1,i)=sqrt((sat(i,1)-A(1,1))^2
+(sat(i,2)-A(1,2))^2
+(sat(i,3)-A(1,3))^2);
J(1,i)=sqrt((sat(i,1)-A(1,1))^2
+(sat(i,2)-A(1,2))^2);
sinB(1,i)=(sat(i,2)-A(1,2))/J(1,i);
cosB(1,i)=(sat(i,1)-A(1,1))/J(1,i);
end

for i=1:4
if isnan(cosB(1,i))
cosB(1,i)=0;
end
if isnan(sinB(1,i))
sinB(1,i)=0;
end
end

for i=1:4
sinH(1,i)=(z(1,i)-A(1,3))/F(1,i);
cosH(1,i)=J(1,i)/F(1,i);
H(i,1)=-1*cosH(1,i)sinB(1,i);
H(i,2)=-1cosH(1,i)cosB(1,i);
H(i,3)=-1sinH(1,i);
H(i,4)=1;
end
K=inv((H’*H)) %站心坐标系的权系矩阵
GDOP=sqrt(K(1,1)+K(2,2)+K(3,3)+K(4,4))