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.

ned

An advantage of NED is you can easily obtain distance between base and rover by simply implementing the 3D distance formula:

\mathbf{distance=\sqrt{north^{2}+east^{2}+down^{2}}}

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

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s