diff --git a/main.py b/main.py
index 41f0bb9..812aee6 100644
--- a/main.py
+++ b/main.py
@@ -684,9 +684,11 @@ def generateGPSGraphs(missionData, gpsMissionData, gpsData, mission):
                 "title":  "North Position",  
                 "yLabel": "Position [m]",
                 "plots": [
+                    {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[0]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
+                    {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[0]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
                     {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[0]], "label":"High Grade Data", "colour":"uq:blue"},
                     {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"Low Grade Data", "colour":"uq:purple"},
-                    {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[0]], "label":"RawGPS Data", "colour":"uq:red"},
+                    {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
             {
@@ -695,16 +697,22 @@ def generateGPSGraphs(missionData, gpsMissionData, gpsData, mission):
                 "yLabel": "East Position [m]",
                 "zLabel": "Down Position [m]",
                 "plots": [
-                    {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
+                    {"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"},
+                    {"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"},
                     {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
+                    {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
             {
                 "title":  "East Position",  
                 "yLabel": "Position [m]",
                 "plots": [
-                    {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
+                    {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
                     {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "label":"Low Grade Data", "colour":"uq:purple"},
+                    {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
             {
@@ -713,16 +721,20 @@ def generateGPSGraphs(missionData, gpsMissionData, gpsData, mission):
                 "yLabel": "East Position [m]",
                 "zLabel": "Down Position [m]",
                 "plots": [
+                    {"x":gpsMissionData[0][NED_POSITION_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[1]], "z":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:green"},
                     {"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
-                    #{"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
             {
                 "title":  "Down Position",  
                 "yLabel": "Position [m]",
                 "plots": [
-                    {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x":gpsMissionData[0][TIME_HEADER[0]], "y":gpsMissionData[0][NED_POSITION_HEADER[2]], "label":"GPS Corrected Low Grade Data", "colour":"uq:orange"},
+                    {"x":gpsMissionData[1][TIME_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:green"},
                     {"x":missionData[0][TIME_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
+                    {"x":missionData[1][TIME_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
             {
@@ -731,20 +743,14 @@ def generateGPSGraphs(missionData, gpsMissionData, gpsData, mission):
                 "yLabel": "East Position [m]",
                 "zLabel": "Down Position [m]",
                 "plots": [
-                    #{"x":missionData[0][NED_POSITION_HEADER[0]], "y":missionData[0][NED_POSITION_HEADER[1]], "z":missionData[0][NED_POSITION_HEADER[2]], "label":"Low Grade Data", "colour":"uq:purple"},
+                    {"x":gpsMissionData[1][NED_POSITION_HEADER[0]], "y":gpsMissionData[1][NED_POSITION_HEADER[1]], "z":gpsMissionData[1][NED_POSITION_HEADER[2]], "label":"GPS Corrected High Grade Data", "colour":"uq:orange"},
                     {"x":missionData[1][NED_POSITION_HEADER[0]], "y":missionData[1][NED_POSITION_HEADER[1]], "z":missionData[1][NED_POSITION_HEADER[2]], "label":"High Grade Data", "colour":"uq:blue"},
+                    {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"},
                 ]
             },
         ]
     }
-    if TRUTH_N_POS_HEADER[1] in missionData[1].columns:
-        graphData["subPlots"][0]["plots"].insert(0, {"x": missionData[1][TIME_HEADER[0]], "y":missionData[1][TRUTH_N_POS_HEADER[1]], "label":"True Position", "colour":"uq:red"})
-    if gpsData is not None:
-        graphData["subPlots"][0]["plots"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[1]], "label":"GPS Position", "colour":"uq:red"})
-        graphData["subPlots"][2]["plots"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[2]], "label":"GPS Position", "colour":"uq:red"})
-        graphData["subPlots"][4]["plots"].insert(0, {"x": gpsData[TIME_HEADER], "y":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"})
-        graphData["subPlots"][1]["plots"].insert(0, {"x": gpsData[GPS_DATA_HEADER[1]], "y":gpsData[GPS_DATA_HEADER[2]], "z":gpsData[GPS_DATA_HEADER[3]], "label":"GPS Position", "colour":"uq:red"})
-    makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d.png")
+    makeGraph(graphData, False, False, figSavePath=f"./images/m{mission}_flight_pos_3d_gps_all.png")
 
 if __name__ == '__main__':
     # Task 1 - b, c, d
@@ -778,21 +784,22 @@ if __name__ == '__main__':
     # Task 4
     #Load, Process & Graph Mission Data for Mission 2
     print("\nLoading / Calculating to Mission 2 Data")
-    missionData = cacheData("./tmp/m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS), forceCalc=False), \
+    missionData_4 = 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)
     print("Starting to Graph Mission 2")
-    #generateGraphs(missionData, 2)
+    #generateGraphs(missionData_4, 2)
 
     # Task 5
     # Calc 5hz Mission
     missionData = cacheData("./tmp/gps_5_m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS, m2_GPSData[0]), forceCalc=False), \
         cacheData("./tmp/gps_5_m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS, m2_GPSData[0]), forceCalc=False)
-    generateGraphs(missionData, "2 - 5Hz GPS", m2_GPSData[0])
+    #generateGraphs(missionData, "2 - 5Hz GPS", m2_GPSData[0])
 
     # Calc 0.5hz Mission
     missionData = cacheData("./tmp/gps_0.5_m2_L_transData.dfz", calculateVelocityPosition, (m2_IMUData[0], INIT_FLIGHT_PRAMS, m2_GPSData[1]), forceCalc=False), \
         cacheData("./tmp/gps_0.5_m2_H_transData.dfz", calculateVelocityPosition, (m2_IMUData[1], INIT_FLIGHT_PRAMS, m2_GPSData[1]), forceCalc=False)
-    generateGraphs(missionData, "2 - 0.5Hz GPS", m2_GPSData[1])
+    generateGPSGraphs(missionData_4, missionData, m2_GPSData[0], "2 - 0.5Hz GPS Correction & Comparison")
+    #generateGraphs(missionData, "2 - 0.5Hz GPS", m2_GPSData[1])