Python program to render possible paths for MH-370 from 19:40UTC ping arc and filter based on Doppler data

Geo.py

import math

earthRadius = 6371.0

def sphericalToECEF(latlon):
    lat, lon = latlon

    theta = lon
    phi = 90 - lat

    x = earthRadius * math.sin(math.radians(phi)) * math.cos(math.radians(theta))
    y = earthRadius * math.sin(math.radians(theta)) * math.sin(math.radians(phi))
    z = earthRadius * math.cos(math.radians(phi))

    return (x,y,z)

# Haversine great circle distance calculation for spherical earth
def greatCircleDistance(origin, destination):
    lat1, lon1 = origin
    lat2, lon2 = destination
    radius = earthRadius 

    dlat = math.radians(lat2-lat1)
    dlon = math.radians(lon2-lon1)
    a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    d = radius * c

    return d

def greatCircleDestination(latlon, bearing, dist):
    R = earthRadius
    lat1, lon1 = latlon
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    bearing = math.radians(bearing)
    lat2 = math.asin(math.sin(lat1)*math.cos(dist/R) + math.cos(lat1)*math.sin(dist/R)*math.cos(bearing))
    lon2 = lon1 + math.atan2(math.sin(bearing)*math.sin(dist/R)*math.cos(lat1), math.cos(dist/R)-math.sin(lat1)*math.sin(lat2))
    
    return math.degrees(lat2), math.degrees(lon2)

def LOSSpeed(pos1, vel1, pos2, vel2):
    P = (pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2])
    V = (vel1[0]-vel2[0], vel1[1]-vel2[1], vel1[2]-vel2[2])

    # LOS = (V dot P)/|P|

    VdotP = V[0]*P[0] + V[1]*P[1] + V[2]*P[2]
    Pbar = math.sqrt(P[0]*P[0] + P[1]*P[1] + P[2]*P[2])

    return VdotP / Pbar

def ecefVelocities(latlon, speed, bearing):
    x,y,z = sphericalToECEF(latlon)

    latCircum = earthRadius * math.cos(math.radians(latlon[0])) * math.pi * 2.0
    equCircum = earthRadius * math.pi * 2.0

    lonDistance = speed * math.sin(math.radians(bearing))
    latDistance = speed * math.cos(math.radians(bearing))

    thetaDot = (lonDistance / latCircum) * 2 * math.pi
    phiDot = (latDistance / equCircum) * 2 * math.pi

    x2,y2,z2 = sphericalToECEF((latlon[0]+math.degrees(phiDot), latlon[1]+math.degrees(thetaDot)))

    return (x2-x, y2-y, z2-z)

def knotsToKms(knots):
    return (knots * 1.852)/(3600.0)

def nmToKm(nm):
    return nm * 1.852

def kmToNm(km):
    return km / 1.852

MH370ForwardTrack.py

import math
import Geo
import matplotlib.pyplot as plt
from matplotlib.patches import Circle

''' Comparison with Victor's spreadsheet
aircraftPos = Geo.sphericalToECEF((-38.3313,87.424086))
aircraftVel = Geo.ecefVelocities((-38.3313,87.424086), 0.247293464, 187.812)
los = Geo.LOSSpeed((18173.906276,38051.980584,433.193954), (0.001476,-0.001458,-0.082097), aircraftPos, aircraftVel)
'''

aircraftGroundSpeed = 400
maxAircraftGSAfterLastRadarContact = 520
filterUsingDoppler = True
pingRingDiffError = 0.04
dopplerDiffError = 0.7
bearingIncrement = 1
startingIncrement = 1

# Last radar position at 18:22UTC lat - 6.5381 lon - 96.408
lastAircraftRadarPos = (6.5381, 96.408)
timeFromLastRadarContactTo1940Ping = 78
maxRangeFromLastRadarContactTo1940Ping = (timeFromLastRadarContactTo1940Ping / 60.0) * maxAircraftGSAfterLastRadarContact

satelliteInfo1940 = { 'LatLon': (1.640,64.520), 'XYZ' : (18140.934782,38066.764468,1206.206412), 'Velocity': (0.001663,-0.000776,-0.001656),  'LOSSpeed': Geo.knotsToKms(-53.74), 'NextPingTimeOffset': 60.0, 'PingRadius': Geo.nmToKm(1762), 'Color': 'b', 'Elevation': 55.80 }
satelliteInfo2040 = { 'LatLon': (1.576,64.510), 'XYZ' : (18147.379654,38064.186790,1159.122617), 'Velocity': (0.001811,-0.000618,-0.024394),  'LOSSpeed': Geo.knotsToKms(-70.87), 'NextPingTimeOffset': 60.0, 'PingRadius': Geo.nmToKm(1805), 'Color': 'c', 'Elevation': 54.98 }
satelliteInfo2140 = { 'LatLon': (1.404,64.500), 'XYZ' : (18154.399609,38061.901891,1032.716137), 'Velocity': (0.001962,-0.000627,-0.045468),  'LOSSpeed': Geo.knotsToKms(-84.20), 'NextPingTimeOffset': 60.0, 'PingRadius': Geo.nmToKm(1962), 'Color': 'g', 'Elevation': 52.01 }
satelliteInfo2240 = { 'LatLon': (1.136,64.490), 'XYZ' : (18161.767618,38059.215141,835.616356),  'Velocity': (0.001981,-0.000841,-0.063437),  'LOSSpeed': Geo.knotsToKms(-97.14), 'NextPingTimeOffset': 91.0, 'PingRadius': Geo.nmToKm(2199), 'Color': 'r', 'Elevation': 47.54 }
satelliteInfo0011 = { 'LatLon': (0.589,64.471), 'XYZ' : (18173.906276,38051.980584,433.193954),  'Velocity': (0.001476,-0.001458,-0.082097),  'LOSSpeed': Geo.knotsToKms(-111.18),'NextPingTimeOffset':  0.0, 'PingRadius': Geo.nmToKm(2642), 'Color': 'm', 'Elevation': 39.33 }

