AERO4200-Ass3/main.py

# 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")