//
// Le GPS: exemple de localisation d'un tracteur (2 méthodes)
//
S1=[-11716.227778, -10118.754628 , 21741.083973];   // position des satellites (en km, repere terrestre))
S2=[-12082.643974, -20428.242179 , 11741.374154];
S3=[14373.286650, -10448.439349, 19596.404858];
S4=[10278.432244,-21116.508618,-12689.101970];
d=[22163.882029,21492.579823,21492.492771,25284.588982];  // pseudo-distance satellites au tracteur
clum=299792.458;  // vitesse de la lumière (km/s)
//
// première méthode: quelques itérations de Newton
//
P0=[0,0,6400,0];   // position initiale (sur la surface de la terre)
for i=1:5
  Lig1=[(P0(1)-S1(1))/(norm(P0(1:3)-S1(1:3))),(P0(2)-S1(2))/(norm(P0(1:3)-S1(1:3))),(P0(3)-S1(3))/(norm(P0(1:3)-S1(1:3))),1];
  Lig2=[(P0(1)-S2(1))/(norm(P0(1:3)-S2(1:3))),(P0(2)-S2(2))/(norm(P0(1:3)-S2(1:3))),(P0(3)-S2(3))/(norm(P0(1:3)-S2(1:3))),1];
  Lig3=[(P0(1)-S3(1))/(norm(P0(1:3)-S3(1:3))),(P0(2)-S3(2))/(norm(P0(1:3)-S3(1:3))),(P0(3)-S3(3))/(norm(P0(1:3)-S3(1:3))),1];
  Lig4=[(P0(1)-S4(1))/(norm(P0(1:3)-S4(1:3))),(P0(2)-S4(2))/(norm(P0(1:3)-S4(1:3))),(P0(3)-S4(3))/(norm(P0(1:3)-S4(1:3))),1];
  H=[Lig1;Lig2;Lig3;Lig4];   // matrice jaobienne 
  DP=linsolve(H,[norm(P0(1:3)-S1(1:3))+P0(4)-d(1);norm(P0(1:3)-S2(1:3))+P0(4)-d(2);norm(P0(1:3)-S3(1:3))+P0(4)-d(3);norm(P0(1:3)-S4(1:3))+P0(4)-d(4)]);
  P0=P0+DP';             // iteration Newton   
end
disp('--------methode de Newton---------')
disp('position calculée (en km, repere terrestre) ');disp(P0(1:3))
disp('decalage temps (ms)  ');disp(1E3*P0(4)/clum)
//
// deuxième méthode: calcul explicite
//
S1bis=[S1,d(1)];S2bis=[S2,d(2)];S3bis=[S3,d(3)];S4bis=[S4,d(4)];
B=[S1bis;S2bis;S3bis;S4bis];
L=diag([1,1,1,-1])
a1=S1bis*L*S1bis'
a2=S2bis*L*S2bis'
a3=S3bis*L*S3bis'
a4=S4bis*L*S4bis'
alpha=[a1;a2;a3;a4];
e=ones(4,1)
//
a=(inv(B)*e)'*L*(inv(B)*e)
b=2*((inv(B)*alpha)'*L*(inv(B)*e)-2)
c=(inv(B)*alpha)'*L*(inv(B)*alpha)
//
xi2=(-b-sqrt(b^2-4*a*c))/(2*a)  // calcul de xi^2 avec equation 2nd degré
//
P=1/2*inv(B*L)*(alpha+xi2*e)    // calcul de xi
//
disp('--------methode explicite---------')
disp('position calculée (en km, repere terrestre) ');disp(P(1:3)')
disp('decalage temps (ms)  ');disp(1E3*P(4)/clum)
//
disp('Vérification: distance du point au centre de la terre (km):');disp(norm(P(1:3)));
disp('------------------------------')
disp('------------------------------')
//
