Interpolate between 2 GPS locations based on walking speed

Question

Problem:

---

Given two locations:

L₁ = (latitude₁, longitude₁, timestamp₁), L₂ = (latitude₂, longitude₂, timestamp₂),

and a configurable, but constant, movement speed:

v = 1.39 meters per second (for instance).

How can we interpolate between these two locations to estimate a users location as he travels from L₁ to L₂?

---

I have been searching for solutions to this problem and so far I have found, that for small distances (away from the poles) linear interpolation can be used. So, I looked up linear interpolation on Wikipedia and found this:

// Imprecise method which does not guarantee v = v1 when t = 1,
// due to floating-point arithmetic error.
float lerp(float v0, float v1, float t) {
    return v0 + t*(v1-v0);
}

So I am thinking of using this lerp function to interpolate latitude and longitude between L₁ and L₂. That was the easy part. How do I calculate t? I'm guessing I have to calculate some time deltas, but how do I factor in the movement speed?

---

Edit:

I'm testing various methods for collecting GPS-locations. For this purpose, I am recording waypoint-locations throughout a walk. I need to interpolate between these waypoints, using the movement speed, to estimate my position along the walk. Then I can compare my results to the estimates to see how well they are faring.

Example:

Answer 1

Take a close look at Calculate distance, bearing and more between Latitude/Longitude points

It contains several formulas and JavaScript examples that might help you. I know that it's NOT Java, but it should be simple enough to port the code over. Especially given the detailed description of the formula.

EDIT:

While it seems OK to use linear interpolation for shorter distances, it can in fact be quite off, especially as you get closer to the poles. Seeing from the example that you are in Hamburg, this will already have an effect that's noticable over a few hundred meters. See this answer for a good explanation.

The Problem: The distance between 1 degree in longitude varies greatly depending on your latitude.

This is because the earth is NOT flat, but a sphere - actually an ellipsoid. Therefore a straight line on a two dimensional map is NOT a straight line on the globe - and vice versa.

To get around this problem one can use the following approach:

1. Get the bearing from the start coordinate (L1) to the end coordinate (L2)
2. Calculate a new coordinate from the start coordinate (L1) along a great circle path, given the calculated bearing and a specified distance
3. Repeat this process, but using the newly calculated coordinate as the starting coordinate

We can create a few simple functions that will do the trick for us:

double radius = 6371; // earth's mean radius in km

// Helper function to convert degrees to radians
double DegToRad(double deg) {
    return (deg * Math.PI / 180);
}

// Helper function to convert radians to degrees
double RadToDeg(double rad) {
    return (rad * 180 / Math.PI);
}

// Calculate the (initial) bearing between two points, in degrees
double CalculateBearing(Location startPoint, Location endPoint) {
    double lat1 = DegToRad(startPoint.latitude);
    double lat2 = DegToRad(endPoint.latitude);
    double deltaLon = DegToRad(endPoint.longitude - startPoint.longitude);

    double y = Math.sin(deltaLon) * Math.cos(lat2);
    double x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(deltaLon);
    double bearing = Math.atan2(y, x);

    // since atan2 returns a value between -180 and +180, we need to convert it to 0 - 360 degrees
    return (RadToDeg(bearing) + 360) % 360;
}

// Calculate the destination point from given point having travelled the given distance (in km), on the given initial bearing (bearing may vary before destination is reached)
Location CalculateDestinationLocation(Location point, double bearing, double distance) {

    distance = distance / radius; // convert to angular distance in radians
    bearing = DegToRad(bearing); // convert bearing in degrees to radians

    double lat1 = DegToRad(point.latitude);
    double lon1 = DegToRad(point.longitude);

    double lat2 = Math.asin(Math.sin(lat1) * Math.cos(distance) + Math.cos(lat1) * Math.sin(distance) * Math.cos(bearing));
    double lon2 = lon1 + Math.atan2(Math.sin(bearing) * Math.sin(distance) * Math.cos(lat1), Math.cos(distance) - Math.sin(lat1) * Math.sin(lat2));
    lon2 = (lon2 + 3 * Math.PI) % (2 * Math.PI) - Math.PI; // normalize to -180 - + 180 degrees

    return new Location(RadToDeg(lat2), RadToDeg(lon2));
}

