pro recon

; Paul Withers, 2001.09.15
; LPL, Arizona

; To run code, enter the following at your IDL prompt
; .run recon
; recon

; Simplified version of trajectory and atmospheric structure
; reconstruction code

; Take aerodynamic accelerations dataset and initial conditions,
; then solve for trajectory and atmospheric structure
; Uses entry state, drag-only aerodynamics,
; constant drag coefficient, spherically symmetric planet 
; and accelerometer dataset to obtain trajectory

; Best documentation is
; http://www.lpl.arizona.edu/~withers/pppp/pdf/oureport.pdf

; Spacecraft are Mars Pathfinder (mpf), Galileo (gal), 
; Pioneer Venus large/sounder probe (pv), Beagle 2 (b2),
; and Huygens (huy)

forward_function get_planet, get_sct, get_entrystate

planet=''
while planet ne 'venus' and planet ne 'earth' and planet ne 'mars' $
 and planet ne 'jupiter' and planet ne 'titan' and planet ne 'user' do $
 read, planet, prompt='What planet? [venus,earth,mars,jupiter,titan,user] '

junk = get_planet(planet)
omega = junk(0)
rho0 = junk(1)
dsh = junk(2)
gm = junk(3)
rplanet = junk(4)
mean_molecular_mass = junk(5)

sct=''
while sct ne 'mpf' and sct ne 'gal' and sct ne 'b2' and sct ne 'huy' and $
 sct ne 'pv' and sct ne 'user' do $
 read, sct, prompt='What spacecraft? [mpf, gal, pv, user] '

junk = get_sct(sct)
mass = junk(0)
area = junk(1)

entry=''
while entry ne 'mpf' and entry ne 'gal' and entry ne 'pv' and $
 entry ne 'user' do $
 read, entry, prompt='What entry state? [mpf, gal, pv, user] '

junk = get_entrystate(entry)
t0 = junk(0)
r0 = junk(1)+rplanet
lat0 = junk(2)
lon0 = junk(3)
speed0 = junk(4)
gamma0 = junk(5)
psi0 = junk(6)

universal_gas_constant = 8.31451 ; Universal gas constant J K-1 mol-1
range_0 = dsh ; Range for dsh and pressure integration condition, m

n=0
read, n, prompt='How many timesteps? [171 for MPF dataset] '

dt=0.
read, dt, prompt='Length of timesteps in seconds? [1 for MPF dataset] '

cd=0.
read, cd, prompt='Dimensionless drag coefficient? [2 is best] '

trustxy = 0.5
while trustxy ne 0. and trustxy ne 1. do read, trustxy, prompt=$
'Use z axis data only [0] or full xyz axis data [1]? [0 works best for MPF] '

filename = 'recon.dat'
print, ''
print, 'Opening ' + filename + ' ...'
print, ''
; Read in the file of accelerometer measurements
data_array = fltarr(4,n)
openr, lun1, filename, /get_lun
readf, lun1, data_array
free_lun, lun1

t_array           = data_array(0,*) 
aeroaccsctx_array = data_array(1,*) 
aeroaccscty_array = data_array(2,*)
aeroaccsctz_array = data_array(3,*)
; t_array is time in seconds since my t=0
; aeroaccsctx,y,z_array is aerodynamic acceleration
; in ms^-2 along spacecraft x,y,z axis

; Setup many arrays used during integration
x_array = dblarr(n+1)
y_array = x_array
z_array = x_array
; position in inertial cartesian frame
vx_array = x_array
vy_array = x_array
vz_array = x_array
; velocity in inertial cartesian frame
height_array = x_array
lat_array = x_array
lon_array = x_array
; position in momentary spherical frame
vxrel_array = x_array
vyrel_array = x_array
vzrel_array = x_array
; velocity relative to atmosphere in inertial cartesian frame
aeroacc_array = x_array
; magnitude of aerodynamic acceleration
vrel_array = x_array
; magnitude of velocity relative to atmosphere
aeroaccinex_array = x_array
aeroacciney_array = x_array
aeroaccinez_array = x_array
; aerodynamic accelerations transformed into inertial cartesian frame
;aeroaccsctx_array = x_array
;aeroaccscty_array = x_array
;aeroaccsctz_array = x_array
; aerodynamic accelerations in spacecraft frame, defined elsewhere now

vwind_array = dblarr(3,n+1)
; wind velocity (due to planetary rotation) at spacecraft in inertial cartesian frame
g_array = dblarr(3,n+1)
; gravitational acceleration at spacecraft in inertial cartesian frame


