From 272f32794f4b0fb9a37fd25107c6371434c06440 Mon Sep 17 00:00:00 2001 From: Cal Wing <20716204+calw20@users.noreply.github.com> Date: Fri, 3 Nov 2023 08:36:34 +1000 Subject: [PATCH] Add comments for style --- main.py | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/main.py b/main.py index ac8a6ce..41f0bb9 100644 --- a/main.py +++ b/main.py @@ -2,7 +2,7 @@ # Cal Wing, Sem 2 2023 # Import System / Common Libs -import os, pickle, time +import os, pickle import numpy as np import pandas as pd from tqdm import tqdm @@ -168,11 +168,13 @@ TRANS_DATA_HEADER = TIME_HEADER + IMU_WBE_HEADERS + NED_ACCELERATION_HEADER + NE def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.DataFrame: print("Translating Motion & Calculating Resulting Forces") + # By default this is a reference to an object, as we update it it needs to be a copy to not modify the original. offset = initial_values.copy() translatedData = pd.DataFrame(columns=TRANS_DATA_HEADER, dtype=float) for i in tqdm(range(len(missionData))): - mData = lambda header, j=i: missionData.loc[j, header].values + mData = lambda header, j=i: missionData.loc[j, header].values # Easy value extraction + # dt = t[i] - t[i-1] BUT for t == 0; dt = t[i+1] - t[i] dt = mData(TIME_HEADER, i)[0] - mData(TIME_HEADER, i-1)[0] if i > 0 else mData(TIME_HEADER, i+1)[0] - mData(TIME_HEADER, i)[0] # Get the time point data @@ -185,14 +187,18 @@ def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.D NED_velocity = NED_acceleration * dt + offset["velocity"] NED_position = NED_velocity * dt + offset["position"] + # If we have GPS data prefer that. + # TL;DR If GPS Data at time point use that else use above calculated position if gpsData is not None and gpsData[TIME_HEADER].isin([mData(TIME_HEADER, i)[0]]).any().any(): timeIdx = gpsData[TIME_HEADER].isin([mData(TIME_HEADER, i)[0]]).idxmax() NED_position = gpsData.loc[timeIdx, GPS_NED_POS_HEADER[0:]].values[0] + # Update the offsets offset["attitude"] = NED_attitude offset["velocity"] = NED_velocity offset["position"] = NED_position + # Add the data to the translated frame translatedData.loc[i, TIME_HEADER] = mData(TIME_HEADER) translatedData.loc[i, IMU_WBE_HEADERS] = NED_attitude_rate translatedData.loc[i, NED_ACCELERATION_HEADER] = NED_acceleration @@ -203,6 +209,7 @@ def calculateVelocityPosition(missionData, initial_values, gpsData=None) -> pd.D def generateGraphs(missionData, mission, gpsData=None): pBar = tqdm(total=7) + # Graph just the R-P-Y graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, @@ -240,6 +247,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_rpy.png") pBar.update(1) + # Graph the position of the UAV (Without 3D Plots) graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, @@ -284,6 +292,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos.png") pBar.update(1) + # Graph the position of the UAV (With 3D Plots) graphData = { "figTitle": f"Mission {mission} Flight Position", "figTitleFontSize": 16, @@ -360,6 +369,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d.png") pBar.update(1) + # Graph Just the velocity graphData = { "figTitle": f"Mission {mission} Flight Velocity", "figTitleFontSize": 16, @@ -397,6 +407,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_velocity.png") pBar.update(1) + # Graph just the acceleration graphData = { "figTitle": f"Mission {mission} Flight Acceleration", "figTitleFontSize": 16, @@ -434,6 +445,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_accell.png") pBar.update(1) + # Graph the position, velocity & acceleration on one big graph graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, @@ -525,6 +537,7 @@ def generateGraphs(missionData, mission, gpsData=None): makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_chars.png") pBar.update(1) + # Graph the R-P-Y, velocity & acceleration on one big graph graphData = { "figTitle": f"Mission {mission} Flight Characteristics", "figTitleFontSize": 16, @@ -613,21 +626,22 @@ def generateGraphs(missionData, mission, gpsData=None): 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): + # Add & calculate the error of each position to the dataframe 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 + # Correct the DataType in the injected column mData[mData.columns] = mData[mData.columns].astype(float) 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]]]) - + # Use polyfit to generate the 2nd order polynomial to best fit the error data poly_low = np.polyfit(missionData[0][TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[0]], 2) poly_high = np.polyfit(missionData[1][TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[0]], 2) + graphData = { "grid": True, "xLabel": "Time [s]", @@ -641,11 +655,8 @@ def generateTruthErrorGraphs(missionData): ] } - makeGraph(graphData, False, False, figSavePath="./images/m1_error_delta.png") - #poly_low_perc = np.polyfit(missionData[0][TIME_HEADER[0]], missionData[0][TRUTH_NPOS_DELTA_HEADER[1]], 2) - #poly_high_perc = np.polyfit(missionData[1][TIME_HEADER[0]], missionData[1][TRUTH_NPOS_DELTA_HEADER[1]], 2) graphData = { "grid": True, "xLabel": "Time [s]", @@ -654,9 +665,6 @@ def generateTruthErrorGraphs(missionData): "plots": [ {"x":missionData[0][TIME_HEADER[0]], "y": np.abs(missionData[0][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "Low Grade IMU Error"}, {"x":missionData[1][TIME_HEADER[0]], "y": np.abs(missionData[1][TRUTH_NPOS_DELTA_HEADER[1]]), "label": "High Grade IMU Error"}, - #{"x":missionData[0][IMU_TIME_HEADER[0]], "y": np.polyval(poly_low_perc, missionData[0][IMU_TIME_HEADER[0]]), "label": "Low Grade Best Fit Curve"}, - #{"x":missionData[1][IMU_TIME_HEADER[0]], "y": np.polyval(poly_high_perc, missionData[1][IMU_TIME_HEADER[0]]), "label": "High Grade Best Fit Curve"}, - ] } makeGraph(graphData, False, False, figSavePath="./images/m1_error_percent.png")