Sorta Finish Task 1
This commit is contained in:
Cal Wing
parent
2fb63c7309
commit
faa2400d4f
252
main.py
252
main.py
@ -20,14 +20,14 @@ for folder in folders:
|
|||||||
# This is a cacheing function, it checks if a cache file exists and loads it or it calcs & saves it.
|
# 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
|
# 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!
|
# 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):
|
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:
|
if len(args) == 0 and defArg is not None:
|
||||||
args = (defArg, )
|
args = (defArg, )
|
||||||
data = None
|
data = None
|
||||||
dataFileExt = dataFilePath.rsplit(".")[-1]
|
dataFileExt = dataFilePath.rsplit(".")[-1]
|
||||||
|
|
||||||
# Check if file exists
|
# Check if file exists
|
||||||
if os.path.isfile(dataFilePath):
|
if os.path.isfile(dataFilePath) and not forceCalc:
|
||||||
print(f"Found data file \"{dataFilePath}\", loading data.")
|
print(f"Found data file \"{dataFilePath}\", loading data.")
|
||||||
# Check if file is a compressed numpy file
|
# Check if file is a compressed numpy file
|
||||||
if dataFileExt == "npz":
|
if dataFileExt == "npz":
|
||||||
@ -41,6 +41,10 @@ def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tup
|
|||||||
else:
|
else:
|
||||||
raise TypeError(f"Cannot determine file type of: {dataFilePath}")
|
raise TypeError(f"Cannot determine file type of: {dataFilePath}")
|
||||||
else:
|
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.")
|
print(f"Could not find data file \"{dataFilePath}\", generating & saving data.")
|
||||||
# Calculate Value
|
# Calculate Value
|
||||||
data = calcFunction(*args, **kargs)
|
data = calcFunction(*args, **kargs)
|
||||||
@ -62,7 +66,7 @@ def cacheData(dataFilePath: str, calcFunction: callable = lambda x: x, args: tup
|
|||||||
if data is None: raise ValueError("Could not import or generate data requested")
|
if data is None: raise ValueError("Could not import or generate data requested")
|
||||||
return data
|
return data
|
||||||
|
|
||||||
|
GRAVITY_VEC = np.array([0, 0, 9.81])
|
||||||
|
|
||||||
# IMU Data Loading
|
# IMU Data Loading
|
||||||
# I map in to a heading to add units / make things make more sense
|
# I map in to a heading to add units / make things make more sense
|
||||||
@ -96,7 +100,7 @@ m2_IMUData = importIMUData(2, 0), importIMUData(2, 1)
|
|||||||
|
|
||||||
# NED Translation & Force Functions
|
# NED Translation & Force Functions
|
||||||
INIT_EULER_ANGLES = (0, 0, 0)
|
INIT_EULER_ANGLES = (0, 0, 0)
|
||||||
def attidude_rate_2NED(angles, euler_angles):
|
def attitude_rate_2NED(angles, euler_angles):
|
||||||
phi, theta, psi = euler_angles
|
phi, theta, psi = euler_angles
|
||||||
p, q, r = angles
|
p, q, r = angles
|
||||||
|
|
||||||
@ -110,61 +114,225 @@ def attidude_rate_2NED(angles, euler_angles):
|
|||||||
[r]
|
[r]
|
||||||
])
|
])
|
||||||
|
|
||||||
return np.matmul(transMat, angleMat)
|
return np.matmul(transMat, angleMat).flatten()
|
||||||
|
|
||||||
def getNEDForces(NEDPos):
|
def calc_NED_acceleration(NEDAttitude, specificForces):
|
||||||
phi, theta, psi = NEDPos # Roll, Pitch, Yaw
|
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)],
|
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(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) ]
|
[-sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi) ]
|
||||||
])
|
])
|
||||||
|
|
||||||
return R41
|
return np.matmul(R41, specificForces) + GRAVITY_VEC
|
||||||
|
|
||||||
# Calculate Mission Translation & Force Data
|
# Calculate Mission Translation & Force Data
|
||||||
TRANS_DATA_HEADER = IMU_TIME_HEADER + IMU_WBE_HEADERS + ["Forces", "ForceSum", "ForceCumSum"]
|
NED_ACCELERATION_HEADER = ["Acceleration N [m/s^2]", "Acceleration E [m/s^2]", "Acceleration D [m/s^2]"]
|
||||||
def calculateTranslatedData(missionWBEData) -> pd.DataFrame:
|
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
|
||||||
|
def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame:
|
||||||
print("Translating Motion & Calculating Resulting Forces")
|
print("Translating Motion & Calculating Resulting Forces")
|
||||||
|
|
||||||
|
offset = initial_values
|
||||||
|
|
||||||
translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER)
|
translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER)
|
||||||
for i in tqdm(range(len(missionWBEData))):
|
for i in tqdm(range(len(missionData))):
|
||||||
dataPoint = missionWBEData[IMU_WBE_HEADERS].iloc[i] # Get the time point data
|
mData = lambda header, j=i: missionData[header].iloc[j].values
|
||||||
trans = attidude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES).flatten() # Translate to Net
|
dt = mData(IMU_TIME_HEADER, i-1)[0] - mData(IMU_TIME_HEADER, i)[0] if i > 0 else mData(IMU_TIME_HEADER, i)[0] - mData(IMU_TIME_HEADER, i+1)[0]
|
||||||
forces = getNEDForces(trans)
|
|
||||||
forceSum = np.array([np.sum(forces[:,0].flatten()), np.sum(forces[:,1].flatten()), np.sum(forces[:,2].flatten())])
|
# Get the time point data
|
||||||
privForce = translatedData[TRANS_DATA_HEADER[5]].iloc[i-1] if i > 0 else [0, 0, 0]
|
dataPoint = missionData[IMU_WBE_HEADERS].iloc[i]
|
||||||
forceCumSum = np.array([ forceSum[ii] + value for ii, value in enumerate(privForce) ])
|
|
||||||
translatedData.loc[i] = {
|
# Translate to NED Frame
|
||||||
TRANS_DATA_HEADER[0]: missionWBEData[IMU_TIME_HEADER].iloc[i],
|
NED_attitude_rate = attitude_rate_2NED(dataPoint.values, INIT_EULER_ANGLES)
|
||||||
TRANS_DATA_HEADER[1]: trans[0],
|
NED_attitude = NED_attitude_rate * dt + offset["attitude"]
|
||||||
TRANS_DATA_HEADER[2]: trans[1],
|
NED_acceleration = calc_NED_acceleration(NED_attitude, mData(IMU_FSP_HEADERS))
|
||||||
TRANS_DATA_HEADER[3]: trans[2],
|
NED_velocity = NED_acceleration * dt + offset["velocity"]
|
||||||
TRANS_DATA_HEADER[4]: forces,
|
NED_position = NED_velocity * dt + offset["position"]
|
||||||
TRANS_DATA_HEADER[5]: forceSum,
|
|
||||||
TRANS_DATA_HEADER[6]: forceCumSum,
|
offset["attitude"] = NED_acceleration
|
||||||
}
|
offset["velocity"] = NED_velocity
|
||||||
|
offset["position"] = NED_position
|
||||||
|
|
||||||
|
translatedData.loc[i, IMU_TIME_HEADER] = mData(IMU_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
|
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=4)
|
||||||
|
graphData = {
|
||||||
|
"figTitle": f"Mission {mission} Flight Characteristics",
|
||||||
|
"figTitleFontSize": 16,
|
||||||
|
"figSize": (15,20), #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][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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},\
|
||||||
|
]
|
||||||
|
},
|
||||||
|
]
|
||||||
|
}
|
||||||
|
makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_rpy.png")
|
||||||
|
pBar.update(1)
|
||||||
|
|
||||||
|
graphData = {
|
||||||
|
"figTitle": f"Mission {mission} Flight Position",
|
||||||
|
"figTitleFontSize": 16,
|
||||||
|
"figSize": (15,15), #Yay America, this is in inches :/ # Note: cm = 1/2.54
|
||||||
|
"yLabel": "Time [s]",
|
||||||
|
"plotDim": (3,1),
|
||||||
|
"grid": True,
|
||||||
|
"subPlots":[
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
]
|
||||||
|
}
|
||||||
|
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_pos.png")
|
||||||
|
pBar.update(1)
|
||||||
|
|
||||||
|
graphData = {
|
||||||
|
"figTitle": f"Mission {mission} Flight Velocity",
|
||||||
|
"figTitleFontSize": 16,
|
||||||
|
"figSize": (15,15), #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][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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"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"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
]
|
||||||
|
}
|
||||||
|
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_velocity.png")
|
||||||
|
pBar.update(1)
|
||||||
|
|
||||||
|
graphData = {
|
||||||
|
"figTitle": f"Mission {mission} Flight Acceleration",
|
||||||
|
"figTitleFontSize": 16,
|
||||||
|
"figSize": (15,15), #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[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"},
|
||||||
|
{"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "East Acceleration",
|
||||||
|
"yLabel": "Acceleration [m/s$^2$]",
|
||||||
|
"plots": [
|
||||||
|
{"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"},
|
||||||
|
{"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"title": "Down Acceleration",
|
||||||
|
"yLabel": "Acceleration [m/s$^2$]",
|
||||||
|
"plots": [
|
||||||
|
{"x":missionData[1][IMU_TIME_HEADER[0]], "y":missionData[1][NED_ACCELERATION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
|
||||||
|
{"x":missionData[0][IMU_TIME_HEADER[0]], "y":missionData[0][NED_ACCELERATION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
|
||||||
|
]
|
||||||
|
},
|
||||||
|
]
|
||||||
|
}
|
||||||
|
makeGraph(graphData, False, False, figSavePath="./images/m{mission}_flight_accell.png")
|
||||||
|
pBar.update(1)
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
missionWBEData = m1_IMUData[0][IMU_TIME_HEADER + IMU_WBE_HEADERS]
|
missionData = cacheData("./tmp/m1_L_transData.dfz", calculateVelocityPosition, (m1_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \
|
||||||
translatedData = cacheData("./tmp/m1_transData.dfz", calculateTranslatedData, (missionWBEData,))
|
cacheData("./tmp/m1_H_transData.dfz", calculateVelocityPosition, (m1_IMUData[1], INIT_FLIGHT_PRAMS), forceCalc=False)
|
||||||
|
|
||||||
x = np.cumsum(translatedData[IMU_WBE_HEADERS[0]])
|
|
||||||
y = np.cumsum(translatedData[IMU_WBE_HEADERS[1]])
|
|
||||||
z = np.cumsum(translatedData[IMU_WBE_HEADERS[2]])
|
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
generateGraphs(missionData, 1)
|
||||||
fig = plt.figure()
|
|
||||||
ax = plt.axes(projection='3d')
|
|
||||||
plots = translatedData[TRANS_DATA_HEADER[6]].values
|
|
||||||
#x = [p[0] for p in plots]
|
|
||||||
#y = [p[1] for p in plots]
|
|
||||||
#z = [p[2] for p in plots]
|
|
||||||
ax.plot3D(x, y, z)
|
|
||||||
ax.plot3D(x+5, y-2, z+1)
|
|
||||||
|
|
||||||
plt.show()
|
#import matplotlib.pyplot as plt
|
||||||
|
#fig = plt.figure()
|
||||||
|
#ax = plt.axes(projection='3d')
|
||||||
|
#x, y, z = missionData[0][NED_POSITION_HEADER[0]], missionData[0][NED_POSITION_HEADER[1]], missionData[0][NED_POSITION_HEADER[2]]
|
||||||
|
#ax.plot3D(x, y, z)
|
||||||
|
#x, y, z = missionData[1][NED_POSITION_HEADER[0]], missionData[1][NED_POSITION_HEADER[1]], missionData[1][NED_POSITION_HEADER[2]]
|
||||||
|
#ax.plot3D(x, y, z)
|
||||||
|
#plt.show()
|
||||||
|
|
||||||
|
|
||||||
print("Complete")
|
print("Complete")
|
||||||
Loading…
x
Reference in New Issue
Block a user