Before submittion cleanup
This commit is contained in:
Cal Wing
parent
d79ff282be
commit
c38791de57
114
main.py
114
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])
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user