last_t = t_array(n-1)
t_array = [t_array(0:*), last_t+dt]
; We will integrate on one timestep beyond our last data points
; So we'll need one more space in the arrays






colat0 = (double(90.)-lat0)*double(!pi/180.)
lon0   = lon0  *double(!pi/180.)
gamma0 = gamma0*double(!pi/180.)
psi0   = psi0  *double(!pi/180.)
; convert angles from degrees to radians

v_r = double(-1.) * speed0*sin(gamma0)
v_colat = double(-1.) * speed0*cos(gamma0)*cos(psi0)
v_lon = speed0*cos(gamma0)*sin(psi0)
; Transform velocity into momentary spherical frame

v_intx = v_r*sin(colat0)*cos(lon0) + v_colat*cos(colat0)*cos(lon0) - v_lon*sin(lon0)
v_inty = v_r*sin(colat0)*sin(lon0) + v_colat*cos(colat0)*sin(lon0) + v_lon*cos(lon0)
v_intz = v_r*cos(colat0)           - v_colat*sin(colat0)
; this transformation from Bradbury, Theoretical Mechanics, p64
; it's a standard one
; Momentary spherical frame to momentary cartesian frame

; then convert to frame1
vx1 = v_intx * cos(omega*t_array(0)) - v_inty * sin(omega*t_array(0))
vy1 = v_intx * sin(omega*t_array(0)) + v_inty * cos(omega*t_array(0))
vz1 = v_intz
; Transform from momentary cartesian frame to inertian cartesian frame

lon0 = lon0 + omega*t_array(0) ; going into frame 1
; Transfrom longitude from
; momentary spherical frame to inertial spherical frame

z1 = r0*cos(colat0)
y1 = r0*sin(colat0)*sin(lon0)
x1 = r0*sin(colat0)*cos(lon0)
; Transform position from inertial spherical frame into inertial cartesian frame

; Initialise some arrays
x_array(0) = x1
y_array(0) = y1
z_array(0) = z1
vx_array(0) = vx1
vy_array(0) = vy1
vz_array(0) = vz1



new_coord = cv_coord(from_rect=[x1,y1,z1], /to_sphere)
height_array(0) = new_coord(2) - rplanet
lat_array(0) = new_coord(1) * double(180. /!pi)
junk = (new_coord(0) - omega*t_array(0)) * double(180. /!pi)
while junk lt 0 do junk = junk + double(360.)
junk = double(junk - fix(junk)/360 * 360.)
lon_array(0) = junk


; Begin trajectory integration
i = 1
while i lt n+1 do begin
; n+1 because we will integrate one timestep beyond the last datapoint

vwind = crossp([0.,0.,1.]*omega, [x_array(i-1),y_array(i-1),z_array(i-1)])
; calculate wind velocity at current position
vxrel = vx_array(i-1) - vwind(0)
vyrel = vy_array(i-1) - vwind(1)
vzrel = vz_array(i-1) - vwind(2)
; calculate spacecraft velocity relative to atmosphere in inertial cartesian frame
aeroacc = sqrt( trustxy*(aeroaccsctx_array(i-1)^2 + aeroaccscty_array(i-1)^2) + aeroaccsctz_array(i-1)^2 )
vrel = sqrt( vxrel^2 + vyrel^2 + vzrel^2 )
; calculate magnitudes of aerodynamic acceleration and relative velocity
aeroaccinex = double(-1.) * aeroacc * vxrel / vrel
aeroacciney = double(-1.) * aeroacc * vyrel / vrel
aeroaccinez = double(-1.) * aeroacc * vzrel / vrel
; The above equations define my aerodynamic assumptions
; The only aerodynamic force is drag which is directed opposite to 
; velocity relative to atmosphere

g_r = double(-1.)*gm/((height_array(i-1)+rplanet)^2)
; Acceleration due to gravity in momentary spherical frame

g_intx=g_r*sin((double(90.)-lat_array(i-1))*double(!pi/180.))*cos(lon_array(i-1)*double(!pi/180.))
g_inty=g_r*sin((double(90.)-lat_array(i-1))*double(!pi/180.))*sin(lon_array(i-1)*double(!pi/180.))
g_intz=g_r*cos((double(90.)-lat_array(i-1))*double(!pi/180.))
; this transformation from Bradbury, Theoretical Mechanics, p64
; it's a standard one
; Transform from momentary spherical frame to momentary cartesian frame

gx1 = g_intx * cos(omega*t_array(i-1)) - g_inty * sin(omega*t_array(i-1))
gy1 = g_intx * sin(omega*t_array(i-1)) + g_inty * cos(omega*t_array(i-1))
gz1 = g_intz
; Transform from momentary cartesian frame to inertial cartesian frame

