October 2000, Issue 123

Navigating With GPS

NAVIGATION FORMULAS

Now that the units are all in line, the latitude and longitude data points can be run through the great circle algorithms, yielding correct results. The distance calculation is performed first because the distance is a factor in the bearing calculation. To compute the great circle distance between two pairs of latitudes and longitudes, use:

d = acos(sin(Lat1) × sin(Lat2) + cos(Lat1) × cos(Lat2) × cos(Lon1 – Lon2))

This formula accurately yields the distance between two points on the globe. Remember that the units are in radians, so to convert from radians to nautical miles, use the formula NM = radians × 3437.7387. Then you can convert to land miles or kilometers. Some languages and programming environments, such as Visual Basic, do not support a direct acos( ) function. Instead, you can use an atan( ) function coupled with the relation acos(x) = atan(sqrt(1 – x2) / x). For calculating distance, I use the sequence of temporary variables as follows:

t1 = sin(Lat1) × sin(Lat2);

t2 = cos(Lat1) × cos(Lat2);

t3 = cos(Lon1 – Lon2);

t4 = t2 × t3;

t5 = t1 + t4;

This sequence works well. While taking a few more steps than one monolithic formula, intermediate variables are exposed, allowing you to debug the distance algorithm as it progresses. And, this sequence works with all programming languages. To prove it, t1 through t5 can be consolidated, but sometimes it’s good to see what’s going on in a mathematical algorithm at different steps.

Why not use the Pythagorean Theorem (remember, ) to compute the distance between two points? For navigation, x would be the absolute value of the difference of the latitudes, and y would be the absolute value of the difference of the longitudes. This is true for the proximity of 300² but rapidly deteriorates beyond that.

Hence, the Pythagorean Theorem is useful only in two-dimensional space. You’re navigating in three-dimensional space, so for short distances, the theorem appears to work, but it fails for long distances. So, although it’s an easier formula touse, you can’t use it for any significant distances.

Now that distance is calculated, the next thing to do is calculate the bearing from one point to another. Bearing tells you which way to go. It is defined as the angle measured horizontally from north to the current direction of travel. North can be true north or magnetic north. Again, the great circle distance (d) between two points must be previously calculated. The classic bearing formula is:

where d equals the great circle distance. The result (c) must be qualified by testing whether or not sin(Lon2 – Lon1) is negative. If negative, the true course is determined by 360° – c. You will end up at the destination, but you’ll be taking the long way around the globe. Again, I like to break down the algorithm into discrete steps using temporary variables (see Listing 3).

if (sin (Lon2 - Lon1) < 0.0)
{
  t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);
  t2 = cos(Lat1) * sin(rad_dist);
  t3 = t1 / t2;
  t4 = atan(-t3 / sqrt(-t3 * t3 + 1)) + 2 * atan(1);
   rad_bearing = t4;
}
else
{
	t1 = sin(Lat2) - sin(Lat1) * cos(rad_dist);
	t2 = cos(Lat1) * sin(rad_dist);
	t3 = t1 / t2;
	t4 = -t3 * t3 + 1;
	t5 = 2 * 3.14 - (atan(-t3 / sqrt(-t3 * t3 + 1)) + 2 * atan(1);
  rad_bearing = t5;
}

To create a direction pointer, subtract the current GPS heading of the RMC message from the calculated bearing. Add 360° if the result is negative, creating an angle value that points from one waypoint to another.

Many different sources are available to determine waypoints. Inexpensive PC-based mapping programs provide methods of converting map points to latitude and longitude. Converting from an address to a latitude and longitude value is called geocoding. Converting from latitude and longitude values to an address is called reverse geocoding. Using the algorithms provided here and a GPS receiver, you can create your own waypoint-capturing program. Simply provide some code that will save the incoming RMC message when you pass over a location. The saved message contains the latitude and longitude of the point passed over. You can use a collection of these values to create accurate maneuver lists for roads, trails, rivers, and lakes.

Figure 4 illustrates the main components of a simple navigation system. Data is input from a GPS receiver serially to an input buffer at 4800 bps, 8 data bits, 1 stop bit, and no parity. The data is input periodically at 1-s intervals. During the time between the input data bursts (typically 200 to 800 ms), the input buffer data is transferred to a parser buffer. The data in the parser buffer is used as input to the NMEA parser that separates the data into different components—latitude, speed, and so forth.

Figure 4—The simple navigation system architecture is pictured here.

Data from the NMEA parser is made available to the display and navigation engines. The navigation engine computes the distance, bearing, and direction pointer, then gives the information to the display engine for rendering and display.

The companion program to this article, navcalc.c, takes latitude and longitude pairs from the command line and computes the great circle distance from the first latitude/longitude pair to the last pair. The source and destination latitude and longitude values supplied in the program text are for southeastern Michigan. To create a dynamic navigation program based on this code, use the latitude and longitude received and parsed from a GPS receiver as the source coordinates, and continuously calculate the distance and bearing to the destination coordinates. Try different latitude/longitude source and destination pairs in your city and compare how well the output values match reality.