Algorithm to calculate speed from two GPS latitude and longitude points and time difference

I found this good description of how to calculate an approximate speed over ground given two latitude and longitude coordinates and a time difference:

http://answers.yahoo.com/question/index?qid=20110325075640AADHGXI

It involves first plotting your two GPS points on a spherical model of the earth, calculating the angle between them using a dot product, calculating distance using this angle and the earth’s radius and finally dividing by the elapsed time to approximate the speed.

Thanks to bpiguy for the answer!

Now I just have to implement it….

Update:

I finally got around to writing some code to play with this distance calculation:

This function takes the latitude and longitude in signed decimal format and returns the distance in metres, I have left in the ‘r’s for clarity but if efficiency is what you’re after then they can be removed as the original post suggests…

//
//
double distance_on_geoid(double lat1, double lon1, double lat2, double lon2) {

	// Convert degrees to radians
	lat1 = lat1 * M_PI / 180.0;
	lon1 = lon1 * M_PI / 180.0;

	lat2 = lat2 * M_PI / 180.0;
	lon2 = lon2 * M_PI / 180.0;

	// radius of earth in metres
	double r = 6378100;

	// P
	double rho1 = r * cos(lat1);
	double z1 = r * sin(lat1);
	double x1 = rho1 * cos(lon1);
	double y1 = rho1 * sin(lon1);

	// Q
	double rho2 = r * cos(lat2);
	double z2 = r * sin(lat2);
	double x2 = rho2 * cos(lon2);
	double y2 = rho2 * sin(lon2);

	// Dot product
	double dot = (x1 * x2 + y1 * y2 + z1 * z2);
	double cos_theta = dot / (r * r);

	double theta = acos(cos_theta);

	// Distance in Metres
	return r * theta;
}

Now once you have the distance between the points you can estimate the average speed by dividing this distance by the time between the two position measurements like this:

  //
  //
  auto dist = distance_on_geoid(p1.latitude, p1.longitude, p2.latitude, p2.longitude);
  auto time_s = (p2.timestamp - p1.timestamp) / 1000.0;
  double speed_mps = dist / time_s;
  double speed_kph = (speed_mps * 3600.0) / 1000.0;
  //

This code assumes that p1 and p2 represent the first and second measured GPS positions and that the time-stamp recorded at each is enumerated in milliseconds, it calculates both metres per second and kilometres per hour. It is important to note that this is only an estimate of the average speed between the two points and its accuracy will depend on various factors including the distance and time elapsed between the two GPS measurements.

7 replies
  1. Ravin
    Ravin says:

    What is the type “auto”? in your declaration:
    auto time_s = (p2.timestamp – p1.timestamp) / 1000.0;

    • Kevin Godden
      Kevin Godden says:

      Hi, The ‘auto’ allows the c++ (11) compiler to infer the type of the expression from what’s on the right-hand side of the equals, so in this case, what’s on the right hand side are doubles time_s will end up being a double too. This is all done at compile time and it really just cuts down on some typing! In c# the equivalent is the var keyword. In the example you quoted I really shouldn’t have used it as I used the ‘double’ type explicitly everywhere else and so the example ends up looking a little bit inconsistent!

      http://en.cppreference.com/w/cpp/language/auto

      So in summary its is very handy but can be overused!

Comments are closed.