; update arrays
vwind_array(*,i-1) = [vwind(0), vwind(1), vwind(2)]
; store wind velocity in inertial cartesian frame at current position
vxrel_array(i-1) = vxrel
vyrel_array(i-1) = vyrel
vzrel_array(i-1) = vzrel
; store spacecraft velocity relative to atmosphere in inertial cartesian frame
aeroacc_array(i-1) = aeroacc
vrel_array(i-1) = vrel
; store magnitudes of aerodynamic acceleration and relative velocity
aeroaccinex_array(i-1) = aeroaccinex
aeroacciney_array(i-1) = aeroacciney
aeroaccinez_array(i-1) = aeroaccinez
; store xyz components of aerodynamic acceleration in inertial cartesian frame
g_array(*,i-1) = [gx1,gy1,gz1]
; store acceleration due to gravity in inertial cartesian frame at spacecraft position

x_array(i) = x_array(i-1) + vx_array(i-1)*dt
y_array(i) = y_array(i-1) + vy_array(i-1)*dt
z_array(i) = z_array(i-1) + vz_array(i-1)*dt

vx_array(i) = vx_array(i-1) + (aeroaccinex+gx1)*dt
vy_array(i) = vy_array(i-1) + (aeroacciney+gy1)*dt
vz_array(i) = vz_array(i-1) + (aeroaccinez+gz1)*dt
; simple first order integration technique
; update arrays

new_coord = cv_coord(from_rect=[x_array(i),y_array(i),z_array(i)], /to_sphere)
height_array(i) = new_coord(2) - rplanet
lat_array(i) = new_coord(1) * double(180. /!pi)
junk = (new_coord(0) - omega*t_array(i)) * double(180. /!pi)
while junk lt 0 do junk = junk + double(360.)
junk = double(junk - fix(junk)/360 * 360.)
lon_array(i) = junk


i = i+1
;print, i
endwhile

; Have assumed that aerodynamic acceleration and relative velocity
; are oppositely directed and that drag coefficient is defined as follows
rho_array = double(2.) * aeroacc_array /( (vrel_array^2) * cd * (area/mass))

; Acceleration due to gravity at each timestep
grav_array = gm / (rplanet+height_array)^2

; Integrate equation of hydrostatic equilibrium for pressure
; At first, without any constant of integration
press_array = dblarr(n+1)
i=1
while i lt n+1 do begin
press_array(i) = int_tabulated(  height_array(0:i), rho_array(0:i)*grav_array(0:i), /sort )
i=i+1
;print, i
endwhile

height_0 = max(height_array)
; apply constant of integration at earliest possible point
aa= where((height_0 - height_array) lt range_0)
result = poly_fit(height_array(aa), alog(rho_array(aa)), 1)
; Do a linear fit to log(rho) in the relevant altitude range
; to calculate the density scale height at height_0

density_scale_height_0 = double(-1.)/result(1)
grav_0 = interpol(grav_array, height_array, [height_0])
rho_0 = interpol(rho_array, height_array, [height_0])
; Obtain density scale height, gravity, and density at height_0

press_0 = rho_0 * grav_0 * density_scale_height_0
; Obtain pressure at height_0

press_offset = interpol(press_array, height_array, [height_0])
; What did we calculate as the pressure at height_0
; without using a constant of integration?
press_array = press_array + press_0(0) - press_offset(0)
; Shift all pressures so that pressure at height_0 is what we told it to be

; Solve ideal gas law for temperature
temp_array = press_array *  mean_molecular_mass/ (rho_array * universal_gas_constant)
plot, temp_array, height_array, $
title='Temperature Profile', xtitle='Temperature (K)', ytitle='Altitude (m)', $
color=0, background=!d.n_colors-1, charsize=2, charthick=1.5, $
position=[0.25,0.15,0.9,0.9]

print, 'Useful arrays are:'
print, 'height_array 	- Altitude above surface (m)'
print, 'lat_array 	- Planetocentric north latitude (deg)'
print, 'lon_array 	- Planetocentric east longitude (deg)'
print, 't_array 	- Time since t0 (sec)'
print, 'vrel_array 	- Relative speed between spacecraft and atmosphere (m s-1)'
print, 'rho_array 	- Atmospheric density (kg m-3)'
print, 'press_array 	- Atmospheric pressure (Pa)'
print, 'temp_array 	- Atmospheric temperature (K)'
print, 'Type "stop" as penultimate line of recon.pro to be able to acess these'

end