www.pudn.com > gps.zip > task_1.m, change:2015-11-18,size:1028b


%定位解算 
 
%概略坐标 
x0=-730000; 
y0=-5440000; 
z0=3230000; 
 
%卫星位置 
x1=15524471.175; 
y1=-16649826.222; 
z1=13512272.387; 
 
x2=-2304058.534; 
y2=-23287906.465; 
z2=11917038.105; 
 
x3=16680243.357; 
y3=-3069625.561; 
z3=20378551.047; 
 
x4=-14799931.395; 
y4=-21425358.240; 
z4=6069947.224; 
 
%伪距观测值 
p1=22274133.863; 
p2=19920232.728; 
p3=24566287.852; 
p4=21491226.260; 
 
%距离近似值 
r1=((x1-x0)^2+(y1-y0)^2+(z1-z0)^2)^0.5; 
r2=((x2-x0)^2+(y2-y0)^2+(z2-z0)^2)^0.5; 
r3=((x3-x0)^2+(y3-y0)^2+(z3-z0)^2)^0.5; 
r4=((x4-x0)^2+(y4-y0)^2+(z4-z0)^2)^0.5; 
 
%自由项 
L1=r1-p1; 
L2=r2-p2; 
L3=r3-p3; 
L4=r4-p4; 
 
%系数项 
l1=(x1-x0)/r1; 
l2=(x2-x0)/r2; 
l3=(x3-x0)/r3; 
l4=(x4-x0)/r4; 
 
m1=(y1-y0)/r1; 
m2=(y2-y0)/r2; 
m3=(y3-y0)/r3; 
m4=(y4-y0)/r4; 
 
n1=(z1-z0)/r1; 
n2=(z2-z0)/r2; 
n3=(z3-z0)/r3; 
n4=(z4-z0)/r4; 
 
%系数矩阵 
A=[l1,m1,n1,-1; 
   l2,m2,n2,-1; 
   l3,m3,n3,-1; 
   l4,m4,n4,-1]; 
 
L=[L1;L2;L3;L4]; 
 
%求解定位误差 
delta_X=inv(A'*A)*A'*L 
 
X=[x0+delta_X(1);y0+delta_X(2);z0+delta_X(3)]