# Aero4200-Ass3 # Cal Wing, Sem 2 2023 # Import System / Common Libs import os, pickle import numpy as np import pandas as pd from tqdm import tqdm #from scipy.optimize import curve_fit 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]) INIT_FLIGHT_PRAMS = { "position": np.array([10, 10, -50]), "velocity": np.array([0, 0, 0]), "attitude": np.array([0, 0, 0]) } # 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 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 = 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 Truth Data TRUTH_N_POS_HEADER = ["Time [s]", "True North Position [m]"] TRUTH_FSP_X_HEADER = ["Time [s]", "True FSP_X [m/s^2]"] m1TruthData = pd.read_csv( f"./data/N_truth_IMU_M1L.txt", header=None, skiprows=1, names=TRUTH_N_POS_HEADER, ), pd.read_csv( f"./data/N_truth_IMU_M1H.txt", header=None, skiprows=1, names=TRUTH_N_POS_HEADER, ), pd.read_csv( f"./data/FSPB1_truth_IMU_M1.txt", header=None, skiprows=1, names=TRUTH_FSP_X_HEADER, ) # Load the GPS Data GPS_NED_POS_HEADER = ["GPS Position N [m]", "GPS Position E [m]", "GPS Position D [m]"] GPS_DATA_HEADER = TIME_HEADER + GPS_NED_POS_HEADER m2_GPSData = pd.read_csv( f"./data/GPS_5_Hz_IMU_M2.txt", header=None, skiprows=1, names=GPS_DATA_HEADER, ), pd.read_csv( f"./data/GPS_0.5_Hz_IMU_M2.txt", header=None, skiprows=1, names=GPS_DATA_HEADER, ) # 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(attitude, euler_angles): phi, theta, psi = euler_angles p, q, r = attitude 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 = TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NED_VELOCITY_HEADER + NED_POSITION_HEADER def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.DataFrame: print("Translating Motion & Calculating Resulting Forces") # By default this is a reference to an object, as we update it it needs to be a copy to not modify the original. offset = initial_values.copy() translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER, dtype=float) for i in tqdm(range(len(missionData))): mData = lambda header, j=i: missionData.loc[j, header].values # Easy value extraction # dt = t[i] - t[i-1] BUT for t == 0; dt = t[i+1] - t[i] dt = mData(TIME_HEADER, i)[0] - mData(TIME_HEADER, i-1)[0] if i > 0 else mData(TIME_HEADER, i+1)[0] - mData(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"] # If we have GPS data prefer that. # TL;DR If GPS Data at time point use that else use above calculated position if gpsData is not None and gpsData[TIME_HEADER].isin([mData(TIME_HEADER, i)[0]]).any().any(): timeIdx = gpsData[TIME_HEADER].isin([mData(TIME_HEADER, i)[0]]).idxmax() NED_position = gpsData.loc[timeIdx, GPS_NED_POS_HEADER[0:]].values[0] # Update the offsets offset["attitude"] = NED_attitude offset["velocity"] = NED_velocity offset["position"] = NED_position # Add the data to the translated frame translatedData.loc[i, TIME_HEADER] = mData(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 def generateGraphs(missionData, mission, gpsData=None): pBar = tqdm(total=7) # Graph just the R-P-Y graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, "figSize": (8,12), #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][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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) # Graph the position of the UAV (Without 3D Plots) graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, "figSize": (8, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,1), "grid": True, "tightLayout": True, "subPlots":[ { "title": "North Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][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][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][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][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, ] } if TRUTH_N_POS_HEADER[1] in missionData[1].columns: graphData["subPlots"][0]["plots"].insert(0, {"x": missionData[1][TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) if gpsData is not None: graphData["subPlots"][0]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][1]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][2]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png") pBar.update(1) # Graph the position of the UAV (With 3D Plots) graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, "figSize": (12, 15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,2), "grid": True, "subPlots":[ { "title": "North Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "UAV Flight Path", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "East Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "UAV Flight Path - Low Grade Data", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, #{"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Down Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "UAV Flight Path - High Grade Data", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ #{"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, ] } if TRUTH_N_POS_HEADER[1] in missionData[1].columns: graphData["subPlots"][0]["plots"].insert(0, {"x": missionData[1][TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) if gpsData is not None: graphData["subPlots"][0]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][2]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][4]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][1]["plots"].append({"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][3]["plots"].append({"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][5]["plots"].append({"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d.png") pBar.update(1) # Graph Just the velocity graphData = { "figTitle": f"Mission {mission} Flight Velocity", "figTitleFontSize": 16, "figSize": (8, 12), #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][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][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][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][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][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][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) # Graph just the acceleration graphData = { "figTitle": f"Mission {mission} Flight Acceleration", "figTitleFontSize": 16, "figSize": (8, 12), #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][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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) # Graph the position, velocity & acceleration on one big graph graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, "figSize": (20, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,3), "grid": True, "subPlots":[ { "title": "North Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "North Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "North Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "East Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "East Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "East Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Down Position", "yLabel": "Position [m]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "Down Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "Down Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, ] } if TRUTH_N_POS_HEADER[1] in missionData[1].columns: graphData["subPlots"][0]["plots"].insert(0, {"x": missionData[1][TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) if gpsData is not None: graphData["subPlots"][0]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][3]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"}) graphData["subPlots"][6]["plots"].append({"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_chars.png") pBar.update(1) # Graph the R-P-Y, velocity & acceleration on one big graph graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, "figSize": (20, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,3), "grid": True, "subPlots":[ { "title": "Roll Rate", "yLabel": "Roll Rate [rad/s]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[0]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "North Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "North Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Pitch Rate", "yLabel": "Pitch Rate [rad/s]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[1]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "East Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "East Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, ] }, { "title": "Yaw Rate", "yLabel": "Yaw Rate [rad/s]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][IMU_WBE_HEADERS[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][IMU_WBE_HEADERS[2]], "label":"High Grade Data", "colour":"uq:blue"},\ ] }, { "title": "Down Velocity", "yLabel": "Velocity [m/s]", "plots": [ {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_VELOCITY_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_VELOCITY_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, ] }, { "title": "Down Acceleration", "yLabel": "Acceleration [m/s$^2$]", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][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_chars_rpy.png") pBar.update(1) TRUTH_NPOS_DELTA_HEADER = ["Calculated vs True Position Delta [m]", "Calculated vs True Position Error [%]"] def doTruthComparison(missionData, truthData): for i, mData in enumerate(missionData): # Add & calculate the error of each position to the dataframe mData[TRUTH_N_POS_HEADER[1]] = truthData[i][TRUTH_N_POS_HEADER[1]] mData[TRUTH_NPOS_DELTA_HEADER[0]] = mData[NED_POSITION_HEADER[0]] - mData[TRUTH_N_POS_HEADER[1]] mData[TRUTH_NPOS_DELTA_HEADER[1]] = np.abs(mData[TRUTH_NPOS_DELTA_HEADER[0]]) / mData[TRUTH_N_POS_HEADER[1]] * 100 # Correct the DataType in the injected column mData[mData.columns] = mData[mData.columns].astype(float) return missionData def generateTruthErrorGraphs(missionData): # Use polyfit to generate the 2nd order polynomial to best fit the error data poly_low = np.polyfit(missionData[0][TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[0]], 2) poly_high = np.polyfit(missionData[1][TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[0]], 2) graphData = { "grid": True, "xLabel": "Time [s]", "yLabel": "Position Delta [m]", "title": "Calculated vs True North Position Delta", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y": missionData[0][TRUTH_NPOS_DELTA_HEADER[0]], "label": "Low Grade IMU Error"}, {"x":missionData[1][TIME_HEADER[0]], "y": missionData[1][TRUTH_NPOS_DELTA_HEADER[0]], "label": "High Grade IMU Error"}, {"x":missionData[0][TIME_HEADER[0]], "y": np.polyval(poly_low, missionData[0][TIME_HEADER[0]]), "label": "Low Grade Best Fit Curve"}, {"x":missionData[1][TIME_HEADER[0]], "y": np.polyval(poly_high, missionData[1][TIME_HEADER[0]]), "label": "High Grade Best Fit Curve"}, ] } makeGraph(graphData, False, False, figSavePath="./images/m1_error_delta.png") graphData = { "grid": True, "xLabel": "Time [s]", "yLabel": "Position Error [%]", "title": "Calculated vs True North Position Error", "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y": np.abs(missionData[0][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "Low Grade IMU Error"}, {"x":missionData[1][TIME_HEADER[0]], "y": np.abs(missionData[1][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "High Grade IMU Error"}, ] } makeGraph(graphData, False, False, figSavePath="./images/m1_error_percent.png") return poly_low, poly_high def generateGPSGraphs(missionData, gpsMissionData, gpsData, mission): graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, "figSize": (12, 15), #Yay America, this is in inches :/ # Note: cm = 1/2.54 "yLabel": "Time [s]", "plotDim": (3,2), "grid": True, "subPlots":[ { "title": "North Position", "yLabel": "Position [m]", "plots": [ {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[0]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"}, {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[0]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"}, ] }, { "title": "UAV Flight Path", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ {"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"}, {"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"}, {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}, ] }, { "title": "East Position", "yLabel": "Position [m]", "plots": [ {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"}, {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"}, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"}, ] }, { "title": "UAV Flight Path - Low Grade Data", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ {"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"}, {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}, ] }, { "title": "Down Position", "yLabel": "Position [m]", "plots": [ {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"}, {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"}, {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"}, {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}, ] }, { "title": "UAV Flight Path - High Grade Data", "xLabel": "Noth Position [m]", "yLabel": "East Position [m]", "zLabel": "Down Position [m]", "plots": [ {"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"}, {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"}, {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}, ] }, ] } makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d_gps_all.png") if __name__ == '__main__': # Task 1 - b, c, d #Load, Process & Graph Mission Data for Mission 1 print("Loading / Calculating to Mission 1 Data") 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) missionData = doTruthComparison(missionData, m1TruthData) print("Starting to Graph Mission 1") #generateGraphs(missionData, 1) # Task 2 - b errorPoly = generateTruthErrorGraphs(missionData) for i, poly in enumerate(errorPoly): string = "" for n, c_n in enumerate(poly): string += f" + {c_n:.4f}*t^{len(poly)-1-n}" print(f"{'High' if i else 'Low'} Grade Error Poly:" + string[2:]) # Task 3 t_15_data = m1TruthData[0][TIME_HEADER + [TRUTH_N_POS_HEADER[1]]].iloc[(m1TruthData[0][TIME_HEADER[0]]-15).abs().argsort()[:1]].values, \ m1TruthData[2][TIME_HEADER + [TRUTH_FSP_X_HEADER[1]]].iloc[(m1TruthData[2][TIME_HEADER[0]]-15).abs().argsort()[:1]].values t_15_data = pd.DataFrame({ TIME_HEADER[0] : [t_15_data[0][0][0]], TRUTH_N_POS_HEADER[1] : [t_15_data[0][0][1]], TRUTH_FSP_X_HEADER[1] : [t_15_data[1][0][1]] }) print(t_15_data) # Task 4 #Load, Process & Graph Mission Data for Mission 2 print("\nLoading / Calculating to Mission 2 Data") missionData_4 = cacheData("./tmp/m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \ cacheData("./tmp/m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False) print("Starting to Graph Mission 2") #generateGraphs(missionData_4, 2) # Task 5 # Calc 5hz Mission missionData = cacheData("./tmp/gps_5_m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS, m2_GPSData[0]), forceCalc=False), \ cacheData("./tmp/gps_5_m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS, m2_GPSData[0]), forceCalc=False) #generateGraphs(missionData, "2 - 5Hz GPS", m2_GPSData[0]) # Calc 0.5hz Mission missionData = cacheData("./tmp/gps_0.5_m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS, m2_GPSData[1]), forceCalc=False), \ cacheData("./tmp/gps_0.5_m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS, m2_GPSData[1]), forceCalc=False) generateGPSGraphs(missionData_4, missionData, m2_GPSData[0], "2 - 0.5Hz GPS Correction & Comparison") #generateGraphs(missionData, "2 - 0.5Hz GPS", m2_GPSData[1]) print("Complete")