Quick refactor and load GPS data
This commit is contained in:
Cal Wing
parent
7e38a29bd7
commit
d79ff282be
204
main.py
204
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")
|
||||
Loading…
x
Reference in New Issue
Block a user