GPS 'Submarine' Radar Detecting Other location - math

Question

Lets say i have a drone and my phone. I have looked at NORTH, and placed the drone 4 meters in my front. I should look at the radar, and see the drone at the north position.
My radar is 256x256 pixels, and max distance of the drone is 200m witch is aprox 0.002 Lat/Long degrees.
So, ive made a simple test class to test this drone at my north.
public static void main(String[] args) {
BigDecimal droneLat = new BigDecimal(-22.811468333334087077446383773349225521087646484375);
BigDecimal droneLong = new BigDecimal(-47.04746500000123177187560941092669963836669921875);
BigDecimal phoneLat = new BigDecimal(-22.81129370000000022855601855553686618804931640625);
BigDecimal phoneLong = new BigDecimal(-47.04832809999999909678081166930496692657470703125);
BigDecimal latDiff = phoneLat.subtract(droneLat);
BigDecimal longDiff = phoneLong.subtract(droneLong);
int pixelsLat = degreesToPixels265x265(latDiff.abs());
int pixelsLong = degreesToPixels265x265(longDiff.abs());
if (latDiff.compareTo(new BigDecimal(0)) < 0) { // drone > phone means phone is farther to north, phone lat will be lower, it will show lower in radar
pixelsLat = -pixelsLat;
}
if (longDiff.compareTo(new BigDecimal(0)) > 0) { // drone > phone means phone is farther to north, phone lat will be lower, it will show lower in radar
pixelsLong = -pixelsLong;
}
System.out.println("DiffLat "+latDiff);
System.out.println("DiffLong "+longDiff);
System.out.println("Pixels lat = "+pixelsLat);
System.out.println("Pixels long = "+pixelsLong);
}
public static int degreesToPixels265x265(BigDecimal n) {
BigDecimal p1 = n.multiply(new BigDecimal(256/2));
return p1.divide(new BigDecimal(0.002d),2,BigDecimal.ROUND_CEILING).intValue();
}
Pixels Lat will sum to 128 to draw the drones position, so is pixelsLong. As the drone is in my north, should pixels lat be more positive ?
Am i doing some wrong math here to detect the true coordinates to draw this ?
Thanks alot for the attention !

Answer 1

The general problem you're trying to solve is "projection": Projecting a lat/lon coordinate that's on a sphere onto a flat surface (the screen). Here's a question/answer about how to solve that problem: Converting longitude/latitude to X/Y coordinate
But if you don't care about solving the general problem of projection, for any projection (Mercator, etc.) and handling very long distances (thousands of miles) and edge cases (you're flying near the north or south pole), you can do something pretty simple: Compute the bearing and distance from point A (your location) and point B (the drone's location). Using that bearing and distance, plot the drone on the screen with an approximation that should work fine for a few hundred meters.
To compute the distance, use the Haversine formula:
public static double distance(double lat1, double lon1, double lat2, double lon2) {
double R = 6372.8; // Radius of the earth in kilometers.
double dLat = Math.toRadians(lat2 - lat1);
double dLon = Math.toRadians(lon2 - lon1);
lat1 = Math.toRadians(lat1);
lat2 = Math.toRadians(lat2);
double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) + Math.sin(dLon / 2) * Math.sin(dLon / 2) * Math.cos(lat1) * Math.cos(lat2);
double c = 2 * Math.asin(Math.sqrt(a));
return R * c;
}
And to compute the angle between two points (really just the initial angle--on a sphere, the angle changes as you get closer but we're approximating over a short distance):
public static double bearing(double lat1, double lon1, double lat2, double lon2) {
double latitude1 = Math.toRadians(lat1);
double latitude2 = Math.toRadians(lat2);
double longitude1 = Math.toRadians(lon1);
double longitude2 = Math.toRadians(lon2);
double longDiff = longitude2 - longitude1;
double y = Math.sin(longDiff) * Math.cos(latitude2);
double x = Math.cos(latitude1) * Math.sin(latitude2) - Math.sin(latitude1) * Math.cos(latitude2) * Math.cos(longDiff);
return Math.atan2(y, x);
}
Once you can compute distance & bearing, you can map that onto your screen (what you're doing is converting polar coordinates to cartesian/XY coordinates):
public static void plotLatLon(double droneLat, double droneLon) {
// Get location from phone geolocation.
double phoneLat = -22.8112937000;
double phoneLon = -47.0483281000;
int width = 256;
int height = 256;
// 256 pixels = 400 meters; 200 meters away = edge of screen.
double scaleFactor = 256.0 / 400.0;
double bearing = bearing(phoneLat, phoneLon, droneLat, droneLon);
double distance = distance(phoneLat, phoneLon, droneLat, droneLon);
System.out.println("Bearing: " + (Math.toDegrees(bearing) + 360.0) % 360.0);
System.out.println("Distance: " + distance);
double angle = (Math.PI / 2.0) - bearing;
int x = (int)(distance * scaleFactor * Math.cos(angle));
int y = (int)(distance * scaleFactor * Math.sin(angle));
// Assumes radar coordinate system has (0,0) at the lower left.
Radar.plotXY(x + width / 2, y + height / 2);
}
And here's the results I get:
Bearing: 102.38029388096368
Distance: 90.59766443003579
X: 184
Y: 116
(Your drone isn't actually to the north; The coordinates you gave place it to the east and a little south.)
Note that double precision is enough: "Double precision floats give sub-millimetre precision anywhere on Earth."
You might also be interested in a library like spatial4j for doing geographic calculations in Java.

Answer 2

When you go north, latitude increases up to 90 degrees at the north pole. So yes, you are correct. The pixels should be more positive. (You're in North America/Europe, I assume?) Read This
However, I'm working on a GPS project right now, and I can tell you from experience that 4 meters is far, far to close. Civilian GPS receivers without assistance from cell towers can get to about 10 meters of accuracy under ideal conditions. Maybe better with GLONASS.
If you're willing to wait, you can buy one of these:
http://store.swiftnav.com
GPS 101: Move the drone further away, and account for results that are only accurate to about 20 meters.