diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py b/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py
new file mode 100644
index 00000000..391fceaa
--- /dev/null
+++ b/autonav_ws/src/autonav_filters/include/autonav_filters/position_graph.py
@@ -0,0 +1,25 @@
+import matplotlib.pyplot as plt 
+import numpy as np
+import pandas as pd
+
+if __name__ == "__main__":
+    my_csv = pd.read_csv('/home/tony/autonav_logs/1713916573060/autonav_filters_pf.csv', delimiter=',', skiprows=1)
+
+    counter = 0
+    for i in range(len(my_csv)):
+        
+        if my_csv.iloc[i, 1] == 0 or my_csv.iloc[i, 2] == 0:
+            print(my_csv.iloc[i, 1])
+            print(my_csv.iloc[i, 2])
+            counter += 1
+
+    print(counter)
+    my_csv = my_csv[counter:]
+    gps_x = my_csv.iloc[:, 1]
+    gps_y = my_csv.iloc[:, 2]
+    print(my_csv)
+    print(gps_x)
+    print(gps_y)
+
+    plt.plot(gps_x, gps_y)
+    plt.show()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py
index d681161a..5657dc55 100644
--- a/autonav_ws/src/autonav_nav/src/astar.py
+++ b/autonav_ws/src/autonav_nav/src/astar.py
@@ -42,7 +42,7 @@
 ]
 
 simulation_waypoints = [
-    [(35.19505, -97.43823), (35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43823)], # Facing North
+    [(35.19510, -97.43823), (35.19505, -97.43823), (35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43824)], # Facing North
     [(35.19469, -97.43859), (35.19469, -97.43823),(35.19483, -97.43824),(35.19506, -97.43823)], # Facing South
 ]