diff --git a/robots/beetle/launch/test/apriltag_detection.launch b/robots/beetle/launch/test/apriltag_detection.launch
index 9e8c36738..bdbede5eb 100644
--- a/robots/beetle/launch/test/apriltag_detection.launch
+++ b/robots/beetle/launch/test/apriltag_detection.launch
@@ -1,21 +1,4 @@
-<!-- <?xml version="1.0"?> -->
-
-<!-- <launch> -->
-<!--   <arg name="usb_cam_test" default="beetle"/> -->
-  
-<!--   <node pkg="usb_cam" type="usb_cam_node" name="usb_cam">  -->
-<!--     <param name="video_device" value="/dev/video2"/> -->
-<!--   </node> -->
-  
-<!--   <include file="$(find apriltag_ros)/launch/continuous_detection.launch"> -->
-<!--     <arg name="camera_name" value="/dev/Video2" /> -->
-<!--     <arg name="image_topic" value="image_raw" /> -->
-<!--   </include> -->
-
-<!--   <rosparam command="load" file="$(find beetle)/config/apriltag/tags_bundle.yaml" ns="$(arg usb_cam_test)/apriltag_ros_continuous_node"/> -->
-
-<!-- </launch> -->
-
+<?xml version="1.0"?>
 <launch>
   <arg name="node_namespace" default="apriltag_ros_continuous_node" />
   <arg name="camera_name" default="usb_cam" />
diff --git a/robots/beetle/script/apriltagposition.py b/robots/beetle/script/apriltagposition.py
index 2d22c53f4..6d962001a 100755
--- a/robots/beetle/script/apriltagposition.py
+++ b/robots/beetle/script/apriltagposition.py
@@ -13,9 +13,9 @@ def __init__(self):
         # parameter's for PID control
 
         #position gains
-        self.kp = -1.0
-        self.ki = -0.001
-        self.kd = -0.05
+        self.kp = -0.2
+        self.ki = -0.0002
+        self.kd = -0.01
 
         #duration
         self.dt = 0.025
@@ -47,7 +47,7 @@ def __init__(self):
         self.lastEuler = np.array([0.0, 0.0, 0.0])
 
         #distance
-        self.targetDistance = np.array([0.08171, -0.01239, 0.01017]) #offset
+        self.targetDistance = np.array([1.08171, -0.01239, 0.01017]) #x offset = 1.58171
         self.currentDistance = np.array([0.0, 0.0, 0.0])
         self.lastDistance = np.array([0.0, 0.0, 0.0])
         self.currentEucDistance = 0
@@ -57,14 +57,14 @@ def __init__(self):
         self.sub = rospy.Subscriber('/beetle1/tag_detections', AprilTagDetectionArray, self.callback1)
         self.yaw_sub = rospy.Subscriber('/beetle1/mocap/pose', PoseStamped, self.callback2)
 
-        #boolean for docking
-        self.docking_msg = Bool()
-        self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1)
+        # #boolean for docking
+        # self.docking_msg = Bool()
+        # self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1)
         
-        #flags for docking
-        self.flag_msg = KeyValue()
-        self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1)
-        self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1)
+        # #flags for docking
+        # self.flag_msg = KeyValue()
+        # self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1)
+        # self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1)
 
         #others
         self.rate = rospy.Rate(40)
@@ -82,28 +82,28 @@ def calculateError(self):
         self.err_i += self.err
         self.pre_err = self.err
         
-    def dockingVerifier(self):
-        #set docking message
-        if self.currentEucDistance <= 0.01:
-            if self.currentEuler[1] <=2:
-                self.flag_msg.key = '1'
-                self.flag_msg.value = '1'
-        else:
-            self.flag_msg.key = '0'
-            self.flag_msg.value = '0'
+    # def dockingVerifier(self):
+    #     #set docking message
+    #     if self.currentEucDistance <= 0.01:
+    #         if self.currentEuler[1] <=2:
+    #             self.flag_msg.key = '1'
+    #             self.flag_msg.value = '1'
+    #     else:
+    #         self.flag_msg.key = '0'
+    #         self.flag_msg.value = '0'
 
     def defineYaw(self):
