function z_out = egm96_correct(lon_in,lat_in,z_in);
%
%
%   Function to correct gps altitudes (where wgs84 ellipsod has been used)
%   using egm96 15' geoid dataset
%
%   Usage:
%       z_corrected = egm96_correct(lon,lat,z);
%
%   lon, lat and z must be vectors of identical length.
%
%   Requires:
%       egm96_na.mat
%
%   Author: Dave Sproson
%   Date:   26/06/2007

if(length(lon_in) ~= length(lat_in) | length(lon_in) ~= length(z_in) | length(lat_in) ~= length(z_in))
    error('Inputs must be of the same size');
end

if(max(lon_in) > 0)
    lon_in = lon_in - 360;
end

load egm96_na.mat

lon_index = zeros(size(z_in));
lat_index = zeros(size(z_in));
z_out = zeros(size(z_in));

for(i = 1:length(lon_in))
    lon_index(i) = min(nearest(lon,lon_in(i)));
    lat_index(i) = min(nearest(lat,lat_in(i)));
    z_out(i) =  z_in(i) - gi(lat_index(i),lon_index(i));
end

end %of function

%===================

function index = nearest(array,value)

array = abs(array - value);

index = find(array == min(min(array)));

end % of function