Add comments for style
This commit is contained in:
Cal Wing
parent
c38791de57
commit
272f32794f
30
main.py
30
main.py
@ -2,7 +2,7 @@
|
||||
# Cal Wing, Sem 2 2023
|
||||
|
||||
# Import System / Common Libs
|
||||
import os, pickle, time
|
||||
import os, pickle
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
from tqdm import tqdm
|
||||
@ -168,11 +168,13 @@ TRANS_DATA_HEADER = TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NE
|
||||
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
|
||||
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
|
||||
@ -185,14 +187,18 @@ def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.D
|
||||
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
|
||||
@ -203,6 +209,7 @@ def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.D
|
||||
|
||||
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,
|
||||
@ -240,6 +247,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -284,6 +292,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -360,6 +369,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -397,6 +407,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -434,6 +445,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -525,6 +537,7 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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,
|
||||
@ -613,21 +626,22 @@ def generateGraphs(missionData, mission, gpsData=None):
|
||||
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):
|
||||
|
||||
#print(missionData[0][[IMU_TIME_HEADER[0]] + [NED_POSITION_HEADER[0]] + [TRUTH_N_POS_HEADER[1]] + [TRUTH_NPOS_DELTA_HEADER[0]]])
|
||||
|
||||
# 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]",
|
||||
@ -641,11 +655,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)
|
||||
graphData = {
|
||||
"grid": True,
|
||||
"xLabel": "Time [s]",
|
||||
@ -654,9 +665,6 @@ def generateTruthErrorGraphs(missionData):
|
||||
"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"},
|
||||
#{"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"},
|
||||
|
||||
]
|
||||
}
|
||||
makeGraph(graphData, False, False, figSavePath="./images/m1_error_percent.png")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user