java interpolate between two gps points with a speed

Question

I'm trying to come up with function that could fill in gps coordinates between two points every second. There are few posts about this here, but I couldn't find something complete. The closest answer I found was:
Interpolate between 2 GPS locations based on walking speed
I modified one of the answer using the bearing. However, it still doesn't seem to work. Especially I think the distance calculation is wrong. Could someone look at the code below and change?
Thank you!

import java.util.ArrayList;

public class Test {

    double radius = 6371;

    public Test() {
        Location start = new Location(lat, lon);
        Location end = new Location(lat, lon);
        double speed = 1.39;
        double distance = CalculateDistanceBetweenLocations(start, end);
        double duration = distance / speed;
        System.out.println(distance + ", " + speed + ", " + duration);
        ArrayList<Location> locations = new ArrayList<Location>();
        for (double i = 0; i < duration; i += 1.0) {
            double bearing = CalculateBearing(start, end);
            double distanceInKm = speed / 1000;
            Location intermediaryLocation = CalculateDestinationLocation(start, bearing, distanceInKm);
            locations.add(intermediaryLocation);
            System.out.println(intermediaryLocation.latitude + ", " + intermediaryLocation.longitude);
            start = intermediaryLocation;
        }
    }

    double DegToRad(double deg) {
        return (deg * Math.PI / 180);
    }

    double RadToDeg(double rad) {
        return (rad * 180 / Math.PI);
    }

    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);
        return (RadToDeg(bearing) + 360) % 360;
    }

    Location CalculateDestinationLocation(Location point, double bearing, double distance) {
        distance = distance / radius;
        bearing = DegToRad(bearing);
        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;
        return new Location(RadToDeg(lat2), RadToDeg(lon2));
    }

    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 - 1));
        return (radius * c);
    }

    public static void main(String[] args) {
        new Test();
    }

    class Location {
        public double latitude, longitude;

        public Location(double lat, double lon) {
            latitude = lat;
            longitude = lon;
        }
    }

}

What are the expected outputs and what did you actually get? What does "wrong" mean? — Hulk, Aug 01 '16 at 05:45
I suspect that part of your confusion comes from the fact that a course with constant bearing is not a straight line. — Hulk, Aug 01 '16 at 06:27
I tried with two points on a straight line, and it's about 250 meter, but it says over 20,000 meters, and interpolated gps points were completely off. — jl303, Aug 01 '16 at 12:34

score 0 · Answer 1 · answered Aug 01 '16 at 13:53

Your method CalculateDistanceBetweenLocations contains an this line:

double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - 1));

which is equivalent to

double c = 2 * Math.atan2(Math.sqrt(a), 0.0);

which means that the result of Math.atan2 is always pi, independent of the value of a as long as a is positive.

Therefore CalculateDistanceBetweenLocations always returns 20015.086796020572 independent of the input coordinates.

score 0 · Accepted Answer · answered Aug 01 '16 at 20:24

0

You have a typing error in line

 double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - 1));

It should be

 double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));

answered Aug 01 '16 at 20:24

AlexWien

28,470
6
53
83

java interpolate between two gps points with a speed

2 Answers2