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

To estimate average speed of travel between two close GPS points.

A relatively simple method involves treating the globe as a sphere with a radius of 6378100 metres and calculating positions on this sphere for the two locations, of course the earth isn’t a sphere, but as we are aiming to calculate the speed of travel between 2 quite close points we should be OK.

First we plot the two GPS points on a spherical model of the earth, then calculate the angle between them using a dot product, then calculate the ‘Great Circle’ distance using this angle and the earth’s radius and finally we divide by the elapsed time to approximate the speed.

Here is some code to demonstrate the 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.

//
//
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, something like this:

//
//
auto dist = distance_on_geoid(p1.latitude, p1.longitude, p2.latitude, p2.longitude);
// timestamp is in milliseconds
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.