cal/AERO4200-Ass3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Create Matrix Translation Functions

Browse Source
This commit is contained in:
Cal Wing 2023-10-22 18:05:38 +10:00
parent 81eb609efe
commit 7946e1fcf7
1 changed files with 43 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
main.py
View File

@ -7,6 +7,8 @@ import numpy as np
import pandas as pd
from tqdm import tqdm
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
@ -29,11 +31,10 @@ for folder in folders:
# - FSP_X (in m/s2) - the specific force in the body-fixed x-direction
# - FSP_Y (in m/s2) - the specific force in the body-fixed y-direction
# - FSP_Z (in m/s2) - the specific force in the body-fixed z-direction
IMU_DATA_HEADER = [
"Time [s]",
"WBE_1 [rad/s]", "WBE_2 [rad/s]", "WBE_3 [rad/s]",
"FSP_X [m/s^2]", "FSP_Y [m/s^2]", "FSP_Z [m/s^2]"
]
IMU_TIME_HEADER = ["Time [s]"]
IMU_WBE_HEADERS = ["WBE_1 [rad/s]", "WBE_2 [rad/s]", "WBE_3 [rad/s]"]
IMU_FSP_HEADERS = ["FSP_X [m/s^2]", "FSP_Y [m/s^2]", "FSP_Z [m/s^2]"]
IMU_DATA_HEADER = IMU_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"
@ -42,7 +43,6 @@ def importIMUData(mission, imu):
f"./data/IMU_M{str(mission) + str(imu)}.txt",
header=None, skiprows=1,
names=IMU_DATA_HEADER,
dtype=[float, float, float, float, float, float, float],
)
return data
@ -50,10 +50,47 @@ def importIMUData(mission, imu):
m1_IMUData = importIMUData(1, 0), importIMUData(1, 1) #(L, H) Data
m2_IMUData = importIMUData(2, 0), importIMUData(2, 1)
INIT_EULER_ANGLES = (0, 0, 0)
def translate2NED(angles, euler_angles):
phi, theta, psi = euler_angles
p, q, r = angles
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)
def getNEDForces(NEDPos):
phi, theta, psi = NEDPos
forceMat = 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 forceMat
if __name__ == '__main__':
dataPoint = m1_IMUData[0][IMU_WBE_HEADERS].iloc[0]
trans = translate2NED(dataPoint.values, INIT_EULER_ANGLES)
forces = getNEDForces((trans[0][0], trans[1][0], trans[2][0]))
print("Raw Data")
print(dataPoint)
print("\nTranslated Point")
print(trans)
print("\nForces")
print(forces)
input("Damn")
if __name__ == '__main__1':