From dea7734429eed15c0cd45e0dbde499bd85777288 Mon Sep 17 00:00:00 2001 From: Cal Wing <20716204+calw20@users.noreply.github.com> Date: Thu, 2 Nov 2023 17:16:52 +1000 Subject: [PATCH] Start Graphing Truth Data --- main.py | 73 +++++++++++++++++++++++++++++++++++++++++++++-- requirements.txt | Bin 86 -> 584 bytes 2 files changed, 71 insertions(+), 2 deletions(-) diff --git a/main.py b/main.py index 0fe47cd..cee38b4 100644 --- a/main.py +++ b/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") \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 44ad8103ca5589efb3475771702f09266ac9e941..d4161209f94d78e2c83889c3ee4e9c528f511856 100644 GIT binary patch literal 584 zcmY+BU2eia420*oQjY>k0#v;84!r_|(nd*^l--ol!?(=X`B8+t4|qKO#`*s%aY4hn z;((U*%3p>xwyYOC;|Wp(B)&aW9qy*d+4pkIow^3^RE!Qe3e%{69z5itebH3fvBhRm z)u?b|MbmswZrUU5&Ui$b$RX;)x5Clf-xJxbJkKR6%win=hC zVp0U0sBl)#GS0tosxg9_P3Cqpar$;$Njt8bp8Y-P=pjwQCsPEhY*S+5p8v_)c|6fb zB>nHu$p%_^cJzqsdA7)8)gpOrUut8y#Pc&Si$phZ=7cX&wAS;lKi^Axb&JGuvU$Jm F`2bG~R2l#P delta 69 zcmX@X62|@iUmim#LoP!BL*>Nvl197?Tnr@)g$yY`ArKpcl^Kc|k{L2VO2GUAhD3%u Lpo&DGxB>$JN!ShJ