-        self.yaw = self.currentmocapEuler[2] + self.currentEuler[1]
-        if self.yaw >= 2:
-            self.nav_msg.yaw_nav_mode = 2
-        else:
-            self.nav_msg.yaw_nav_mode = 0
+        self.yaw = self.currentmocapEuler[2] - self.currentEuler[2]
+        # if self.yaw <= -0.03:
+        #     self.nav_msg.yaw_nav_mode = 2
+        # else:
+        #     self.nav_msg.yaw_nav_mode = 0
         
     def callback1(self,data):
         if data.detections:
             #set values to current pose
             position = data.detections[0].pose.pose.pose.position
-            self.currentDistance = [position.z, -position.x, -position.y] #x = z, y = -x, z = -y
+            self.currentDistance = [position.z, -position.x, -position.y]
             
             #calculate current euclid distance
             self.currentEucDistance = ((position.z-self.targetDistance[0])**2 + (-position.x-self.targetDistance[1])**2 + (-position.y-self.targetDistance[2])**2)**0.5
@@ -114,9 +114,9 @@ def callback1(self,data):
             #calculate current Euler angle
             orientation = data.detections[0].pose.pose.pose.orientation
             self.currentQuarter = [orientation.x, orientation.y, orientation.z, orientation.w]
-            self.currentEuler = np.degrees(np.array([np.arctan2(2 * (orientation.w * orientation.x + orientation.y * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)),
-                             np.arcsin(2 * (orientation.w * orientation.y - orientation.z * orientation.x)),
-                             np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))]))
+            self.currentEuler = np.array([np.arctan2(2 * (orientation.x * orientation.y + orientation.w * orientation.z), 1 - 2 * (orientation.x**2 + orientation.y**2)),
+                             np.arcsin(2 * (orientation.w * orientation.y - orientation.x * orientation.z)),
+                             np.arctan2(2 * (orientation.w * orientation.z + orientation.x * orientation.y), 1 - 2 * (orientation.y**2 + orientation.z**2))])
 
             #set angular log
             self.lastEuler = self.currentEuler
@@ -146,7 +146,7 @@ def callback2(self, data):
             self.currentmocapEuler = self.lastmocapEuler
             
     def main(self):
-        self.docking_msg.data = True 
+        # self.docking_msg.data = True 
         while not rospy.is_shutdown():
             #callback1, 2
 
@@ -181,29 +181,21 @@ def main(self):
             self.nav_msg.target_vel_z = self.vel[2]
             self.pub.publish(self.nav_msg)
 
-            #beetle1 mode verifier
-            self.dockingVerifier()
+            # #beetle1 mode verifier
+            # self.dockingVerifier()
 
-            #switch beetle1 into assembled mode
-             # true ->activated false->disactivate
-            self.docking_pub.publish(self.docking_msg)
+            # #switch beetle1 into assembled mode
+            #  # true ->activated false->disactivate
+            # self.docking_pub.publish(self.docking_msg)
             
-             #flag_msg = KeyValue()
-             #flag_msg.key = '1'
-             #flag_msg.value = '1' # 1 -> assembly, 2->separate
-            self.flag_pub_1.publish(self.flag_msg)
-            self.flag_pub_2.publish(self.flag_msg)
-
-            #rospy.loginfo(self.currentDistance)
-            rospy.loginfo(self.currentEucDistance)
-            #rospy.loginfo(self.vel)
-            #rospy.loginfo(self.currentQuarter)
-            #rospy.loginfo(self.currentmocapEuler)
-            rospy.loginfo(self.currentEuler)
-            #rospy.loginfo(self.yaw)
-            rospy.loginfo(self.flag_msg)
-            #rospy.loginfo(self.tag_lost_flag)
-            rospy.loginfo("")
+            #  #flag_msg = KeyValue()
+            #  #flag_msg.key = '1'
+            #  #flag_msg.value = '1' # 1 -> assembly, 2->separate
+            # self.flag_pub_1.publish(self.flag_msg)
+            # self.flag_pub_2.publish(self.flag_msg)
+
+            rospy.loginfo(self.currentDistance)
+
             self.rate.sleep()
             
 if __name__ == "__main__":
