Solve the pseudo-range navigation equations by Multivariate Newtons Method Given four known satellite positions and an intial guess of the receiver location, function will approximate receiver position and receiver clock error INPUT: sat - an array of structs, length 4; k0 - initial guess of receiver position and clock error; c - speed of light OUTPUT: x,y,z - receiver position; d - receiver clock error function handle of d/dx( F(x,y,z,d) )
0001 % Solve the pseudo-range navigation equations by Multivariate Newtons 0002 % Method 0003 % Given four known satellite positions and an intial guess of the receiver 0004 % location, function will approximate receiver position and receiver clock 0005 % error 0006 % INPUT: sat - an array of structs, length 4; k0 - initial guess of 0007 % receiver position and clock error; c - speed of light 0008 % OUTPUT: x,y,z - receiver position; d - receiver clock error 0009 function [x y z d] = solveNavEqns1( sat, k0, c ) 0010 % function handle of d/dx( F(x,y,z,d) ) 0011 rx_k = @(x, sat) 2*(x - sat.x); 0012 % function handle of d/dy( F(x,y,z,d) ) 0013 ry_k = @(y, sat) 2*(y - sat.y); 0014 % function handle of d/dz( F(x,y,z,d) ) 0015 rz_k = @(z, sat) 2*(z - sat.z); 0016 % function handle of d/dd( F(x,y,z,d) ) 0017 rd_k = @(d, sat) 2*c^2*(sat.t - d); 0018 % function handle for pseudo-range navigation function, F(x,y,z,d) 0019 F_k = @(d, sat, dist) dist - (c * (sat.t - d))^2; 0020 % function handle for computing euclidean distance of a satellite to the 0021 % receiver position 0022 euclidean_dist = @(x_k, sat) ... 0023 (x_k(1) - sat.x)^2 + ... 0024 (x_k(2) - sat.y)^2 + ... 0025 (x_k(3) - sat.z)^2; 0026 % k is our iterative solution of Ax = b 0027 k = k0; 0028 % NOTE: k(1) - x; k(2) - y; k(3) - z; k(4) - d 0029 % we choose an arbitrary number of times to iterate 0030 % (i.e. there is no mechanism to say when the solution is good enough) 0031 for i = 1:20, 0032 % create an array of distances to the receiver 0033 ed = [ ... 0034 euclidean_dist(k, sat(1)) ... 0035 euclidean_dist(k, sat(2)) ... 0036 euclidean_dist(k, sat(3)) ... 0037 euclidean_dist(k, sat(4)) ]; 0038 % create the Jacobian matrix from our functions of partial derivatives 0039 DF= [ ... 0040 rx_k(k(1), sat(1)) ry_k(k(2), sat(1)) rz_k(k(3), sat(1)) rd_k(k(4), sat(1)); ... 0041 rx_k(k(1), sat(2)) ry_k(k(2), sat(2)) rz_k(k(3), sat(2)) rd_k(k(4), sat(2)); ... 0042 rx_k(k(1), sat(3)) ry_k(k(2), sat(3)) rz_k(k(3), sat(3)) rd_k(k(4), sat(3)); ... 0043 rx_k(k(1), sat(4)) ry_k(k(2), sat(4)) rz_k(k(3), sat(4)) rd_k(k(4), sat(4)) ]; 0044 % create the vector of solutions; this is the 'b' in Ax = b 0045 F = [ ... 0046 F_k(k(4), sat(1), ed(1)); ... 0047 F_k(k(4), sat(2), ed(2)); ... 0048 F_k(k(4), sat(3), ed(3)); ... 0049 F_k(k(4), sat(4), ed(4)) ]; 0050 % solve the system of equations, DF * s + F = 0 0051 s = DF \ -F; 0052 % this is the 'Newton' part; add our solution to the sys. of eqns. to 0053 % our current guess, and get our next best guess 0054 k = k + s; 0055 % RINSE, REPEAT ... 0056 end % END for i 0057 % return the approximated results 0058 x = k(1); 0059 y = k(2); 0060 z = k(3); 0061 d = k(4); 0062 end % END solveNavEqns1