Convert Latitude, Longitude, Altitude to North, East, Down and Find Distance Between Base and Rover

In order to achieve accurate and precise positioning on a moving platform like a drone, an RTK (real time kinematics) GNSS is the best option. The u-blox C94M8P achieves RTK positioning by using two GNSS receivers: the rover mounted to the drone and the base stationary on land.
For many of the imaging tasks we wanted to perform in my undergrad research group, we needed to convert the typical latitude, longitude, altitude (LLA) coordinate system to a north, east, down (NED) coordinate system.
For more information about what NED is and why it’s often used in aerospace, this Wikipedia article is a good start.
The short version is NED measures the north, east, and down directions from an origin of your choosing. It’s a convenient system to use if you want to know where a moving platform is relative to some reference point, as is the case with our drone’s rover-base positioning system. The drone’s initial position can also be used as the origin, but in our application with two GPS receivers, the base is a natural choice.
An advantage of NED is you can easily obtain distance between base and rover by simply implementing the 3D distance formula:
While MATLAB has a built-in function, geodetic2ned, which converts geodetic (LLA) coordinates to NED, it’s only available in their mapping toolbox, which requires an additional purchase. So, I did some research and created my own function, which I named LLAtoNED. I share it freely in the name of open source.
function [time, north, east, down, dist] = LLAtoNED(baseFile, roverFile) %{ File: LLAtoNED.m Author: Brandi Downs Last Updated: 12.11.2018 LLAtoNED.m reads in a file specified by the user, looks for the parameters given below, and stores them to a cell array for further processing. This script currently stores data only from $GNGGA sentences. This script assumes the base GPS unit is stationary. Parameters: TIME LAT LONG FIX HDOP ALT SEP %} %% Open base and rover txt files, read in NMEA sentence data, store data in new arrays roverData = fopen(roverFile,'r'); % open rover file for reading row = 1; % begin with row 1 % initialize a matrix for each parameter you want to store gngga = []; % stores all data from GNGGA sentences time = []; % time in UTC lat = []; % latitude long = []; % longitude fix = []; % fix type or quality indicator (see note above) sats = []; % satellites used hdop = []; % horizontal dilution of precision alt = []; % altitude sep = []; % geoidal separation format long; while ~feof(roverData) % while not end of file line = fgetl(roverData); % get next line line = regexp(line,',','split'); % split line into cell array according to commas if ~any(strcmpi('$GNGGA',line)) % if line doesn't contain 'GNGGA', continue to next iteration of loop continue end for i = 2:length(line) gngga(row,i-1) = str2double(line{i}); % store all data to gngga matrix % store time parameter to column vector time(row,1) = str2double(line{2}); % convert degree, minute, decimal minute to decimal degrees lat(row,1) = str2double(line{3})/100; % move decimal place left 2 latdeg = floor(lat(row,1)); % extract integer part latdec = lat(row,1) - latdeg; % extract decimal part latdec = latdec/60; % convert decimal minutes to decimal degrees latdec = latdec*100; % move decimal place right 2 lat(row,1) = latdeg + latdec; % add integer and decimal to complete calculation % do same for longitude long(row,1) = str2double(line{5})/100; % move decimal place legt 2 longdeg = floor(long(row,1)); % extract integer part longdec = long(row,1) - longdeg; % extract decimal part longdec = longdec/60; % convert decimal minutes to decimal degrees longdec = longdec*100; % move decimal place right 2 long(row,1) = longdeg + longdec; % add integer and decimal to complete calculation % continue to store remaining parameters fix(row,1) = str2double(line{7}); sats(row,1) = str2double(line{8}); hdop(row,1) = str2double(line{9}); alt(row,1) = str2double(line{10}); sep(row,1) = str2double(line{12}); end row = row + 1; % next row end fclose(roverData); % Do same for base data % Only keep lat, long, alt baseData = fopen(baseFile,'r'); % open base file for reading row = 1; latBase = []; % latitude longBase = []; % longitude altBase = []; % altitude while ~feof(baseData) % while not end of file line = fgetl(baseData); % get next line line = regexp(line,',','split'); % split line into cell array according to commas if ~any(strcmpi('$GNGGA',line)) % if line doesn't contain 'GNGGA', continue to next iteration of loop continue end for i = 2:length(line) % get base latitude and convert to decimal degrees latBase(row,1) = str2double(line{3})/100; latBasedeg = floor(latBase(row,1)); latBasedec = latBase(row,1) - latBasedeg; latBasedec = latBasedec/60; latBasedec = latBasedec*100; latBase(row,1) = latBasedeg + latBasedec; % get base longitude and convert to decimal degrees longBase(row,1) = str2double(line{5})/100; longBasedeg = floor(longBase(row,1)); longBasedec = longBase(row,1) - longBasedeg; longBasedec = longBasedec/60; longBasedec = longBasedec*100; longBase(row,1) = longBasedeg + longBasedec; % get base altitude altBase(row,1) = str2double(line{10}); end row = row + 1; % next row end fclose(baseData); %% Convert from lat, long, altitude (LLA) to north, east, down (NED) coordinate system % lat0, long0, alt0 will form the origin of the NED coordinate system lat0 = median(latBase); long0 = median(longBase); alt0 = median(altBase); % geodetic frame parameters Rea = 6378137; % semi-major axis (m) f = 1/298.257223563; % flattening factor Reb = Rea*(1-f); % semi-minor axis ecc = sqrt(Rea^2 - Reb^2)/Rea; % first eccentricity Ne = zeros(1,numel(lat)); % prime vertical radius of curvature for k = 1:numel(lat) Ne(k) = Rea./sqrt(1-(ecc^2)*(sind(lat(k))).^2); end Neref = Rea./sqrt(1-(ecc^2)*(sind(lat0)).^2); % prime vertical radius of curvature for reference point %% Geodetic to Earth-Centered Earth-Fixed (ECEF) -- Intermediate step % preallocate arrays for speed xe = zeros(1,numel(lat)); ye = zeros(1,numel(lat)); ze = zeros(1,numel(lat)); % convert rover lat, long, alt (LLA) to ECEF for k = 1:numel(lat) xe(k) = (Ne(k) + alt(k))*cosd(lat(k))*cosd(long(k)); ye(k) = (Ne(k) + alt(k))*cosd(lat(k))*sind(long(k)); ze(k) = (Ne(k)*(1-ecc^2)+alt(k))*sind(lat(k)); end % convert reference position (origin in NED frame) from LLA to ECEF xeref = (Neref + alt0)*cosd(lat0)*cosd(long0); yeref = (Neref + alt0)*cosd(lat0)*sind(long0); zeref = (Neref*(1-ecc^2)+alt0)*sind(lat0); Pe = [xe;ye;ze]; Peref = [xeref;yeref;zeref]; %% ECEF to NED % Rne is the rotation matrix from ECEF frame to local NED frame Rne = [-sind(lat0)*cosd(long0) -sind(lat0)*sind(long0) cosd(lat0); ... -sind(long0) cosd(long0) 0; ... -cosd(lat0)*cosd(long0) -cosd(lat0)*sind(long0) -sind(lat0)]; % convert ECEF to local NED frame Pn = zeros(5,numel(lat)); for k = 1:numel(lat) Pn(1,k) = time(k); Pn(2:4,k) = Rne*(Pe(:,k) - Peref); Pn(5,k) = sqrt((Pn(2,k))^2 + (Pn(3,k))^2 + (Pn(4,k))^2); end % use num2str(x,'%4.10f') to display full numbers time = Pn(1,:); north = Pn(2,:); east = Pn(3,:); down = Pn(4,:); dist = Pn(5,:);
LLAtoNED.m is a MATLAB function and hence must be called as a function command. By typing:
[time,north,east,down,dist] = LLAtoNED(‘nameOfBaseFile.txt’,’nameOfRoverFile.txt’)
in the command window, LLAtoNED will read in text files for the base and rover position data, do the coordinate conversions from LLA to NED, and output arrays for time, north, east, down, and distance between base and rover at each observation data point. For the ublox C94M8P in wireless RTK mode, the observation rate — or how many times the base and rover record their position data — is a maximum of 3 Hz max (3 times per second).
Enjoy and please leave comments if you have any questions or feedback!
Note: For the coordinate conversion matrices, I used this very helpful reference document: Coordinate Systems and Transformations