diff --git a/robots/beetle/script/apriltagposition_simulation.py b/robots/beetle/script/apriltagposition_simulation.py
new file mode 100755
index 000000000..33e011fbc
--- /dev/null
+++ b/robots/beetle/script/apriltagposition_simulation.py
@@ -0,0 +1,303 @@
+#!/usr/bin/env python
+import rospy
+from apriltag_ros.msg import AprilTagDetectionArray
+from std_msgs.msg import Float32, Float64, Bool
+from geometry_msgs.msg import Twist, PoseStamped
+from nav_msgs.msg import Odometry
+from aerial_robot_msgs.msg import FlightNav
+import numpy as np
+import tf.transformations as tf
+from diagnostic_msgs.msg import KeyValue
+
+class aprilPIDcontroller():
+    def __init__(self):
+        rospy.init_node("aprilpidVel")
+        # parameters for PID control
+
+        #gains
+        self.kp = -2.0 #-0.2
+        self.ki = -0.005 #-0.0002
+        self.kd = -0.05 #-0.01
+        self.recovery_gain = 3
+        self.yaw_kp = -0.4
+        self.yaw_ki = -0.001
+        self.yaw_kd = -0.02
+
+        #duration
+        self.dt = 0.025
+
+        #terms
+        self.p_term = np.array([0.0, 0.0, 0.0])
+        self.i_term = np.array([0.0, 0.0, 0.0])
+        self.d_term = np.array([0.0, 0.0, 0.0])
+        self.yaw_p_term = 0.0
+        self.yaw_i_term = 0.0
+        self.yaw_d_term = 0.0
+
+        #term corrector
+        self.TermCorrector = np.array([0.5, 1, 1])
+        
+        #position and angular error
+        self.err_i = np.array([0.0, 0.0, 0.0])
+        self.err = np.array([0.0, 0.0, 0.0])
+        self.pre_err = np.array([0.0, 0.0, 0.0])
+        self.yaw_err_i = 0.0
+        self.yaw_err = 0.0
+        self.yaw_pre_err = 0.0
+
+        #vel, yaw
+        self.vel = np.array([0.0, 0.0, 0.0])
+        self.vel_recovery = np.array([0.0, 0.0, 0.0])
+        self.currentVelocity = np.array([0.0, 0.0, 0.0])
+        self.velocitylog_queue = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]])
+        self.yaw = 0.0
+        self.yaw_pre = 0.0
+        self.target_yaw = 0.0
+        self.pre_yaw_diff = 0.0
+
+        #angular
+        self.currentmocapQuarter = np.array([0.0, 0.0, 0.0, 0.0])
+        self.currentmocapEuler = np.array([0.0, 0.0, 0.0])
+        self.lastmocapEuler = np.array([0.0, 0.0, 0.0])
+        self.currentQuarter = np.array([0.0, 0.0, 0.0, 0.0])
+        self.currentEuler = np.array([0.0, 0.0, 0.0])
+        self.lastEuler = np.array([0.0, 0.0, 0.0])
+
+        #distance
+        self.targetDistance = np.array([0.38171, 0.009239, 0.01017]) #x offset = 1.58171
+        self.currentDistance = np.array([0.0, 0.0, 0.0])
+        self.lastDistance = np.array([0.0, 0.0, 0.0])
+        self.distancelog_queue = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]])
+        self.currentEucDistance = 0.0
+
+        #ros
+        self.pub = rospy.Publisher('/beetle1/uav/nav', FlightNav, queue_size=1)
+        self.sub = rospy.Subscriber('/beetle1/tag_detections', AprilTagDetectionArray, self.callback1)
+        self.sub = rospy.Subscriber('/beetle1/uav/cog/odom', Odometry, self.callback3)
+        self.yaw_sub = rospy.Subscriber('/beetle1/mocap/pose', PoseStamped, self.callback2)
+
+        # #boolean for docking
+        # self.docking_msg = Bool()
+        # self.docking_pub = rospy.Publisher('/beetle1/docking_cmd', Bool, queue_size=1)
+        
+        # #flags for docking
+        # self.flag_msg = KeyValue()
+        # self.flag_pub_1 = rospy.Publisher('/beetle1/assembly_flag', KeyValue, queue_size=1)
+        # self.flag_pub_2 = rospy.Publisher('/beetle2/assembly_flag', KeyValue, queue_size=1)
+
+        #others
+        self.rate = rospy.Rate(40)
+        self.nav_msg = FlightNav()
+
+        #flags
+        self.tag_lost_flag = False
+        self.mocap_lost_flag = False
+
+    def calculateError(self):
+        #calculate errors for position and yaw control
+        self.pre_err = self.err
+        self.err = self.targetDistance - self.currentDistance
+        self.err_i += self.err
+
+        self.yaw_err = self.yaw - self.currentmocapEuler[2]
+        self.yaw_err_i += self.yaw_err
+        self.yaw_pre_err = self.yaw_err
+
+        
+    # def dockingVerifier(self):
+    #     #set docking message
+    #     if self.currentEucDistance <= 0.01:
+    #         if self.currentEuler[1] <=2:
+    #             self.flag_msg.key = '1'
+    #             self.flag_msg.value = '1'
+    #     else:
+    #         self.flag_msg.key = '0'
+    #         self.flag_msg.value = '0'
+
+    def defineYaw(self):
+        self.nav_msg.yaw_nav_mode = 2
+        self.nav_msg.target_yaw = self.yaw
+        
+        # if abs(self.currentEuler[1]) >= 0.1:
+        #     self.nav_msg.yaw_nav_mode = 2
+        #     self.nav_msg.target_yaw = self.target_yaw
+            
+        #     if self.currentEuler[1] >= 0.1:
+        #         self.nav_msg.target_yaw = self.yaw - 0.05
+        #         rospy.loginfo("+")
+        #     elif self.currentEuler[1] <= 0.1:
+        #         self.nav_msg.target_yaw = self.yaw + 0.05
+        #         rospy.loginfo("-")
+        #     else:
+        #         self.nav_msg.target_yaw = self.yaw
+        # else:
+        #     self.nav_msg.yaw_nav_mode = 0
+
+    def callback1(self,data):
+        if data.detections:
+            #set values to current pose
+            position = data.detections[0].pose.pose.pose.position
+            self.currentDistance = np.array([position.z, -position.x, -position.y])
+            currentDistance_log = np.array([[position.z, -position.x, -position.y]])
+            
+            #calculate current euclid distance
+            self.currentEucDistance = ((position.z-self.targetDistance[0])**2 + (-position.x-self.targetDistance[1])**2 + (-position.y-self.targetDistance[2])**2)**0.5
+
+            #set distance log
+            self.lastDistance = self.currentDistance
+            self.distancelog_queue = np.append(self.distancelog_queue, currentDistance_log, axis = 0)
+            if len(self.distancelog_queue) > 10:
+                self.distancelog_queue = np.delete(self.distancelog_queue, 0, axis = 0)
+            #rospy.loginfo(self.distancelog_queue)
+            # self.double_lastDistance = self.lastDistance
+            # self.lastDistance = self.currentDistance
+
+            #set values to current quarter angle
+            orientation = data.detections[0].pose.pose.pose.orientation
+            self.currentQuarter = [orientation.x, orientation.y, orientation.z, orientation.w]
+
+            #calculate current Euler angle
+            self.currentEuler = tf.euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w))
+
+            #set angular log
+            self.lastEuler = self.currentEuler
+
+            #calculate target yaw
+            yaw_change_diff = abs(self.currentEuler[1] - self.pre_yaw_diff)
+            if yaw_change_diff > 0.2:
+                rospy.loginfo("ignore unreliable data!")
+                self.pre_yaw_diff = self.currentEuler[1]
+                return
+            # rospy.loginfo("stable!")
+            self.pre_yaw_diff = self.currentEuler[1]
+            self.yaw = self.currentmocapEuler[2] + self.currentEuler[1]
+            # rospy.loginfo(self.currentmocapEuler[2])
+            # rospy.loginfo(self.currentEuler[1])
+            # rospy.loginfo(self.yaw)
+            
+            self.tag_lost_flag = False
+
+        else:
+            self.tag_lost_flag = True
+
+            #load log
+            average_position = np.mean(self.distancelog_queue, axis = 0)
+            self.currentDistance = self.lastDistance +  self.recovery_gain * (self.lastDistance - average_position)
+            self.currentEuler = self.lastEuler
+
+        #calculate target yaw
+        yaw_change_diff = abs(self.currentEuler[1] - self.pre_yaw_diff)
+        if yaw_change_diff > 0.2:
+            rospy.loginfo("ignore unreliable data!")
+            self.pre_yaw_diff = self.currentEuler[1]
+            return
+        self.pre_yaw_diff = self.currentEuler[1]
+        self.yaw = self.currentmocapEuler[2] + self.currentEuler[1]
+
+    def callback2(self, data):
+        if data.pose:
+            moc = data.pose.orientation
+            self.currentmocapQuarter = [moc.x, moc.y, moc.z, moc.w]
+            #calculate current mocap Euler angular
+            self.currentmocapEuler = tf.euler_from_quaternion((moc.x, moc.y, moc.z, moc.w))
+            self.lastmocapEuler = self.currentmocapEuler
+            self.mocap_lost_flag = False
+
+        else:
+            self.mocap_lost_flag = True
+            self.currentmocapEuler = self.lastmocapEuler
+
+    def callback3(self, data):
+        #set values to current velocity
+        odom = data.twist.twist.linear
+        self.currentVelocity = np.array([[odom.x, odom.y, odom.z]])
+        # rospy.loginfo(self.currentVelocity)
+        if not self.tag_lost_flag:
+            self.velocitylog_queue = np.append(self.velocitylog_queue, self.currentVelocity, axis = 0)
+            if len(self.velocitylog_queue) > 10:
+                self.velocitylog_queue = np.delete(self.velocitylog_queue, 0, axis = 0)
+            average_velocity = np.mean(self.velocitylog_queue, axis = 0)
+            self.vel_recovery = -1.0 * average_velocity
+        
+    def main(self):
+        # self.docking_msg.data = True 
+        while not rospy.is_shutdown():
+            #callback1, 2
+
+            #calcurate error
+            self.calculateError()
+
+            #calculate term
+            self.p_term = self.kp * self.err
+            self.i_term = self.ki * self.err_i
+            self.d_term = self.kd * (self.err - self.pre_err) / self.dt
+
+            self.yaw_p_term = -self.yaw_kp * self.yaw_err
+            self.yaw_i_term = -self.yaw_ki * self.yaw_err_i
+            self.yaw_d_term = -self.yaw_kd * (self.yaw_err - self.yaw_pre_err) / self.dt
+            # rospy.loginfo(self.yaw_p_term)
+            
+            # calcurate terms
+            if self.currentEucDistance <= 0.5:
+                #weaken x PID
+                self.p_term *= self.TermCorrector
+                self.i_term *= self.TermCorrector
+                self.d_term *= self.TermCorrector
+        
+            # calculate main process
+            if not self.tag_lost_flag:
+                self.vel = self.p_term + self.i_term + self.d_term            
+                self.target_yaw = self.yaw_p_term + self.yaw_i_term + self.yaw_d_term
+            else:
+                self.err = 0
+                self.err_i = 0
+                self.pre_err = 0
+                self.yaw_nav_mode = 0
+                self.vel = self.vel_recovery
+        
+            #publish message
+            self.nav_msg.control_frame = 1
+            self.nav_msg.target = 1
+            self.nav_msg.pos_xy_nav_mode = 1
+            
+            self.nav_msg.target_vel_x = self.vel[0]
+            self.nav_msg.target_vel_y = self.vel[1]
+
+            self.defineYaw()
+            self.nav_msg.pos_z_nav_mode = 1
+            self.nav_msg.target_vel_z = self.vel[2]
+            self.pub.publish(self.nav_msg)
+
+            # #beetle1 mode verifier
+            # self.dockingVerifier()
+
+            # #switch beetle1 into assembled mode
+            #  # true ->activated false->disactivate
+            # self.docking_pub.publish(self.docking_msg)
+            
+            #  #flag_msg = KeyValue()
+            #  #flag_msg.key = '1'
+            #  #flag_msg.value = '1' # 1 -> assembly, 2->separate
+            # self.flag_pub_1.publish(self.flag_msg)
+            # self.flag_pub_2.publish(self.flag_msg)
+
+            #rospy.loginfo(self.yaw)
+            # rospy.loginfo(self.currentmocapEuler[2])
+            # rospy.loginfo(self.currenotEuler[1])
+            # rospy.loginfo(self.targetDistance[0])
+            # rospy.loginfo(self.currentDistance[2])
+            # rospy.loginfo(self.err[0])
+            # rospy.loginfo(self.p_term)
+            # rospy.loginfo(self.i_term)
+            #rospy.loginfo(self.target_yaw)
+            rospy.loginfo(self.yaw_err)
+            rospy.loginfo(self.target_yaw)
+            rospy.loginfo("")
+
+            self.rate.sleep()
+            
+if __name__ == "__main__":
+    try:
+        aprilpid = aprilPIDcontroller()
+        aprilpid.main()
+    except rospy.ROSInterruptException: pass