CrossTrack error calculator

[John Coffey]

Dr. Karney,

I have been working with GeographicLib for a few weeks and I have a couple
of questions regarding how to determine the cross track error from a flight
path. I use the library to make accurate point to point calculations along
a flight path, but now I need to determine the cross track error away from
the path (based on GPS coordinates received during flight). I saw
somewhere where you performed some sort of successive approximation
technique for cross track evaluation (sorry I cannot find the link) but I
think the easiest/fastest way for me to determine this value would be to
use Ed Williams’ formulary
ftp://ftp.bartol.udel.edu/anita/amir/My_thesis/Figures4Thesis/CRC_plots/Aviation%20Formulary%20V1.46.pdf
which indicates:

Cross track error:

Suppose you are proceeding on a great circle route from A to B (course
=crs_AB) and end up at D, perhaps off course. (We presume that A is ot a
pole!) You can calculate the course from A to D (crs_AD) and the distance
from A to D (dist_AD) using the formulae above. In terms of these the cross
track error, XTD, (distance off course) is given by:

XTD =asin(sin(dist_AD)*sin(crs_AD-crs_AB))

My question is:

Given 3 points A, B and D (as described in the cross track error problem
above with A and B along a geodesic), which of the GeographicLib formulae
should I use to compute the required crs_AD and crs_AB course that I could
plug into the simple formula above?

I thought these course values might be the azi1 and azi2 values returned
form Geodesic.Inverse(….) from A to D and A to D respectively, but these
returned azimuth values are very different (after conversion to radians) to
those in the Williams JavaScript worked example
https://gist.github.com/missinglink/ab7c8b9b8699622539986373e6237fd4
(which is very easy to convert and debug in C++, note I changed to a
havDistance() instead of distance() for improved accuracy). The worked
example site (Williams) indicates the calculation for course between
geoPoints a & b is defined as:

Course = std::acos((std::sin(b.lat) - std::sin(a.lat) * std::cos(d)) /
(std::sin(d) * std::cos(a.lat)))

Here you have the relevant part of my code – I would like to eliminate hand
coded course and distance calculations below favor of those in the
GeograhicLib and just use the xTrack function which as far as I am aware is
not available in GeographicLib to evaluate the cross track error.

One more thing I would also like to know how to solve is:

