inital_guess.m

以最小平方法解算使用者的位置 裡面有數據提供GPS的衛星位置以及使用者位置

invent=[0 0 0 0]

estusr_store = [] ;
sv1= [ 7766188.44  -21960535.34  12522838.56  ];
sv2= [ -25922679.66  -6629461.28  31864.37 ];
sv3= [-5743774.02  -25828319.92  1692757.72 ] ;
sv4= [ -2786005.69  -15900725.80 21302003.49 ];
for i=1:5
   
Rni1=((invent(1)-sv1(1))^2+(invent(2)-sv1(2))^2+(invent(3)-sv1(3))^2)^0.5
Rni2=((invent(1)-sv2(1))^2+(invent(2)-sv2(2))^2+(invent(3)-sv2(3))^2)^0.5
Rni3=((invent(1)-sv3(1))^2+(invent(2)-sv3(2))^2+(invent(3)-sv3(3))^2)^0.5
Rni4=((invent(1)-sv4(1))^2+(invent(2)-sv4(2))^2+(invent(3)-sv4(3))^2)^0.5

Ri=[22228206.42 24096139.11 21729070.63 21259581.09 ];

H=[(invent(1)-sv1(1))/Rni1 (invent(2)-sv1(2))/Rni1 (invent(3)-sv1(3))/Rni1 1;
    (invent(1)-sv2(1))/Rni2 (invent(2)-sv2(2))/Rni2 (invent(3)-sv2(3))/Rni2 1;
    (invent(1)-sv3(1))/Rni3 (invent(2)-sv3(2))/Rni3 (invent(3)-sv3(3))/Rni3 1;
    (invent(1)-sv4(1))/Rni4 (invent(2)-sv4(2))/Rni4 (invent(3)-sv4(3))/Rni4 1]

Rni=[Rni1  Rni2 Rni3  Rni4]
ri=Ri-Rni
dR=inv(H'*H)*H'*(ri)'

invent=invent+(dR)';

estusr_store = [estusr_store ;invent ] 
end