%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Radiolocation Scavanger Hunt											%
% Assignment Two - 'Late For Dinner'									%
% Georgia Institute of Technology										%
% ECE 6390, Fall 2005													%
% Solution by Team Space Invaders										%
%	Melissa Amoros														%
%	Dean Lewis															%
%	Joel Prothro														%
%	Tushar Thrivikraman													%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Purpose:																%
%	Calculates the location of a GPS receiver on Earth based on four	%
%	GPS psuedo-ranges and an initial position guess.					%
% Input:																%
%	Psuedoranges to four GPS satellites									%
%	Location of satellite subpoint for the same four GPS satellites		%
%		- in latitude/longitude units									%
%	Initial position estimate in latitude/longitude units				%
% Output:																%
%	Position of the GPS receiver in latitude/longitude units			%
% Requires:																%
%	converttocart() function											%
%	converttolatlon() function											%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize MatLab
clear
clc
close all
format compact

% Define Constants
c = 299860.00; % Speed of Light (km/s)
re = 6380.00;  % Radius of Earth (km)
rs = 20200+re; % Radius of GPS Satellite Orbit (km)

% Initial Position Guess - Hartsfield-Jackson Atlanta International Airport
Lat_ATL = 33.65;
Lon_ATL = -84.42;

% GPS Satellite Data
% Satellite 1
Lon_Sat1 = -62.3256; % Longitude of satellite subpoint (degrees)
Lat_Sat1 = 43.2345;  % Latitude of satellite subpoint (degrees)
PR1 = 0*c;			 % Psuedo-range to satellite (km)
% Satellite 2
Lon_Sat2 = -85.2234;
Lat_Sat2 = 35.5521;
PR2 = -0.001587009*c;
% Satellite 3
Lon_Sat3 = -90.9760;
Lat_Sat3 = 29.0510;
PR3 = -0.001373255*c;
% Satellite 4
Lon_Sat4 = -95.0234;
Lat_Sat4 = 40.7777;
PR4 = -0.001092284*c;

% Calculating XYZ Locations in Earth-centric Cartesian coordinates
[Xatl Yatl Zatl] = converttocart(Lat_ATL, Lon_ATL, re); % Atlanta airport
[X1 Y1 Z1] = converttocart(Lat_Sat1, Lon_Sat1, rs); % Satellite 1
[X2 Y2 Z2] = converttocart(Lat_Sat2, Lon_Sat2, rs); % Satellite 2
[X3 Y3 Z3] = converttocart(Lat_Sat3, Lon_Sat3, rs); % Satellite 3
[X4 Y4 Z4] = converttocart(Lat_Sat4, Lon_Sat4, rs); % Satellite 4

% Reorganize data into a MatLab-friendly form
Xs = [X1;X2;X3;X4];
Ys = [Y1;Y2;Y3;Y4];
Zs = [Z1;Z2;Z3;Z4];

% Set first position estimate to Atlanta airport
Xg = Xatl;
Yg = Yatl;
Zg = Zatl;
Y = [PR1; PR2; PR3; PR4];

% Iterate through the ranging algorithm until the solution converges
% 50 steps was found to be sufficient for convergence
% Linearized ranging algorithm taken from [INSERT REFERENCE NUMBER]
for i=0:50
    % Calculate estimated psuedoranges from estimated position (use A.1)
    PRg = sqrt((Xg-Xs).^2+(Yg-Ys).^2+(Zg-Zs).^2);

    % Calculate transformation matrix H (defined in A.11)
    R = sqrt((Xg-Xs).^2+(Yg-Ys).^2+(Zg-Zs).^2);
    H = [(Xg - Xs)./R (Yg - Ys)./R (Zg - Zs)./R [1;1;1;1]];

    % Calculate error in estimated psuedoranges
    dY = Y - PRg;

	% Calculate necessary change in position estimation (A.12)
    dB = ((transpose(H)*H)^(-1))*transpose(H)*dY;

	% Update the position estimate
    Xg = Xg+dB(1);
    Yg = Yg+dB(2);
    Zg = Zg+dB(3);
end

% Calculate the latitude/longitude location of the GPS receiver based on the XYZ position
[Lat_rec Lon_rec] = converttolatlon(Xg, Yg, Zg)

Lat_rec =
   33.7727
Lon_rec =
  -84.3799

Published with MATLAB® 7.0.4