// Calculate the distance between two points in km
double CalculateDistanceBetweenLocations(Location startPoint, Location endPoint) {

    double lat1 = DegToRad(startPoint.latitude);
    double lon1 = DegToRad(startPoint.longitude);

    double lat2 = DegToRad(endPoint.latitude);
    double lon2 = DegToRad(endPoint.longitude);

    double deltaLat = lat2 - lat1;
    double deltaLon = lon2 - lon1;

    double a = Math.sin(deltaLat / 2) * Math.sin(deltaLat / 2) + Math.cos(lat1) * Math.cos(lat2) * Math.sin(deltaLon / 2) * Math.sin(deltaLon / 2);
    double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

    return (radius * c);
}

This uses a mean earth radius of 6371 km. See Wikipedia for an explanation of this number and its accuracy.

One can now calculate a new intermediary location between the two points, given a distance travelled (in km):

double bearing = CalculateBearing(startLocation, endLocation);

Location intermediaryLocation = CalculateDestinationLocation(startLocation, bearing, distanceTravelled);

Assuming a speed of v (e.g. 1.39) meters per second, one can now use a simple for loop to get points 1 second apart:

List<Location> locations = new ArrayList<Location>();

// assuming duration in full seconds
for (int i = 0; i < duration; i++){
    double bearing = CalculateBearing(startLocation, endLocation);
    double distanceInKm = v / 1000;
    Location intermediaryLocation = CalculateDestinationLocation(startLocation, bearing, distanceInKm);

    // add intermediary location to list
    locations.add(intermediaryLocation);

    // set intermediary location as new starting location
    startLocation = intermediaryLocation;
}

As an added bonus, you can even determin the time required to travel between any two points:

double distanceBetweenPoints = CalculateDistanceBetweenLocations(startPoint, endPoint) * 1000; // multiply by 1000 to get meters instead of km

double timeRequired = distanceBetweenPoints / v;

This will result in greater accuracy over any distance than a simple linear interpolation using just the delta of the coordinates. Although this approach is not perfect, it will have an error of generally 0.3% or less, which is quite acceptable. If you need a better solution, you might want to look into the Vincenty formula.

Answer 2

Calculations like these are actually very simple if you first convert your lat/longs to n-vectors (https://en.wikipedia.org/wiki/N-vector). After converting you can use standard interpolation, and you will also avoid any problems with long distances, the poles or the dateline.

If you check "External links" on the Wikipedia page, there is a page (http://www.navlab.net/nvector/) where ten problems are solved, and problem 6 on that page (interpolated position) should be the same as your question. As you can see, that solution is exact for any distance and also works at any earth-positions, like the poles.

Answer 3

I'm guessing I have to calculate some time deltas, but how do I factor in the movement speed?

At linear interpolation, in your casem you iterate between two time points, using the iteration variable t which runs from start time t1 to end time t2, with a predefined step. Asume step = 1 second, which is quite usable for your application.

long t1 = location1.getTimeStamp(); // in milliseconds;
long t2 = location2.getTimeStamp();
double deltaLat = location2.latitude - location1.latitude;
doule deltaLon =  location2.longitude- location1.longtude;
// remove this line if you don't have measured speed:
double deltaSpeed =  location2.speed - location1.speed;

long step = 1 * 1000; // 1 second in millis 
for (long t = t1; t1 < t2; t+= step) {

   // t0_1 shall run from 0.0 to (nearly) 1.0 in that loop
  double t0_1 = (t - t1) / (t2 - t1);
  double latInter = lat1 + deltaLat  * t0_1;
  double lonInter = lon1 + deltaLon  * t0_1;
  // remove the line below if you dont have speed
  double speedInter = speed1 + deltaSpeed  * t0_1;
  Location interPolLocation = new Location(latInter, lonInter, speedInter);
  // add interPolLocation to list or plot.
}

Answer 4

There are some other interpolation strategies that perform better than linear interpolation, includind kinematic interpolation, which takes as input the initial and final speed of anchor points. As an example, see this comparison from a recent paper (Long JA (2015) Kinematic interpolation of movement data. Int J Geogr Inf Sci 8816:1–15. doi: 10.1080/13658816.2015.1081909):

There are R and Python implementations for the Kinematic Interpolation. It should be easy to write a Java version.

Answer 5

Yep. Linear interpolation.

L1 = (1, 2, 3)
L2 = (4, 5, 6)
desired_number_of_interpolation_points = 9
interpolation_points = []

lat_step = (L2[0] - L1[0]) / (desired_number_of_interpolation_points + 1)
lon_step = (L2[1] - L1[1]) / (desired_number_of_interpolation_points + 1)
time_step = (L2[2] - L1[2]) / (desired_number_of_interpolation_points + 1) 

for i in range(1, desired_number_of_interpolation_points + 1)
    interpolation_points.append((lat_step * i, lon_step * i, time_step * i))