Start Graphing Truth Data

main.py

@@ -6,6 +6,7 @@ import os, pickle, time
 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
 
@@ -94,6 +95,17 @@ def importIMUData(mission, imu):
 
     return data
 
+# Load Truth Data
+TRUTH_N_POS_HEADER= ["Time [s]", "True North Position [m]"]
+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,
+)
 # Load the Mission Data
 m1_IMUData = importIMUData(1, 0), importIMUData(1, 1) #(L, H) Data
 m2_IMUData = importIMUData(2, 0), importIMUData(2, 1)
@@ -136,7 +148,7 @@ def calculateVelocityPosition(missionData, initial_values) -> pd.DataFrame:
 
     offset = initial_values.copy()
 
-    translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER)
+    translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER, dtype=np.float32)
     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]
@@ -241,7 +253,9 @@ 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"})
     makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png")
     pBar.update(1)
 
@@ -319,9 +333,64 @@ def generateGraphs(missionData, mission):
     makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_accell.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):
+        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
+
+        mData[mData.columns] = mData[mData.columns].astype(np.float32)
+
+    return missionData
+
+def generateTruthErrorGraphs(missionData):
+    
+    #print(missionData[0][[IMU_TIME_HEADER[0]] + [NED_POSITION_HEADER[0]] + [TRUTH_N_POS_HEADER[1]] + [TRUTH_NPOS_DELTA_HEADER[0]]])
+
+    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"}
+        ]
+    }
+    makeGraph(graphData, False, False, figSavePath="./images/m1_error_delta.png")
+
+    poly_low  = np.polyfit(missionData[0][IMU_TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[1]], 4)
+    poly_high = np.polyfit(missionData[1][IMU_TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[1]], 4)
+    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[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.abs(missionData[1][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "High Grade IMU Error"},
+            {"x":missionData[1][IMU_TIME_HEADER[0]], "y": np.polyval(poly_high, missionData[1][IMU_TIME_HEADER[0]]), "label": "High Grade Best Fit Curve"},
+
+        ]
+    }
+    makeGraph(graphData, True, False, figSavePath="./images/m1_error_percent.png")
+
+
 if __name__ == '__main__':
+    #Load, Process & Graph Mission Data for Mission 1
     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)
+    generateTruthErrorGraphs(missionData)
     generateGraphs(missionData, 1)
 
+
+    #Load, Process & Graph Mission Data for Mission 1
+    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)
+    generateGraphs(missionData, 2)
+
     print("Complete")