# ============================================================================= # Compute target FNPEG params from nominal FNPEG run (comment out during MC) # ============================================================================= print('\nCOMPUTING NOMINAL PARAMETERS...') r = xxvec2[0, :] vmag = xxvec2[3, :] / 1e3 h = (r - paramsNom_PBC.p.rad * 1e3) / 1e3 # get density and speed of sound at each altitude step rho = [] assfun = interp.interp1d(paramsNom_PBC.p.sound[0, :], paramsNom_PBC.p.sound[1, :]) ass = assfun(h) for hi in h: rho.append(getRho_from_table(paramsNom_PBC.atmdat, hi)) rho = np.asarray(rho) # compute mach number and dynamic pressure at each step Mvec = vmag * 1e3 / ass Qinc = 1 / 2 * rho * (vmag * 1e3)**2 # range lon0 = xxvec2[1, 0] lonf = xxvec2[1, -1] lat0 = xxvec2[2, 0] latf = xxvec2[2, -1] dlon = abs(lonf - lon0) dsig = np.arccos(np.sin(lat0) * np.sin(latf)\ + np.cos(lat0) * np.cos(latf) * np.cos(dlon))
def sphericalEntryEOMsAug(t, yy, sig0, e0, params): ''' Like sphericalEntryEOMs from ASEN 6015, but dimensional and with GRAM dens. Defines 3D 3DOF EOMs for lifting entry vehicle over *ellipsoidal* rotating planet. Defined as in FNPAG (Lu et. al), then augmented with 7th state to capture evolution of range. Uses bank angle profile as defined in FNPEG. ASSUMPTIONS: - constant bank angle - exponential atmosphere - rotating planet - ellipsoidal planet (J2) params requirements: OmP: rotation rate of planet, rad/s radP: equatorial radius of planet, m mu: gravitational parameter of planet, m^3/s^2 J2: zonal coefficient J2 of planet L_D: vehicle L/D ratio BC: vehicle ballistic coefficient, kg/m^2 INPUTS: t: time (not used) yy: state vector: r: radial distance, m lon: longitude, radians lat: latitude, radians v: planet-relative velocity magnitude, m/s gam: flight path angle, negative-down, radians hda: heading angle, clockwise from north, radians sig: bank angle, rad OUTPUTS: dydt: time derivative of each state ''' # extract constants from params OmP = params.p.om radP = params.p.rad * 1e3 mu = params.p.mu * 1e9 # convert from km^3/s^2 to m^3/s^2 J2 = params.p.J2 L_D = params.LD BC = params.BC ef = params.ef sigf = params.sigf # extract state variables r = yy[0] # lon = yy[1] # not used in EOMs lat = yy[2] v = yy[3] gam = yy[4] hda = yy[5] # get gravity terms from J2 model gr = mu / r**2 * (1 + J2 * (radP / r)**2 * (1.5 - 4.5 * np.sin(lat)**2)) gphi = mu / r**2 * (J2 * (radP / r)**2 * (3 * np.sin(lat) * np.cos(lat))) # get density at this altitude if params.dMode == 'fun': rho = params.dFun((r - radP) / 1e3) elif params.dMode == 'table': rho = getRho_from_table(params.atmdat, (r - radP) / 1e3) else: sys.exit('atm mode not recognized') # #### TROUBLESHOOTING CODE #### # rho0 = 0.0263 # kg/m^3 # H = 10153 # m # h = r - radP # rho = rho0 * np.exp(-h / H) # #### TROUBLESHOOTING CODE #### # compute e e = mu / r - v**2 / 2 # compute current bank angle sig0 = (sig0 + 180) % (360) - 180 sig = sig0 + (e - e0) / (ef - e0) * (sigf - sig0) sig = np.radians(sig) # compute lift and drag accelerations D = rho * v**2 / (2 * BC) L = L_D * D # EOMs dydt = np.empty(7) dydt[:] = np.NaN dydt[0] = v * np.sin(gam) dydt[1] = v * np.cos(gam) * np.sin(hda) / (r * np.cos(lat)) dydt[2] = v * np.cos(gam) * np.cos(hda) / r dydt[3] = -D\ - gr * np.sin(gam)\ - gphi * np.cos(gam) * np.cos(hda)\ + OmP**2 * r * np.cos(lat) * (np.sin(gam) * np.cos(lat)\ - np.cos(gam) * np.sin(lat)\ * np.cos(hda)) dydt[4] = 1/v * (L * np.cos(sig)\ + (v**2/r - gr) * np.cos(gam)\ + gphi * np.sin(gam) * np.cos(hda)\ + 2 * OmP * v * np.cos(lat) * np.sin(hda)\ + OmP**2 * r * np.cos(lat) * (np.cos(gam) * np.cos(lat)\ + np.sin(gam) * np.cos(hda)\ * np.sin(lat))) dydt[5] = 1/v * (L * np.sin(sig) / np.cos(gam)\ + v**2/r * np.cos(gam) * np.sin(hda) * np.tan(lat)\ + gphi * np.sin(hda) / np.cos(gam)\ - 2 * OmP * v * (np.tan(gam) * np.cos(hda) * np.cos(lat)\ - np.sin(lat))\ + OmP**2 * r / np.cos(gam) * np.sin(hda)\ * np.sin(lat) * np.cos(lat)) dydt[6] = -v * np.cos(gam) / r return dydt
def dynamics(t, yy, sig, params, returnForces=False, returnAccelerations=False): ''' ODEs for full dynamics acting on the vehicle assume: - just one vehicle - yy is [position; velocity] in INERTIAL frame - sig is bank angle in deg ''' dydt = np.empty(6) dydt[:] = np.NaN # extract inertial state components x = yy[0] y = yy[1] z = yy[2] dx = yy[3] dy = yy[4] dz = yy[5] xvec_N = np.array([x, y, z]) vvec_N = np.array([dx, dy, dz]) r = norm(xvec_N) mu_r3 = params.p.mu / r**3 ## Get gravitational force agvec_N = -mu_r3 * xvec_N ## Get aerodynamics forces # TODO: carefully replace below with VN2Vinf function call SN = DCMs.getSN(t, params) NS = DCMs.getNS(t, params) OMvec = np.array([0, 0, -params.p.om]) vvec_S = SN @ (vvec_N + cross(OMvec, xvec_N)) h = r - params.p.rad if params.dMode == 'fun': rho = params.dFun(h) elif params.dMode == 'table': rho = getRho_from_table(params.atmdat, h) else: sys.exit('atm mode not recognized') wvec_S = getWind(h) vInfvec_S = vvec_S + wvec_S vInf = norm(vInfvec_S) vInfvec_N = NS @ vInfvec_S # Lmag = 1/2 * rho * (vInf*1e3)**2 * params.CL * params.A / 1e3 # Dmag = 1/2 * rho * (vInf*1e3)**2 * params.CD * params.A / 1e3 Dmag = rho * (vInf * 1e3)**2 / (2 * params.BC) / 1e3 Lmag = Dmag * params.LD hvec_N = cross(xvec_N, vInfvec_N) Lupvec_N = cross(vInfvec_N, hvec_N) LupvecU_N = Lupvec_N / norm(Lupvec_N) # TODO - following code check is a little sketchy. can improve w/ 6 DOF c1 = cross(xvec_N, params.v0) c2 = cross(xvec_N, vInfvec_N) # if we are "flying upside down" if np.dot(c1, c2) < 0: # then change the sign on the lift vector to be upside down LupvecU_N = -LupvecU_N # print(LupvecU_N) # print(t) vInfvecU_N = vInfvec_N / norm(vInfvec_N) LvecU_N = LupvecU_N * np.cos(np.radians(sig)) + \ cross(vInfvecU_N, LupvecU_N) * np.sin(np.radians(sig)) DvecU_N = -vInfvecU_N aLvec_N = Lmag * LvecU_N aDvec_N = Dmag * DvecU_N dydt[0] = dx dydt[1] = dy dydt[2] = dz dydt[3:6] = (agvec_N + aLvec_N + aDvec_N) if returnForces: return agvec_N * params.m, aLvec_N, aDvec_N elif returnAccelerations: return agvec_N, aLvec_N, aDvec_N else: return dydt
# ============================================================================= # Compute target parameters for FNPEG from probe # ============================================================================= # compute Mach number profile rvec = outsProbe.rvec_N vvec = outsProbe.vvec_N h = np.linalg.norm(rvec, axis=0) - params.p.rad vmag = np.linalg.norm(vvec, axis=0) # get density and speed of sound at each altitude step rho = [] assfun = interp.interp1d(params.p.sound[0, :], params.p.sound[1, :]) ass = assfun(h) for hi in h: rho.append(getRho_from_table(params.atmdat, hi)) rho = np.asarray(rho) # compute mach number and dynamic pressure at each step Mvec = vmag * 1e3 / ass Qinc = 1 / 2 * rho * (vmag * 1e3)**2 # compute range lon0 = np.radians(outsProbe.lon0) lonf = np.radians(outsProbe.lonf) lat0 = np.radians(outsProbe.lat0) latf = np.radians(outsProbe.latf) dlon = abs(lonf - lon0) dsig = np.arccos(np.sin(lat0) * np.sin(latf)\ + np.cos(lat0) * np.cos(latf) * np.cos(dlon))
def mainAD(params, tspan, events, outs, verbose=True): ### CONVERT GIVEN INPUT TO INERTIAL VECTORS (and other input types as well) # NOTE - no matter what the input type is, vectors should be in N frame if params.inputType == 'inertial vectors': params.vInfvec_N = VN2Vinf(params.x0, params.v0, params, tspan[0]) params.lat, params.lon, params.alt, params.efpa, params.hda,\ params.vmag = RV2LLAEHV(params.x0, params.v0, params, tspan[0]) _, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \ RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0]) elif params.inputType == 'wind-relative vectors': params.v0 = Vinf2VN(params.x0, params.vInfvec_N, params, tspan[0]) params.lat, params.lon, params.alt, params.efpa, params.hda,\ params.vmag = RV2LLAEHV(params.x0, params.v0, params, tspan[0]) _, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \ RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0]) elif params.inputType == 'inertial angles': params.x0, params.v0 = LLAEHV2RV(params.lat, params.lon, params.alt, params.efpa, params.hda, params.vmag, params, tspan[0]) params.vInfvec_N = VN2Vinf(params.x0, params.v0, params, tspan[0]) _, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \ RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0]) elif params.inputType == 'wind-relative angles': params.x0, params.vInfvec_N = LLAEHV2RV(params.lat, params.lon, params.alt, params.efpaWR, params.hdaWR, params.vmagWR, params, tspan[0]) params.v0 = Vinf2VN(params.x0, params.vInfvec_N, params, tspan[0]) _, _, _, params.efpa, params.hda, params.vmag = \ RV2LLAEHV(params.x0, params.v0, params, tspan[0]) else: sys.exit('input type not recognized') ### CALL SIMRUN sol = simRun(params, tspan, events, verbose=verbose) rvec_N = sol.y[0:3, :] vvec_N = sol.y[3:6, :] ### GET OUTPUT PARAMS OF INTEREST # final inertial state rfvec_N = rvec_N[:, -1] vfvec_N = vvec_N[:, -1] tf = sol.t[-1] # convert final state params (these are inertial) lat, lon, alt, fpa, hda, vmag = RV2LLAEHV(rfvec_N, vfvec_N, params, tf) ## Compute peak heat rate, add to output # get airspeed at each time, vInf vInfvec_N = [] for i in range(vvec_N.shape[1]): vInfvec_N.append(VN2Vinf(rvec_N[:, i], vvec_N[:, i], params, sol.t[i])) vInfvec_N = np.asarray(vInfvec_N).T vInf = np.linalg.norm(vInfvec_N, axis=0) # get density at each time r = np.linalg.norm(rvec_N, axis=0) h = r - params.p.rad if params.dMode == 'fun': rho = params.dFun(h) elif params.dMode == 'table': rho = getRho_from_table(params.atmdat, h) else: sys.exit('atm mode not recognized') # calculate S-G heat rate without coefficient, SG SG = [] for i in range(len(rho)): SG.append(np.sqrt(rho[i] / params.Rn) * vInf[i]**3) SG = np.asarray(SG) SGpeak = SG.max() # calculate peak heat rate qpeak = params.p.k * SGpeak * 1e5 # puts q in W/cm^2 units q = params.p.k * SG * 1e5 # now integrate numerically to get heat load Qload = trapz(q, sol.t) # J / cm^2 ## Compute max g, add to output # run through dynamics again to get forces output agvec_N = np.empty((3, len(sol.t))) agvec_N[:] = np.NaN aLvec_N = np.empty((3, len(sol.t))) aLvec_N[:] = np.NaN aDvec_N = np.empty((3, len(sol.t))) aDvec_N[:] = np.NaN for i, (ti, xi, vi) in enumerate(zip(sol.t, rvec_N.T, vvec_N.T)): yyi = np.block([xi, vi]) agvec_N[:, i], aLvec_N[:, i], aDvec_N[:, i] = ODEs.dynamics( ti, yyi, params.bank, params, returnAccelerations=True) # compute g-load at each time gload = np.linalg.norm(aLvec_N + aDvec_N, axis=0)\ / (constants.G0 / 1e3) # divide g0 by 1000 to get it in km/s^2 units gpeak = gload.max() ## Compute apoapsis at final time, add to output vf = np.linalg.norm(vfvec_N) rf = np.linalg.norm(rfvec_N) engf = vf**2 / 2 - params.p.mu / rf hfvec_N = np.cross(rfvec_N, vfvec_N) hf = np.linalg.norm(hfvec_N) af = -params.p.mu / (2 * engf) eccf = np.sqrt(1 + 2 * engf * hf**2 / params.p.mu**2) raf = af * (1 + eccf) haf = raf - params.p.rad ### ASSIGN OUTPUTS TO outs CLASS # initial state outs.lat0 = params.lat outs.lon0 = params.lon outs.alt0 = params.alt outs.efpa0 = params.efpa outs.hda0 = params.hda outs.vmag0 = params.vmag outs.efpaWR0 = params.efpaWR outs.hdaWR0 = params.hdaWR outs.vmagWR0 = params.vmagWR outs.rvec_N0 = params.x0 outs.vvec_N0 = params.v0 outs.vInfvec_N0 = params.vInfvec_N outs.tspan = tspan # vehicle outs.BC = params.BC outs.Rn = params.Rn outs.bank = params.bank # final state outs.latf = lat outs.lonf = lon outs.altf = alt outs.fpaf = fpa outs.hdaf = hda outs.vmagf = vmag outs.rvec_Nf = rfvec_N outs.vvec_Nf = vfvec_N outs.raf = raf outs.haf = haf outs.engf = engf outs.af = af outs.eccf = eccf outs.t = tf # peak values and total loads outs.SGpeak = SGpeak outs.qpeak = qpeak outs.Qload = Qload outs.gpeak = gpeak if params.returnTimeVectors: outs.rvec_N = rvec_N outs.vvec_N = vvec_N outs.tvec = sol.t outs.q = q outs.gload = gload return outs
''' # get index of first element in altitude array great than h ind = np.argmax(params.atmdat[0,:] > h) x0 = params.atmdat[0, ind-1] x1 = params.atmdat[0, ind] y0 = params.atmdat[1, ind-1] y1 = params.atmdat[1, ind] rho = (y0 * (x1 - h) + y1 * (h - x0)) / (x1 - x0) return rho h = 96.15498715 tic1 = time.time() rho1 = getRho_from_table(params.atmdat, h) toc1 = time.time() tic2 = time.time() rho2 = myinterp(h, params) toc2 = time.time() tic3 = time.time() rho3 = scipyfun(h) toc3 = time.time() print(rho1) print(rho2) print(rho3) # NOTE: just do timeit _____ for each rho command, in the IPython prompts