# Aero4200-Ass3 # Cal Wing, Sem 2 2023 # Import System / Common Libs import os, pickle, time import numpy as np import pandas as pd from tqdm import tqdm from numpy import pi, sin, cos, tan #Import makeGraph, a custom graphing wrapper I developed, refer to it for documentation from makeGraph import makeGraph, pltKeyClose, UQ_COLOURS as UQC # Custom Graphing Lib # Make sure the relevant folders folder exists folders = ["./images", "./tmp"] for folder in folders: if not os.path.isdir(folder): os.mkdir(folder) # This is a cacheing function, it checks if a cache file exists and loads it or it calcs & saves it. # TL;DR This takes a file path & either a single variable (defArg) or some sort of callable and either # loads it or dumps it to disk. Note: It does not validate the data inside! def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tuple = (), kargs: dict = {}, defArg=None, forceCalc=False, doFileDelete=False): if len(args) == 0 and defArg is not None: args = (defArg, ) data = None dataFileExt = dataFilePath.rsplit(".")[-1] # Check if file exists if os.path.isfile(dataFilePath) and not forceCalc: print(f"Found data file \"{dataFilePath}\", loading data.") # Check if file is a compressed numpy file if dataFileExt == "npz": data = np.load(dataFilePath, allow_pickle=True)["arr_0"] elif dataFileExt == "pkl": #If its not then just pickle it normally with open(dataFilePath, 'rb') as handle: data = pickle.load(handle) elif dataFileExt == "dfz": data = pd.read_pickle(dataFilePath, compression="gzip") else: raise TypeError(f"Cannot determine file type of: {dataFilePath}") else: if doFileDelete and os.path.isfile(dataFilePath): print(f"Found data file \"{dataFilePath}\", deleting data.") os.remove(dataFilePath) print(f"Could not find data file \"{dataFilePath}\", generating & saving data.") # Calculate Value data = calcFunction(*args, **kargs) # Check if file is a compressed numpy file if dataFileExt == "npz": np.savez_compressed(dataFilePath, data, allow_pickle=True) elif dataFileExt == "dfz": data.to_pickle(dataFilePath, compression="gzip") elif dataFileExt == "pkl": #If its not then just pickle it normally with open(dataFilePath, 'wb') as handle: pickle.dump(data, handle) else: raise TypeError(f"Cannot determine file type of: {dataFilePath}") if data is None: raise ValueError("Could not import or generate data requested") return data GRAVITY_VEC = np.array([0, 0, 9.81]) # IMU Data Loading # I map in to a heading to add units / make things make more sense ## The gyroscopic body angular rates from the IMU are given: # - WBE_1 (in rad/s) - the roll rate about the body-fixed x-axis # - WBE_2 (in rad/s) - the pitch rate about the body-fixed y-axis # - WBE_3 (in rad/s) - the yaw rate about the body-fixed z-axis ## Specific forces: # - FSP_X (in m/s^2) - the specific force in the body-fixed x-direction # - FSP_Y (in m/s^2) - the specific force in the body-fixed y-direction # - FSP_Z (in m/s^2) - the specific force in the body-fixed z-direction IMU_TIME_HEADER = ["Time [s]"] IMU_WBE_HEADERS = ["WBE_1 [rad/s]", "WBE_2 [rad/s]", "WBE_3 [rad/s]"] # Roll, Pitch, Yaw - Rate IMU_FSP_HEADERS = ["FSP_X [m/s^2]", "FSP_Y [m/s^2]", "FSP_Z [m/s^2]"] # Specific Force X, Y , Z IMU_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + IMU_FSP_HEADERS def importIMUData(mission, imu): # If IMU is not a string, then convert based on bool eval where "H" == True if type(imu) != str: imu = "H" if imu else "L" data = pd.read_csv( f"./data/IMU_M{str(mission) + str(imu)}.txt", header=None, skiprows=1, names=IMU_DATA_HEADER, ) return data # Load the Mission Data m1_IMUData = importIMUData(1, 0), importIMUData(1, 1) #(L, H) Data m2_IMUData = importIMUData(2, 0), importIMUData(2, 1) # NED Translation & Force Functions INIT_EULER_ANGLES = (0, 0, 0) def attitude_rate_2NED(angles, euler_angles): phi, theta, psi = euler_angles p, q, r = angles transMat = np.array([ [1, sin(phi)*tan(theta), cos(phi)*tan(theta) ], [0, cos(phi), -sin(phi) ], [0, sin(phi)/cos(theta), cos(phi)/cos(theta) ] ]) angleMat = np.array([ [p], [q], [r] ]) return np.matmul(transMat, angleMat).flatten() def calc_NED_acceleration(NEDAttitude, specificForces): phi, theta, psi = NEDAttitude # Roll, Pitch, Yaw R41 = np.array([ [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)], [sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)], [-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi) ] ]) return np.matmul(R41, specificForces) + GRAVITY_VEC # Calculate Mission Translation & Force Data NED_ACCELERATION_HEADER = ["Acceleration N [m/s^2]", "Acceleration E [m/s^2]", "Acceleration D [m/s^2]"] NED_VELOCITY_HEADER = ["Velocity N [m/s]", "Velocity E [m/s]", "Velocity D [m/s]"] NED_POSITION_HEADER = ["Position N [m]", "Position E [m]", "Position D [m]"] TRANS_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NED_VELOCITY_HEADER + NED_POSITION_HEADER def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: print("Translating Motion & Calculating Resulting Forces") offset = initial_values.copy() translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER) for i in tqdm(range(len(missionData))): mData = lambda header, j=i: missionData.loc[j, header].values dt = mData(IMU_TIME_HEADER, i)[0] - mData(IMU_TIME_HEADER, i-1)[0] if i > 0 else mData(IMU_TIME_HEADER, i+1)[0] - mData(IMU_TIME_HEADER, i)[0] # Get the time point data dataPoint = missionData.loc[i, IMU_WBE_HEADERS] # Translate to NED Frame NED_attitude_rate = attitude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES) NED_attitude = NED_attitude_rate * dt + offset["attitude"] NED_acceleration = calc_NED_acceleration(NED_attitude, mData(IMU_FSP_HEADERS)) NED_velocity = NED_acceleration * dt + offset["velocity"] NED_position = NED_velocity * dt + offset["position"] offset["attitude"] = NED_attitude offset["velocity"] = NED_velocity offset["position"] = NED_position translatedData.loc[i, IMU_TIME_HEADER] = mData(IMU_TIME_HEADER) translatedData.loc[i, IMU_WBE_HEADERS] = NED_attitude_rate translatedData.loc[i, NED_ACCELERATION_HEADER] = NED_acceleration translatedData.loc[i, NED_VELOCITY_HEADER] = NED_velocity translatedData.loc[i, NED_POSITION_HEADER] = NED_position return translatedData INIT_FLIGHT_PRAMS = { "position": np.array([10, 10, -50]), "velocity": np.array([0, 0, 0]), "attitude": np.array([0, 0, 0]) } def generateGraphs(missionData, mission): pBar = tqdm(total=4) graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, "figSize": (15,20), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,1), "grid": True, "subPlots":[ { "title": "Roll Rate", "yLabel": "Roll Rate [rad/s]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[0]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Pitch Rate", "yLabel": "Pitch Rate [rad/s]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[1]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Yaw Rate", "yLabel": "Yaw Rate [rad/s]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[2]], "label":"High Grade Data", "colour":"uq:blue"},\ ] }, ] } makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_rpy.png") pBar.update(1) graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,1), "grid": True, "subPlots":[ { "title": "North Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "East Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "Down Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, ] } makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png") pBar.update(1) graphData = { "figTitle": f"Mission {mission} Flight Velocity", "figTitleFontSize": 16, "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,1), "grid": True, "subPlots":[ { "title": "North Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "East Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "Down Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, ] } makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_velocity.png") pBar.update(1) graphData = { "figTitle": f"Mission {mission} Flight Acceleration", "figTitleFontSize": 16, "figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,1), "grid": True, "subPlots":[ { "title": "North Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "East Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Down Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, ] } makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_accell.png") pBar.update(1) if __name__ == '__main__': missionData = cacheData("./tmp/m1_L_transData.dfz", calculateVelocityPosition, (m1_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \ cacheData("./tmp/m1_H_transData.dfz", calculateVelocityPosition, (m1_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False) generateGraphs(missionData, 1) print("Complete")