diff --git a/main.py b/main.py index 6747818..45e77af 100644 --- a/main.py +++ b/main.py @@ -68,6 +68,11 @@ def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tup 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 @@ -79,10 +84,10 @@ GRAVITY_VEC = np.array([0, 0, 9.81]) # - 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]"] +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 +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" @@ -111,6 +116,18 @@ m1TruthData = pd.read_csv( 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) @@ -147,7 +164,7 @@ def calc_NED_acceleration(NEDAttitude, specificForces): 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 +TRANS_DATA_HEADER = 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") @@ -156,7 +173,7 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: 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 - 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] + 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] @@ -172,7 +189,7 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: offset["velocity"] = NED_velocity offset["position"] = NED_position - translatedData.loc[i, IMU_TIME_HEADER] = mData(IMU_TIME_HEADER) + 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 @@ -180,12 +197,6 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: 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=7) graphData = { @@ -200,24 +211,24 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"},\ + {"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"},\ ] }, ] @@ -238,30 +249,30 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"}, + {"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][IMU_TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) + 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"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png") pBar.update(1) @@ -277,8 +288,8 @@ def generateGraphs(missionData, mission): "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"}, + {"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"}, ] }, { @@ -295,8 +306,8 @@ def generateGraphs(missionData, mission): "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"}, + {"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"}, ] }, { @@ -313,8 +324,8 @@ def generateGraphs(missionData, mission): "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"}, + {"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"}, ] }, { @@ -330,7 +341,7 @@ def generateGraphs(missionData, mission): ] } if TRUTH_N_POS_HEADER[1] in missionData[1].columns: - graphData["subPlots"][0]["plots"].insert(0, {"x": missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) + 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"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d.png") pBar.update(1) @@ -346,24 +357,24 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"}, + {"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"}, ] }, ] @@ -383,24 +394,24 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"}, + {"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"}, ] }, ] @@ -420,78 +431,78 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][IMU_TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"}) + 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"}) makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_chars.png") pBar.update(1) @@ -507,72 +518,72 @@ def generateGraphs(missionData, mission): "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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"}, + {"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][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"},\ + {"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][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"}, + {"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][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"}, + {"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"}, ] }, ] @@ -580,7 +591,6 @@ def generateGraphs(missionData, mission): 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): @@ -597,34 +607,34 @@ def generateTruthErrorGraphs(missionData): #print(missionData[0][[IMU_TIME_HEADER[0]] + [NED_POSITION_HEADER[0]] + [TRUTH_N_POS_HEADER[1]] + [TRUTH_NPOS_DELTA_HEADER[0]]]) - poly_low = np.polyfit(missionData[0][IMU_TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[0]], 2) - poly_high = np.polyfit(missionData[1][IMU_TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[0]], 2) + 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][IMU_TIME_HEADER[0]], "y": missionData[0][TRUTH_NPOS_DELTA_HEADER[0]], "label": "Low Grade IMU Error"}, - {"x":missionData[1][IMU_TIME_HEADER[0]], "y": missionData[1][TRUTH_NPOS_DELTA_HEADER[0]], "label": "High Grade IMU Error"}, - {"x":missionData[0][IMU_TIME_HEADER[0]], "y": np.polyval(poly_low, missionData[0][IMU_TIME_HEADER[0]]), "label": "Low Grade Best Fit Curve"}, - {"x":missionData[1][IMU_TIME_HEADER[0]], "y": np.polyval(poly_high, missionData[1][IMU_TIME_HEADER[0]]), "label": "High Grade Best Fit Curve"}, + {"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") - poly_low_perc = np.polyfit(missionData[0][IMU_TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[1]], 2) - poly_high_perc = np.polyfit(missionData[1][IMU_TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[1]], 2) + poly_low_perc = np.polyfit(missionData[0][TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[1]], 2) + poly_high_perc = np.polyfit(missionData[1][TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[1]], 2) graphData = { "grid": True, "xLabel": "Time [s]", "yLabel": "Position Error [%]", "title": "Calculated vs True North Position Error", "plots": [ - {"x":missionData[0][IMU_TIME_HEADER[0]], "y": np.abs(missionData[0][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "Low Grade IMU Error"}, - {"x":missionData[1][IMU_TIME_HEADER[0]], "y": np.abs(missionData[1][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "High Grade IMU Error"}, + {"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"}, #{"x":missionData[0][IMU_TIME_HEADER[0]], "y": np.polyval(poly_low_perc, missionData[0][IMU_TIME_HEADER[0]]), "label": "Low Grade Best Fit Curve"}, #{"x":missionData[1][IMU_TIME_HEADER[0]], "y": np.polyval(poly_high_perc, missionData[1][IMU_TIME_HEADER[0]]), "label": "High Grade Best Fit Curve"}, @@ -655,10 +665,10 @@ if __name__ == '__main__': print(f"{'High' if i else 'Low'} Grade Error Poly:" + string[2:]) # Task 3 - t_15_data = m1TruthData[0][IMU_TIME_HEADER + [TRUTH_N_POS_HEADER[1]]].iloc[(m1TruthData[0][IMU_TIME_HEADER[0]]-15).abs().argsort()[:1]].values, \ - m1TruthData[2][IMU_TIME_HEADER + [TRUTH_FSP_X_HEADER[1]]].iloc[(m1TruthData[2][IMU_TIME_HEADER[0]]-15).abs().argsort()[:1]].values + 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({ - IMU_TIME_HEADER[0] : [t_15_data[0][0][0]], + 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]] }) @@ -670,9 +680,11 @@ if __name__ == '__main__': missionData = 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, 2) + #generateGraphs(missionData, 2) + + # Task 5 + - print("Complete") \ No newline at end of file