satelliteInfos = [satelliteInfo1940, satelliteInfo2040, satelliteInfo2140, satelliteInfo2240, satelliteInfo0011] 

# Calculate starting points on 19:40UTC arc
startingPoints = []
for bearing in range(0, 180, startingIncrement):
    pingRingPos = Geo.greatCircleDestination(satelliteInfo1940['LatLon'], bearing, satelliteInfo1940['PingRadius'])
    if Geo.greatCircleDistance(pingRingPos, lastAircraftRadarPos) < Geo.nmToKm(maxRangeFromLastRadarContactTo1940Ping):
        startingPoints.append(pingRingPos)

# Set up plot
fig = plt.figure(1, figsize=(8.5, 11))
ax = plt.subplot(111, aspect='equal')
ax.set_xlim(60,115)
ax.set_ylim(-40,50)
#ax.set_xlim(80,120)
#ax.set_ylim(-40,20)

# Circle for 19:40 ping circle - 29.5 radius
pingCircle = Circle((satelliteInfo1940['LatLon'][1],satelliteInfo1940['LatLon'][0]), radius=29.5, fill=False)
ax.add_artist(pingCircle)

if filterUsingDoppler:
    dopplerError = "%d%%" % (dopplerDiffError * 100.0)
else:
    dopplerError = filterUsingDoppler
ax.set_title("MH370 Forwardtrack - %dkt - Doppler Filter - %s" % (aircraftGroundSpeed, dopplerError))

# Satellite positions
for satelliteInfo in satelliteInfos:
    ax.scatter(satelliteInfo['LatLon'][1], satelliteInfo['LatLon'][0])

# Render all possible starting positions
for pos in startingPoints:
    ax.scatter(pos[1], pos[0])

# Iterate over the starting points 
for pos in startingPoints:
    for bearing in range(0, 359, bearingIncrement):
    #for bearing in range(90, 270, 1):
        segments = [pos]
        lastPos = pos
        for index in range(0, len(satelliteInfos)-1):
            pingInterval = satelliteInfos[index]['NextPingTimeOffset']
            newPos = Geo.greatCircleDestination(lastPos, bearing, Geo.nmToKm((pingInterval/60.0)*aircraftGroundSpeed))
            satelliteRange = Geo.greatCircleDistance(newPos, satelliteInfos[index+1]['LatLon'])
            if math.fabs(satelliteRange - satelliteInfos[index+1]['PingRadius']) < pingRingDiffError * satelliteInfos[index+1]['PingRadius']:
                if filterUsingDoppler:
                    aircraftECEF = Geo.sphericalToECEF(newPos)
                    aircraftECEFVel = Geo.ecefVelocities(newPos, Geo.knotsToKms(aircraftGroundSpeed), bearing)
                    LOSSpeed = Geo.LOSSpeed(satelliteInfos[index+1]['XYZ'], satelliteInfos[index+1]['Velocity'], aircraftECEF, aircraftECEFVel) * -1.0
                    if math.fabs(LOSSpeed - satelliteInfos[index+1]['LOSSpeed']) < math.fabs(dopplerDiffError * satelliteInfos[index+1]['LOSSpeed']):
                        segments.append(newPos)
                        lastPos = newPos
                    else:
                        break
                else:
                    segments.append(newPos)
                    lastPos = newPos
            else:
                break
        if len(segments) == 5:
            for index in range(1, len(satelliteInfos)):
                segmentPos = segments[index]
                ax.scatter(segmentPos[1], segmentPos[0], c=satelliteInfos[index]['Color'])
                prevSegmentPos = segments[index-1]
                ax.plot([prevSegmentPos[1], segmentPos[1]], [prevSegmentPos[0], segmentPos[0]])

# Last radar position lat - 6.5381 lon - 96.408
ax.scatter(lastAircraftRadarPos[1], lastAircraftRadarPos[0], c='y', s=50, marker='D')
ax.annotate('Last radar position', xy=(lastAircraftRadarPos[1], lastAircraftRadarPos[0]), xytext=(lastAircraftRadarPos[1]+5, lastAircraftRadarPos[0]+5), arrowprops=dict(facecolor='black'))

if filterUsingDoppler:
    dopplerError = "%d" % (dopplerDiffError * 100.0)
else:
    dopplerError = filterUsingDoppler

plt.savefig("MH370 Forwardtrack - %dkt - Doppler Filter - %s.png" % (aircraftGroundSpeed, dopplerError))

plt.show()