Given a set of flight waypoints (a flight plan), how can I calculate the
coordinates for a thick box around the flight plan as a center line. The
width of this line would effectively describe the an acceptable cross track
error (as long as I am flying within the bounds. At each waypoint, I would
need to calculate 2 normal points +/- some max cross track error away. I
have no idea how to do so and it seems like GeographicLib is perfectly
setup to calculate these values for me.

Thank you for a great library

John Coffey

// STATIC VARIABLE INITIALIZATIONS

static Geodesic gGeodesic(Constants::WGS84_a(), Constants::WGS84_f());

. . .

double

distance(const GeoPointRads& a, const GeoPointRads& b) {

return std::acos(std::sin(a.lat) * std::sin(b.lat) +

    std::cos(a.lat) * std::cos(b.lat) * std::cos(a.lon - b.lon));

}

double

havDistance(const GeoPointRads& a, const GeoPointRads& b) {

return 2 * std::asin(std::sqrt(std::pow(std::sin((a.lat - b.lat) /2), 2) +

    std::cos(a.lat)*std::cos(b.lat)*std::pow(std::sin((a.lon -

b.lon)/2), 2)));

}

double

course(const GeoPointRads& a, const GeoPointRads& b, double d) {

return std::acos((std::sin(b.lat) - std::sin(a.lat) *

    std::cos(d)) / (std::sin(d) * std::cos(a.lat)));

}

double

xTrack(const double d, const double crs1, const double crs2) {

return std::asin(std::sin(d) * std::sin(crs1 - crs2));

}

double aTrack(const double d, const double xtd) {

return std::acos(std::cos(d) / std::cos(xtd));

}

double aTrackSimple(const double d, const double xtd) {

return std::asin(std::sqrt(std::pow(std::sin(d), 2) -

    std::pow(std::sin(xtd), 2)) / std::cos(xtd));

}

/static/

double

CoPilotUtils::getCrossTrackErrorNM(

const GeoPointRads& a,

const GeoPointRads& b,

const GeoPointRads& d)

{

// solve the Geodesic inverse problem from A->D

double s12, azi1, azi2, m12, M12, M21, S12 = 0.0;

auto arcLenAD = gGeodesic.Inverse(a.lat, a.lon, d.lat, d.lon,

    s12, azi1, azi2, m12, M12, M21, S12);

const auto dist_AD = havDistance(a, d);

const auto crs_AD = course(a, d, dist_AD);

auto crsADDegrees = crs_AD / Math::degree();

const auto dist_AB = distance(a, b);

const auto distABMeters = Constants::WGS84_a() * dist_AB;

// should be 3973km

const auto crs_AB = course(a, b, dist_AB);

const auto errorMeters = Constants::WGS84_a() * xTrack

(dist_AD, crs_AD, crs_AB);

return errorMeters / GeographicLib::Constants::nauticalmile();

}

[Charles Karney]

Without trying to address all of your post in detail, let me, nevertheless offer some pointers:

• azi1 and azi2 are the angles α₁ and α₂ shown in the documentation.

• I believe the formulas for Williams are correct for a sphere. So you should be able to get everything to match up between these and the results from GeographicLib by specifying a sphere (f = 0) when you instantiate a Geodesic object.

• Assuming that D is close to the geodesic AB, then the ellipsoidal formula for cross track distance is [α₁(AD) − α₁(AB)] m₁₂(AD), Here m₁₂ is the reduced length.

Please let me know if you have any questions on this...

I defer addressing your second question (which seems awfully fiddly) until after we've cleared up the first.

[John Coffey]

Thanks for following up with me. Perhaps I misunderstood something (I'm new to geodesics) but I am getting a cross track error of 15.71 nautical miles with the above formula. With the Williams formula I correctly get 0.0021674699088523514 from the xTrack function (I presume these are radians). This value correctly matches the worked example - presumably converting this to NM is just a matter of multiplying by WGS84_a() and dividing by Constants::nauticalmile(), which ends up being 7.464NM. This value is way off (even accounting for using a non zero flattening factor). I wonder if you can spot something really obviously wrong with the following block of test code.

Thanks

John

// we travel from JFK to LAX and end up at D
const auto laxRadians = GeoPointRads{ 0.592539, 2.066470 };
const auto jfkRadians = GeoPointRads{ 0.709186, 1.287762 };
const auto lostRadians = GeoPointRads{ 0.6021386, 2.033309 };
auto xTrack = getTrackInfoNM(laxRadians, jfkRadians, lostRadians);

. . .
/static/
std::pair<double, double="">
CoPilotUtils::getTrackInfoNM(
const GeoPointRads& a,
const GeoPointRads& b,
const GeoPointRads& d)
{
std::pair<double, double=""> result;
// Solve the Geodesic inverse problem from A->D
double s12, azi1, azi2, m12, M12, M21, S12 = 0.0;
auto arcAD = gGeodesic.Inverse(a.lat, a.lon, d.lat, d.lon,
s12, azi1, azi2, m12, M12, M21, S12);
const auto azi1AD = azi1;
const auto m12AD = m12;
auto arcAB = gGeodesic.Inverse(a.lat, a.lon, b.lat, b.lon,
s12, azi1, azi2, m12, M12, M21, S12);
const auto azi1AB = azi1;
const auto ellipsiodalXtrackNM = (azi1AD - azi1AB) * m12AD /
GeographicLib::Constants::nauticalmile();
// Assuming D is close to the geodesic AB, then the
// ellipsoidal formula for cross track distance is
// [azi1(AD) - azi1(AB)] * m12(AD). (m12 = reduced length).
// https://sourceforge.net/p/geographiclib/discussion/1026621/thread/299518a3e4/</double,></double,>

const auto dist_AD = havDistance(a, d);
const auto crs_AD = course(a, d, dist_AD);
const auto dist_AB = havDistance(a, b);
const auto crs_AB = course(a, b, dist_AB);
const auto xTrackRads = xTrack(dist_AD, crs_AD, crs_AB);
const auto aTrackRads = aTrack(dist_AD, xTrackRads);
return  {
    Constants::WGS84_a() * xTrackRads / GeographicLib::Constants::nauticalmile(),
    Constants::WGS84_a() * aTrackRads / GeographicLib::Constants::nauticalmile()
};

}

[Charles Karney]

I can't reproduce your results and don't terribly want to plough through your code snippets. However, here's my working of your example:

JFK="40.6333646897648 -73.78332761732592"
LAX="33.94998389690229 -118.4000094903992"
LOST="34.50000046191607 -116.5000241459659"
echo $JFK $LAX | GeodSolve -i | cut -f1 -d' '
=> azi1AB = -86.09664540
echo $JFK $LOST | GeodSolve -i | cut -f1 -d' '
=> azi1AD = -86.32035721
echo $JFK $LOST | GeodSolve -i -f | cut -f9 -d' '
==> m12AD = 3576172.023

Finally

(azi1AD-azi1AB)*pi/180 * m12AD / 1852 => -7.54 NM

The factor of pi/180 = Math::degree() is needed to convert degrees to radians (always .needed in a formula like this) and 1/1852 converts to NM. This seems to .be close to the result you got for the spherical case?

[John Coffey]

Thank you for the above script, I figured out that my error was passing in radians to GeographicLib - I should have been passing in degrees. I also needed to convert the returned azimuth angles back to radians. Everything was in radians in the williams approach.

By the way, I am not sure where I was it before but from what I can recall I saw an approach you used to use to make a sort of successive approximation to calculating the cross track error. Was that just old code that has been replaced with this updated formula? I wish I could find the example but no luck (I do recall you posted it somewher as a C++ example).

Now I can replace the Williams temporary code with GeographicLib calls.

John

This was the missing trick before calling Geographic lib

   // convert my proprietary GeoCoordsRads to GeoCoords
    GeoCoords geoA = { a.lat / Constants::degree(), a.lon / Constants::degree() };
    GeoCoords geoB = { b.lat / Constants::degree(), b.lon / Constants::degree() };
    GeoCoords geoD = { d.lat / Constants::degree(), d.lon / Constants::degree() };

[Charles Karney]

OK, good. GeographicLib uses degrees for its interface so that directions like "due south" and positions like the "north pole" can be represented exactly.

NOTE WELL: the formula I gave you applies only for points close to the track. In the general case I recommend using the approach given in Section 8 of Algorithms for geodesics. Elsewhere on this site I give a C++ implementation. Here's an implementation in MATLAB.

function [lat, lon, s, azi] = geodintercept ...
      (lata, lona, latb, lonb, latc, lonc, ell)
%GEODINTERCEPT Approximate distance between a point and a geodesic track
%
%   [lat, lon, s, azi] = GEODINTERCEPT(lata, lona, latb, lonb, latc, lonc)
%   [lat, lon, s, azi] =
%     GEODINTERCEPT(lata, lona, latb, lonb, latc, lonc, ell)
%
%   Given a point C at (latitude, longitude) = (latc, lonc) and a geodesic
%   AB from (lata, lona) to (latb, lonb), compute the intercept point O =
%   (lat, lon), i.e., the point on AB which is closest to C.  Also return
%   the distance, s, and starting azimuth, azi, of the geodesic from C to
%   O.  The arguments must be column vectors of the same size.  All angles
%   (latitudes, longitudes, azimuth) are in degrees; distance is in meters.
%
%   The optional ell vector is of the form [a, e], where a is the
%   equatorial radius in meters, e is the eccentricity.  If ell is omitted,
%   the WGS84 ellipsoid (more precisely, the value returned by
%   defaultellipsoid) is used.
%
%   The method performs two iterations of the solution of the intercept
%   problem from "Algorithms for geodesics", Section 8, using C as the
%   initial guess for O.  Because the gnomonic projection is used, A and B
%   should be within about 9900 km of C.
%
%   If A and B are within 8000 km of each other and if C is within 200 km
%   of AB, then the results are accurate to round-off.
%
%   This function requires the installation of the GeographicLib toolbox
%     https://www.mathworks.com/matlabcentral/fileexchange/50605
%
%   See also GNOMONIC_FWD, GNOMONIC_FWD, DEFAULTELLIPSOID.

  if nargin < 7, ell = defaultellipsoid; end

  k = 0; kmax = 2;
  lat = latc; lon = lonc;               % starting guess
  while k < kmax
    % project A, B, C to gnomonic projection centered at O
    [x, y] = gnomonic_fwd(lat, lon, lata, lona, ell); a = [x, y];
    [x, y] = gnomonic_fwd(lat, lon, latb, lonb, ell); b = [x, y];
    if k == 0
      c = 0 * a;
    else
      [x, y] = gnomonic_fwd(lat, lon, latc, lonc, ell); c = [x, y];
    end
    % compute interception point (last eq. of Section 8)
    d = b - a;
    o = (sum(c .* d, 2) .* d - ...
         (a(:,1).*b(:,2) - a(:,2).*b(:,1)) .* [-d(:,2), d(:,1)]) ./ ...
        sum(d.^2, 2);
    % update O
    [lat, lon] = gnomonic_inv(lat, lon, o(:,1), o(:,2), ell);
    k = k + 1;
  end
  % compute geodesic CO
  if nargout > 2
    [s, azi] = geoddistance(latc, lonc, lat, lon, ell);
  end
end