diff --git a/FRC_Fiducial_Tracking/April_PNP_Live.py b/FRC_Fiducial_Tracking/April_PNP_Live.py
index 7b1a8ca..4aab4cc 100644
--- a/FRC_Fiducial_Tracking/April_PNP_Live.py
+++ b/FRC_Fiducial_Tracking/April_PNP_Live.py
@@ -65,7 +65,7 @@ tag_coords = [0, [1, 610.77, 42.19, 18.22, 180], [2, 610.77, 108.19, 18.22, 180]
 [4, 636.96, 265.74, 27.38, 180], [5, 14.25, 265.74, 27.38, 0], [6, 40.45, 174.19, 18.22, 0], [7, 40.45, 108.19, 18.22, 0],
 [8, 40.45, 42.19, 18.22, 0]]
 
-# x,y,z,rx,ry,rx
+# x,y,z,rx,ry,rz
 robo_space_pose = [0, 0, 0, 0, 0, 0]
 
 def getTagCoords(tag_id):
diff --git a/FRC_Fiducial_Tracking/Pose_Estimation.py b/FRC_Fiducial_Tracking/Pose_Estimation.py
index db8d91c..b82c784 100644
--- a/FRC_Fiducial_Tracking/Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/Pose_Estimation.py
@@ -6,20 +6,29 @@ class PoseEstimation:
     def __init__(self, tag_coords, robo_space_pose):
         self.tag_coords = tag_coords
         self.robot_space_pose = robo_space_pose
-        self.orientation = [0, 0, 0]
-        self.coordinates = [0, 0, 0]
+        self.orientation = [0, 0, 0, 0]
+        self.coordinates = [0, 0, 0, 0]
 
     def update(self, data_array):
         self.visible_tags = len(data_array)
 
     def getCoords(self):
         coord_array = []
-        for i in self.visible_tags:
-            tvecXCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.cos(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[0]
-            tvecYCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_y**2) * math.sin(90-self.data_array[i].rvec_x - self.robot_space_pose[3])
-            tvecZCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.sin(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[2]
+        if(len(self.visible_tags)>1):
+            for i in self.visible_tags:
+                tvecXCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.cos(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[0]
+                tvecYCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_y**2) * math.sin(90-self.data_array[i].rvec_x - self.robot_space_pose[3])
+                tvecZCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.sin(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[2]
+
+                self.coordinates = [self.data_array[i].tag_id, tvecXCoord, tvecYCoord, tvecZCoord]
+                coord_array.append(self.coordinates)
+
+                rvecPitch = -self.data_array[i].rvec_z-self.robot_space_pose[3]
+                rvecRoll = -self.data_array[i].rvec_x-self.robot_space_pose[4]
+                rvecYaw = self.tag_coords[self.data_array[i].tag_id][4]-self.data_array[i].rvec_y-self.robot_space_pose[5]
+        
+                self.orientation = [self.data_array[i].tag_id, rvecPitch, rvecRoll, rvecYaw]
+                coord_array.append(self.orientation)
+        
 
-            self.coordinates = [self.data_array[i].tag_id, tvecXCoord, tvecYCoord, tvecZCoord]
-            coord_array.append(self.coordinates)
-    
         return coord_array