solveNavEqns2

 Solve the pseudo-range navigation equations by algebraically
 Given four known satellite positions, compute the receiver error by
 quadratic equation; also, solves for receiver position for verification
 INPUT: satN - satellite position; k0 - initial guess of receiver position
 and clock error; c - speed of light
 OUTPUT: x,y,z - receiver position; rt1, rt2 - roots from quadratic eqn.
 comments would be pretty much useless here because the code is a one-
 -to-one map of the algebraic expression
 see the web page for a detailed discussion of the algebra, and
 consequently, the code

0001 % Solve the pseudo-range navigation equations by algebraically
0002 % Given four known satellite positions, compute the receiver error by
0003 % quadratic equation; also, solves for receiver position for verification
0004 % INPUT: satN - satellite position; k0 - initial guess of receiver position
0005 % and clock error; c - speed of light
0006 % OUTPUT: x,y,z - receiver position; rt1, rt2 - roots from quadratic eqn.
0007 function [rt1 rt2 x y z] = solveNavEqns2( sat1, sat2, sat3, sat4, d, c )
0008     % comments would be pretty much useless here because the code is a one-
0009     % -to-one map of the algebraic expression
0010     % see the web page for a detailed discussion of the algebra, and
0011     % consequently, the code
0012     ux = [ 2*(sat2.x - sat1.x) 2*(sat3.x - sat1.x) 2*(sat4.x - sat1.x) ]';
0013     uy = [ 2*(sat2.y - sat1.y) 2*(sat3.y - sat1.y) 2*(sat4.y - sat1.y) ]';
0014     uz = [ 2*(sat2.z - sat1.z) 2*(sat3.z - sat1.z) 2*(sat4.z - sat1.z) ]';
0015     ud = [ ...
0016         2 * c^2 * (sat1.t - sat2.t) ...
0017         2 * c^2 * (sat1.t - sat3.t) ...
0018         2 * c^2 * (sat1.t - sat4.t) ]';
0019     
0020     w1= sat1.x^2 - sat2.x^2 + ...
0021         sat1.y^2 - sat2.y^2 + ...
0022         sat1.z^2 - sat2.z^2 + ...
0023         c^2 * ( sat2.t^2 - sat1.t^2 );
0024     w2= sat1.x^2 - sat3.x^2 + ...
0025         sat1.y^2 - sat3.y^2 + ...
0026         sat1.z^2 - sat3.z^2 + ...
0027         c^2 * ( sat3.t^2 - sat1.t^2 );
0028     w3= sat1.x^2 - sat4.x^2 + ...
0029         sat1.y^2 - sat4.y^2 + ...
0030         sat1.z^2 - sat4.z^2 + ...
0031         c^2 * ( sat4.t^2 - sat1.t^2 );
0032     uw = [ w1 w2 w3 ]';
0033 
0034     D = det( [ ux uy uz ] );
0035     Ddx = det( [ ud uy uz ] );
0036     Dwx = det( [ uw uy uz ] );
0037     Ddy = det( [ ux ud uz ] );
0038     Dwy = det( [ ux uw uz ] );
0039     Ddz = det( [ ux uy ud ] );
0040     Dwz = det( [ ux uy uw ] );
0041     
0042     DDdx = Ddx / D;
0043     DDdy = Ddy / D;
0044     DDdz = Ddz / D;
0045     DDwx = Dwx / D;
0046     DDwy = Dwy / D;
0047     DDwz = Dwz / D;
0048     
0049     a = DDdx^2 + DDdy^2 + DDdz^2 - c^2;
0050     b = 2*DDdx*(DDwx + sat1.x) + ...
0051         2*DDdy*(DDwy + sat1.y) + ...
0052         2*DDdz*(DDwz + sat1.z) + ...
0053         2 * c^2 * sat1.t;
0054     ce= (DDwx + sat1.x)^2 + ...
0055         (DDwy + sat1.y)^2 + ...
0056         (DDwz + sat1.z)^2 - ...
0057         c^2 * sat1.t^2;
0058     % for verification, solve for the receiver position
0059     x = (-d * Ddx - Dwx) / D;
0060     y = (-d * Ddy - Dwy) / D;
0061     z = (-d * Ddz - Dwz) / D;
0062     % find the roots of the receiver clock error
0063     rt1 = (-b + sqrt( b^2 - 4 * a * ce )) / (2 * a);
0064     rt2 = (-b - sqrt( b^2 - 4 * a * ce )) / (2 * a);
0065 end