807 lines
43 KiB
Python
807 lines
43 KiB
Python
# Aero4200-Ass3
|
|
# Cal Wing, Sem 2 2023
|
|
|
|
# Import System / Common Libs
|
|
import os, pickle
|
|
import numpy as np
|
|
import pandas as pd
|
|
from tqdm import tqdm
|
|
#from scipy.optimize import curve_fit
|
|
|
|
from numpy import pi, sin, cos, tan
|
|
|
|
#Import makeGraph, a custom graphing wrapper I developed, refer to it for documentation
|
|
from makeGraph import makeGraph, pltKeyClose, UQ_COLOURS as UQC # Custom Graphing Lib
|
|
|
|
# Make sure the relevant folders folder exists
|
|
folders = ["./images", "./tmp"]
|
|
for folder in folders:
|
|
if not os.path.isdir(folder): os.mkdir(folder)
|
|
|
|
# This is a cacheing function, it checks if a cache file exists and loads it or it calcs & saves it.
|
|
# TL;DR This takes a file path & either a single variable (defArg) or some sort of callable and either
|
|
# loads it or dumps it to disk. Note: It does not validate the data inside!
|
|
def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tuple = (), kargs: dict = {}, defArg=None, forceCalc=False, doFileDelete=False):
|
|
if len(args) == 0 and defArg is not None:
|
|
args = (defArg, )
|
|
data = None
|
|
dataFileExt = dataFilePath.rsplit(".")[-1]
|
|
|
|
# Check if file exists
|
|
if os.path.isfile(dataFilePath) and not forceCalc:
|
|
print(f"Found data file \"{dataFilePath}\", loading data.")
|
|
# Check if file is a compressed numpy file
|
|
if dataFileExt == "npz":
|
|
data = np.load(dataFilePath, allow_pickle=True)["arr_0"]
|
|
elif dataFileExt == "pkl":
|
|
#If its not then just pickle it normally
|
|
with open(dataFilePath, 'rb') as handle:
|
|
data = pickle.load(handle)
|
|
elif dataFileExt == "dfz":
|
|
data = pd.read_pickle(dataFilePath, compression="gzip")
|
|
else:
|
|
raise TypeError(f"Cannot determine file type of: {dataFilePath}")
|
|
else:
|
|
if doFileDelete and os.path.isfile(dataFilePath):
|
|
print(f"Found data file \"{dataFilePath}\", deleting data.")
|
|
os.remove(dataFilePath)
|
|
|
|
print(f"Could not find data file \"{dataFilePath}\", generating & saving data.")
|
|
# Calculate Value
|
|
data = calcFunction(*args, **kargs)
|
|
|
|
# Check if file is a compressed numpy file
|
|
if dataFileExt == "npz":
|
|
np.savez_compressed(dataFilePath, data, allow_pickle=True)
|
|
|
|
elif dataFileExt == "dfz":
|
|
data.to_pickle(dataFilePath, compression="gzip")
|
|
|
|
elif dataFileExt == "pkl":
|
|
#If its not then just pickle it normally
|
|
with open(dataFilePath, 'wb') as handle:
|
|
pickle.dump(data, handle)
|
|
else:
|
|
raise TypeError(f"Cannot determine file type of: {dataFilePath}")
|
|
|
|
if data is None: raise ValueError("Could not import or generate data requested")
|
|
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
|
|
## The gyroscopic body angular rates from the IMU are given:
|
|
# - WBE_1 (in rad/s) - the roll rate about the body-fixed x-axis
|
|
# - WBE_2 (in rad/s) - the pitch rate about the body-fixed y-axis
|
|
# - WBE_3 (in rad/s) - the yaw rate about the body-fixed z-axis
|
|
## Specific forces:
|
|
# - 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
|
|
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 = 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"
|
|
|
|
data = pd.read_csv(
|
|
f"./data/IMU_M{str(mission) + str(imu)}.txt",
|
|
header=None, skiprows=1,
|
|
names=IMU_DATA_HEADER,
|
|
)
|
|
|
|
return data
|
|
|
|
# Load Truth Data
|
|
TRUTH_N_POS_HEADER = ["Time [s]", "True North Position [m]"]
|
|
TRUTH_FSP_X_HEADER = ["Time [s]", "True FSP_X [m/s^2]"]
|
|
m1TruthData = pd.read_csv(
|
|
f"./data/N_truth_IMU_M1L.txt",
|
|
header=None, skiprows=1,
|
|
names=TRUTH_N_POS_HEADER,
|
|
), pd.read_csv(
|
|
f"./data/N_truth_IMU_M1H.txt",
|
|
header=None, skiprows=1,
|
|
names=TRUTH_N_POS_HEADER,
|
|
), pd.read_csv(
|
|
f"./data/FSPB1_truth_IMU_M1.txt",
|
|
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)
|
|
|
|
# NED Translation & Force Functions
|
|
INIT_EULER_ANGLES = (0, 0, 0)
|
|
def attitude_rate_2NED(attitude, euler_angles):
|
|
phi, theta, psi = euler_angles
|
|
p, q, r = attitude
|
|
|
|
transMat = np.array([ [1, sin(phi)*tan(theta), cos(phi)*tan(theta) ],
|
|
[0, cos(phi), -sin(phi) ],
|
|
[0, sin(phi)/cos(theta), cos(phi)/cos(theta) ]
|
|
])
|
|
|
|
angleMat = np.array([ [p],
|
|
[q],
|
|
[r]
|
|
])
|
|
|
|
return np.matmul(transMat, angleMat).flatten()
|
|
|
|
def calc_NED_acceleration(NEDAttitude, specificForces):
|
|
phi, theta, psi = NEDAttitude # Roll, Pitch, Yaw
|
|
|
|
R41 = np.array([ [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi)-sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi)+sin(psi)*sin(phi)],
|
|
[sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi)+cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi)-cos(psi)*sin(phi)],
|
|
[-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi) ]
|
|
])
|
|
|
|
return np.matmul(R41, specificForces) + GRAVITY_VEC
|
|
|
|
# Calculate Mission Translation & Force Data
|
|
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 = TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NED_VELOCITY_HEADER + NED_POSITION_HEADER
|
|
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 # 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
|
|
dataPoint = missionData.loc[i, IMU_WBE_HEADERS]
|
|
|
|
# Translate to NED Frame
|
|
NED_attitude_rate = attitude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES)
|
|
NED_attitude = NED_attitude_rate * dt + offset["attitude"]
|
|
NED_acceleration = calc_NED_acceleration(NED_attitude, mData(IMU_FSP_HEADERS))
|
|
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
|
|
translatedData.loc[i, NED_VELOCITY_HEADER] = NED_velocity
|
|
translatedData.loc[i, NED_POSITION_HEADER] = NED_position
|
|
|
|
return translatedData
|
|
|
|
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,
|
|
"figSize": (8,12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,1),
|
|
"grid": True,
|
|
"subPlots":[
|
|
{
|
|
"title": "Roll Rate",
|
|
"yLabel": "Roll Rate [rad/s]",
|
|
"plots": [
|
|
{"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][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][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"},\
|
|
]
|
|
},
|
|
]
|
|
}
|
|
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,
|
|
"figSize": (8, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,1),
|
|
"grid": True,
|
|
"tightLayout": 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"},
|
|
]
|
|
},
|
|
{
|
|
"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": "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"},
|
|
]
|
|
},
|
|
]
|
|
}
|
|
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)
|
|
|
|
# Graph the position of the UAV (With 3D Plots)
|
|
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"},
|
|
]
|
|
},
|
|
{
|
|
"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"].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)
|
|
|
|
# Graph Just the velocity
|
|
graphData = {
|
|
"figTitle": f"Mission {mission} Flight Velocity",
|
|
"figTitleFontSize": 16,
|
|
"figSize": (8, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,1),
|
|
"grid": True,
|
|
"subPlots":[
|
|
{
|
|
"title": "North Velocity",
|
|
"yLabel": "Velocity [m/s]",
|
|
"plots": [
|
|
{"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][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][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"},
|
|
]
|
|
},
|
|
]
|
|
}
|
|
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,
|
|
"figSize": (8, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,1),
|
|
"grid": True,
|
|
"subPlots":[
|
|
{
|
|
"title": "North Acceleration",
|
|
"yLabel": "Acceleration [m/s$^2$]",
|
|
"plots": [
|
|
{"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][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][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"},
|
|
]
|
|
},
|
|
]
|
|
}
|
|
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,
|
|
"figSize": (20, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,3),
|
|
"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"},
|
|
]
|
|
},
|
|
{
|
|
"title": "North Velocity",
|
|
"yLabel": "Velocity [m/s]",
|
|
"plots": [
|
|
{"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][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][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][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][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][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][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][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][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)
|
|
|
|
# Graph the R-P-Y, velocity & acceleration on one big graph
|
|
graphData = {
|
|
"figTitle": f"Mission {mission} Flight Characteristics",
|
|
"figTitleFontSize": 16,
|
|
"figSize": (20, 12), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
|
"yLabel": "Time [s]",
|
|
"plotDim": (3,3),
|
|
"grid": True,
|
|
"subPlots":[
|
|
{
|
|
"title": "Roll Rate",
|
|
"yLabel": "Roll Rate [rad/s]",
|
|
"plots": [
|
|
{"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][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][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][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][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][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][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][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][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"},
|
|
]
|
|
},
|
|
]
|
|
}
|
|
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):
|
|
# 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):
|
|
# 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]",
|
|
"yLabel": "Position Delta [m]",
|
|
"title": "Calculated vs True North Position Delta",
|
|
"plots": [
|
|
{"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")
|
|
|
|
graphData = {
|
|
"grid": True,
|
|
"xLabel": "Time [s]",
|
|
"yLabel": "Position Error [%]",
|
|
"title": "Calculated vs True North Position Error",
|
|
"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"},
|
|
]
|
|
}
|
|
makeGraph(graphData, False, False, figSavePath="./images/m1_error_percent.png")
|
|
|
|
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":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[0]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
|
|
{"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[0]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
|
|
{"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": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"},
|
|
]
|
|
},
|
|
{
|
|
"title": "UAV Flight Path",
|
|
"xLabel": "Noth Position [m]",
|
|
"yLabel": "East Position [m]",
|
|
"zLabel": "Down Position [m]",
|
|
"plots": [
|
|
{"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"},
|
|
{"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"},
|
|
{"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"},
|
|
{"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": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
|
|
]
|
|
},
|
|
{
|
|
"title": "East Position",
|
|
"yLabel": "Position [m]",
|
|
"plots": [
|
|
{"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
|
|
{"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
|
|
{"x":missionData[0][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": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"},
|
|
]
|
|
},
|
|
{
|
|
"title": "UAV Flight Path - Low Grade Data",
|
|
"xLabel": "Noth Position [m]",
|
|
"yLabel": "East Position [m]",
|
|
"zLabel": "Down Position [m]",
|
|
"plots": [
|
|
{"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"},
|
|
{"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": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
|
|
]
|
|
},
|
|
{
|
|
"title": "Down Position",
|
|
"yLabel": "Position [m]",
|
|
"plots": [
|
|
{"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
|
|
{"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
|
|
{"x":missionData[0][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": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
|
|
]
|
|
},
|
|
{
|
|
"title": "UAV Flight Path - High Grade Data",
|
|
"xLabel": "Noth Position [m]",
|
|
"yLabel": "East Position [m]",
|
|
"zLabel": "Down Position [m]",
|
|
"plots": [
|
|
{"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"},
|
|
{"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"},
|
|
{"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_gps_all.png")
|
|
|
|
if __name__ == '__main__':
|
|
# Task 1 - b, c, d
|
|
#Load, Process & Graph Mission Data for Mission 1
|
|
print("Loading / Calculating to Mission 1 Data")
|
|
missionData = cacheData("./tmp/m1_L_transData.dfz", calculateVelocityPosition, (m1_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \
|
|
cacheData("./tmp/m1_H_transData.dfz", calculateVelocityPosition, (m1_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False)
|
|
missionData = doTruthComparison(missionData, m1TruthData)
|
|
print("Starting to Graph Mission 1")
|
|
#generateGraphs(missionData, 1)
|
|
|
|
# Task 2 - b
|
|
errorPoly = generateTruthErrorGraphs(missionData)
|
|
for i, poly in enumerate(errorPoly):
|
|
string = ""
|
|
for n, c_n in enumerate(poly):
|
|
string += f" + {c_n:.4f}*t^{len(poly)-1-n}"
|
|
|
|
print(f"{'High' if i else 'Low'} Grade Error Poly:" + string[2:])
|
|
|
|
# Task 3
|
|
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({
|
|
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]]
|
|
})
|
|
print(t_15_data)
|
|
|
|
# Task 4
|
|
#Load, Process & Graph Mission Data for Mission 2
|
|
print("\nLoading / Calculating to Mission 2 Data")
|
|
missionData_4 = 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_4, 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)
|
|
generateGPSGraphs(missionData_4, missionData, m2_GPSData[0], "2 - 0.5Hz GPS Correction & Comparison")
|
|
#generateGraphs(missionData, "2 - 0.5Hz GPS", m2_GPSData[1])
|
|
|
|
|
|
|
|
|
|
print("Complete") |