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
