from numpy import *
from pylab import *
from scipy.interpolate import *
# t sec az deg, el deg, range nmi v?
az_data = array([
[12 + 10./30, -43],
[13 + 10./30, -44],
[13 + 20./30, -45],
[14 + 19./30, -46],
[15 + 13./30, -47],
[16 + 13./30, -48],
[18 + 0./30, -49],
[21 + 6./30, -50],
[23 + 6./30, -51],
[24 + 18./30, -52],
[26 + 6./30, -53],
[27 + 18./30, -54],
[29 + 6./30, -55],
[30 + 13./30, -56],
[31 + 19./30, -57],
[32 + 25./30, -58],
[33 + 0./30, -58]
])
el_data = array([
[12 + 10./30, -26],
[13 + 19./30, -27],
[16 + 6./30, -28],
[18 + 25./30, -29],
[21 + 7./30, -30],
[23 + 13./30, -31],
[25 + 25./30, -32],
[28 + 1./30, -33],
[30 + 1./30, -34],
[32 + 7./30, -35],
[33 + 0./30, -35]
])
rng_data = array([
[12 + 10./30, 4.4],
[13 + 10./30, 4.3],
[15 + 3./30, 4.2],
[17 + 4./30, 4.1],
[19 + 1./30, 4.0],
[20 + 29./30, 3.9],
[22 + 28./30, 3.8],
[24 + 28./30, 3.7],
[27 + 1./30, 3.6],
[29 + 13./30, 3.5],
[31 + 22./30, 3.4],
[33 + 0./30, 3.4],
])
az = interp1d(az_data[:,0], az_data[:,1])
el = interp1d(el_data[:,0], el_data[:,1])
rng = interp1d(rng_data[:,0], rng_data[:,1])
def pos(t):
vel = array([258 * KTS, 0, 0])
vel = array([369 * KTS, 0, 0])
return array([0, 0, 25000 * FEET])[newaxis] + vel[newaxis,:] * (t[:,newaxis] - az_data[0, 0])
NMI = 1852.
HOUR = 3600.
DEG = pi / 180.
KTS = NMI / HOUR
FEET = FOOT = .3048
rel_speed = (diff(rng_data[:,1] * NMI) / diff(rng_data[:,0])) / KTS
t = arange(15, 30, .1)
azs = az(t)
els = el(t)
rngs = rng(t)
x = rngs * NMI * cos(azs * DEG) * cos(els * DEG)
y = -rngs * NMI * sin(azs * DEG) * cos(els * DEG)
z = rngs * NMI * sin(els * DEG)
xyz = vstack([x, y, z]).T
p = pos(t)
print (((linalg.norm(p[0] + xyz[0] - p[-1] - xyz[-1])) / (t[-1] - t[0])) / KTS)
ax = subplot(4, 1, 1)
plot(az_data[:,0], az(az_data[:,0]))
plot(t, -arctan2(y, x) / DEG)
ylabel('Az [deg]')
subplot(4, 1, 2, sharex=ax)
ylabel('El [deg]')
plot(el_data[:,0], el(el_data[:,0]))
plot(t, arcsin(z / linalg.norm(xyz, axis=1)) / DEG)
subplot(4, 1, 3, sharex=ax)
ylabel('Range [nmi]')
plot(rng_data[:,0], rng(rng_data[:,0]))
plot(t, linalg.norm(xyz, axis=1) / NMI)
subplot(4, 1, 4, sharex=ax)
ylabel('Rel vel [kts]')
xlabel('t [sec]')
plot(rng_data[2:-1,0], rel_speed[1:-1])
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot([0], [0], [0], 'bo')
ax.plot(x / NMI, y / NMI, z / FEET, 'r-')
ax.plot(x[:1]/NMI, y[:1] / NMI, z[:1] / FEET, 'ro')
xlabel('x [nmi]')
ylabel('y [nmi]')
ax.set_zlabel('z [feet]')
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot((p[:,0])/NMI, (p[:,1]) / NMI, (p[:,2]) / FEET, 'b-')
ax.plot((p[:1,0])/NMI, (p[:1,1]) / NMI, (p[:1,2]) / FEET, 'bo')
ax.plot((x + p[:,0])/NMI, (y + p[:,1]) / NMI, (z + p[:,2]) / FEET, 'r-')
ax.plot((x[:1] + p[:1,0])/NMI, (y[:1] + p[:1,1]) / NMI, (z[:1] + p[:1,2]) / FEET, 'ro')
xlabel('x [nmi]')
ylabel('y [nmi]')
ax.set_zlabel('z [feet]')
show()