diff --git a/main.py b/main.py
index 133cd5a..095e08c 100644
--- a/main.py
+++ b/main.py
@@ -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':