diff --git a/tools/plotjuggler/layouts/gps_vs_llk.xml b/tools/plotjuggler/layouts/gps_vs_llk.xml new file mode 100644 index 00000000000000..44980712edaf3a --- /dev/null +++ b/tools/plotjuggler/layouts/gps_vs_llk.xml @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R = 6378.137 -- Radius of earth in KM + -- Compute the Haversine distance between +-- two points defined by latitude and longitude. +-- Return the distance in meters +lat1, lon1 = value, v1 +lat2, lon2 = v2, v3 +dLat = (lat2 - lat1) * math.pi / 180 +dLon = (lon2 - lon1) * math.pi / 180 +a = math.sin(dLat/2) * math.sin(dLat/2) + +math.cos(lat1 * math.pi / 180) * math.cos(lat2 * math.pi / 180) * +math.sin(dLon/2) * math.sin(dLon/2) +c = 2 * math.atan(math.sqrt(a), math.sqrt(1-a)) +d = R * c +distance = d * 1000 -- meters +return distance + /gpsLocationExternal/latitude + + /gpsLocationExternal/longitude + /liveLocationKalman/positionGeodetic/value/0 + /liveLocationKalman/positionGeodetic/value/1 + + + + + + +