diff --git a/main.py b/main.py index 45e77af..ac8a6ce 100644 --- a/main.py +++ b/main.py @@ -165,7 +165,7 @@ NED_ACCELERATION_HEADER = ["Acceleration N [m/s^2]", "Acceleration E [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) -> pd.DataFrame: +def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.DataFrame: print("Translating Motion & Calculating Resulting Forces") offset = initial_values.copy() @@ -185,6 +185,10 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: NED_velocity = NED_acceleration * dt + offset["velocity"] NED_position = NED_velocity * dt + offset["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] + offset["attitude"] = NED_attitude offset["velocity"] = NED_velocity offset["position"] = NED_position @@ -197,7 +201,7 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame: return translatedData -def generateGraphs(missionData, mission): +def generateGraphs(missionData, mission, gpsData=None): pBar = tqdm(total=7) graphData = { "figTitle": f"Mission {mission} Flight Characteristics", @@ -273,6 +277,10 @@ def generateGraphs(missionData, mission): } 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) @@ -342,6 +350,13 @@ def generateGraphs(missionData, mission): } 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) @@ -503,6 +518,10 @@ def generateGraphs(missionData, mission): } 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) @@ -625,8 +644,8 @@ def generateTruthErrorGraphs(missionData): makeGraph(graphData, False, False, figSavePath="./images/m1_error_delta.png") - 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) + #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]", @@ -642,8 +661,82 @@ def generateTruthErrorGraphs(missionData): } makeGraph(graphData, False, False, figSavePath="./images/m1_error_percent.png") - return (poly_low, poly_high), (poly_low_perc, poly_high_perc) + 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":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":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"RawGPS Data", "colour":"uq:red"}, + ] + }, + { + "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"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"}) + graphData["subPlots"][2]["plots"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"}) + graphData["subPlots"][4]["plots"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"}) + graphData["subPlots"][1]["plots"].insert(0, {"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") if __name__ == '__main__': # Task 1 - b, c, d @@ -656,7 +749,7 @@ if __name__ == '__main__': #generateGraphs(missionData, 1) # Task 2 - b - errorPoly = generateTruthErrorGraphs(missionData)[0] + errorPoly = generateTruthErrorGraphs(missionData) for i, poly in enumerate(errorPoly): string = "" for n, c_n in enumerate(poly): @@ -683,6 +776,15 @@ if __name__ == '__main__': #generateGraphs(missionData, 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) + generateGraphs(missionData, "2 - 0.5Hz GPS", m2_GPSData[1])