Distance

[RealMadridFK]

Hi.
I have a question about the calculation of the distance. You are using the Harversine function, but it is wrong.
And I calculated the distance using the Mercator transformation, and got a completely different result.

load('D:\ins-gnss\NaveGo-1.3\examples\real-data\ekinox_gnss.mat');
ekinox.time_device = ekinox_gnss.t- ekinox_gnss.t(1);

R_equator = 6.3781 * 10^6;      %радиус Земелюшки

lat0Rad = ekinox_gnss.lon(1,1);
lon0Rad = ekinox_gnss.lon(1,1);

latRad  = ekinox_gnss.lat(:,1);
lonRad  = ekinox_gnss.lon(:,1);

scale = cos(lat0Rad) ;

R = R_equator * (0.99832407 + 0.00167644 * cos(2.0*lat0Rad) - 0.00000352*cos(4.0*lat0Rad) );

x = scale * R * lonRad;
y = scale * R * log(tan(pi/4.0 + latRad/2.0)) ;

x0 = scale * R * lon0Rad;
y0 = scale * R * log(tan(pi/4.0 + lat0Rad/2.0)) ;
x = (x - x0);
y = (y - y0);
z = (ekinox_gnss.h - ekinox_gnss.h(1));

%cartesian_coord
coord(:,1) = x(:,1);
coord(:,2) = y(:,1);
coord(:,3) = z(:,1);
%delta distance between points
delta_x = [0;diff(coord(:,1))];
delta_y = [0;diff(coord(:,2))];
delta_z = [0;diff(coord(:,3))];
delta_r = sqrt(delta_x.^2+delta_y.^2+delta_z.^2);
distance_cartesian = sum(delta_r);

[rodralez]

Hi,

I'm not familiar with the Mercator transformation, but I have compared the NaveGo Haversine distance function to the MATLAB Haversine distance function. The error between both is around 6%.

clc
clear 
close all

addpath ./examples/real-data/
addpath ./performance_analysis/

load ekinox_gnss.mat

%% NAVEGO DISTANCE

navego_distance = gnss_distance (ekinox_gnss.lat, ekinox_gnss.lon);

%% MATLAB DISTANCE

M = length (ekinox_gnss.lat);

% referenceEllipsoid('WGS 84')
%                 Code: 7030
%                  Name: 'WGS 84'
%            LengthUnit: 'meter'
%         SemimajorAxis: 6378137
%         SemiminorAxis: 6356752.31424518
%     InverseFlattening: 298.257223563
%          Eccentricity: 0.0818191908426215
         
R = sqrt(6378137 * 6356752.31424518);  

delta_pos = zeros(M,1);

for i=1:M-1
    
    % [ARCLEN, AZ] = distance(LAT1,LON1,LAT2,LON2)
    [ARCLEN, AZ] = distance( ekinox_gnss.lat(i), ekinox_gnss.lon(i) , ...
                             ekinox_gnss.lat(i+1), ekinox_gnss.lon(i+1), 'radians');
delta_pos(i) = R .* ARCLEN;

end

matlab_distance = sum(delta_pos);

%% ERROR ANALYSIS

error_m = matlab_distance - navego_distance;
error = error_m / matlab_distance * 100;

fprintf("Errors between MATLAB and NaveGo haversine distances are %.2f m or %.2f%% \n", error_m, error)

Regards.

[RealMadridFK]

The distance of the Haversine is calculated assuming that the Earth is in the shape of a sphere, but the Earth is in the shape of an ellipsoid.
Check out the code I posted above. The closer the origin (lat0, lon0) is to the trajectory, the smaller the calculation error.

[rodralez]

It would be great if you can provide a function for obtaining the distance with the Mercator method. It should have the same function prototype as gnss_distance.m. I can add your function to NaveGo.

[RealMadridFK]

Было бы здорово, если бы вы могли предоставить функцию для получения расстояния с помощью метода Меркатора. Он должен иметь тот же прототип функции, что и gnss_distance.m. Я могу добавить вашу функцию в NaveGo.

function [distance, delta_pos] = gnss_distance_Mercator (lat, lon, lat0, lon0, alt)
% gnss_distance_Mercator: provides the distance distance traveled
%
% INPUT
% lat, Mx1 latitudes (radians).
% lon, Mx1 longitudes (radians).
% lat0, 1x1 latitude (radians).
% lon0, 1x1 longitude (radians).
% alt, Mx1 altitude(meters).
%
% OUTPUT
% distance, total distance (meters).
% delta_pos, Mx1 incremental distances between near points (meters).
%
%
%
R_equator = 6378137.000; % Earth's radius in meters
%
scale = cos(lat0) ;
%
R = R_equator * (0.99832407 + 0.00167644 * cos(2.0lat0) - 0.00000352cos(4.0*lat0) );
%
x = scale * R * lon;
y = scale * R * log(tan(pi/4.0 + lat/2.0)) ;
%
x0 = scale * R * lon0;
y0 = scale * R * log(tan(pi/4.0 + lat0/2.0)) ;
x = (x - x0);
y = (y - y0);
z = (alt - alt(1));
%
%Cartesian coords
coord(:,1) = x(:,1);
coord(:,2) = y(:,1);
coord(:,3) = z(:,1);
%
%Сalculating distances between coordinate points
delta_x = [0;diff(coord(:,1))];
delta_y = [0;diff(coord(:,2))];
delta_z = [0;diff(coord(:,3))];
delta_pos = sqrt(delta_x.^2+delta_y.^2+delta_z.^2);
%
%Calculating full distance
distance = sum(delta_pos);
end

[rodralez]

Hi, thank you very much for your contribution. I have added this function to the NaveGo repository under the "performance_analysis" directory.

Regards.

[RealMadridFK]

The question is about velocities in the NED coordinate system. Do your sensor issue them automatically? Or do you get them by calculation methods? If by calculations, then tell me how, because my GNSS does not give out the speed of NEDs.

[rodralez]

The GNSS sensor should provide the NED velocities. This is the best scenario. If not, I recommend to use the RTKLIB library to estimate NED velocities.

[BrunoOtavio98]

Or you could use the GPS heading and decompose your ground velocity. Thats works well for me. You can acquire north and east velocity in this way.

Vn = V * cos(heading)
Ve = V * sin(heading)

[RealMadridFK]

Sorry. I found the problem in the code (copy-paste).

Distances now have a much smaller delta.