diff --git a/orange_bringup/launch/orange_robot.launch.xml b/orange_bringup/launch/orange_robot.launch.xml
index 0c816c8..e884ac5 100644
--- a/orange_bringup/launch/orange_robot.launch.xml
+++ b/orange_bringup/launch/orange_robot.launch.xml
@@ -17,7 +17,7 @@
   <arg name="cmd_dist_topic" default="/pos/cmd_dist"/>
   <arg name="odom_topic" default="/odom"/>
   <arg name="imu_topic" default="/livox/imu"/>
-  <arg name="fusion_odom_topic" default="/fusion/odom"/>
+  <arg name="fusion_odom_topic" default="/localization/odom"/>
   <arg name="ekf_publish_TF" default="false"/>
   <arg name="hokuyo_scan_topic" default="/hokuyo_scan"/>
   <arg name="velodyne_scan_topic" default="/velodyne_scan"/>
@@ -78,12 +78,14 @@
     <arg name="fusion_odom_out" value="$(var fusion_odom_topic)"/>
   </include>-->
   <!-- GPSposition_publisher -->
-  <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 291.09504 -->
-  <node pkg="orange_bringup" exec="fix_to_GPSodom" output="screen">
+  <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->
+  <node pkg="orange_bringup" exec="get_lonlat" output="screen">
     <param name="port" value="/dev/sensors/GNSSbase"/>
     <param name="baud" value="9600"/>
     <param name="country_id" value="0"/>
-    <param name="heading" value="180"/>
+    <param name="type" value="1"/>
+  </node>
+  <node pkg="orange_bringup" exec="lonlat_to_odom" output="screen">
     <param name="Position_magnification" value="1.675"/>
   </node>
   <!-- GPSheading_publisher -->
@@ -92,14 +94,36 @@
     <param name="baud" value="9600"/>
     <param name="time_out" value="1.0"/>
   </node>
+  <!-- 
   <node pkg="orange_bringup" exec="GPSodom_correction" output="screen">
   </node>
+   -->
   <!-- combination_GPSposition_GPSheading -->
   <node pkg="orange_bringup" exec="combination" output="screen">
   </node>
   <!-- ekf_myself -->
   <node pkg="orange_bringup" exec="ekf_myself" output="screen">
-    <param name="ekf_publish_TF" value="$(var ekf_publish_TF)"/>
+    <param name="ekf_publish_TF" value="True"/>
+    <param name="speed_limit" value="3.5"/>
+    <param name="speed_stop" value="0.15"/>
+  </node>
+  <!-- motor_driver_node -->
+  <node pkg="orange_bringup" exec="motor_driver_node" output="screen">
+    <param from="$(var motor_driver_config_file_path)"/>
+    <param name="port" value="/dev/ZLAC8015D"/>
+    <param name="control_mode" value="$(var control_mode)"/>
+    <param name="debug" value="$(var debug_motor)"/>
+    <param name="twist_cmd_vel_topic" value="$(var twist_cmd_vel_topic)"/>
+    <param name="cmd_vel_topic" value="$(var cmd_vel_topic)"/>
+    <param name="cmd_rpm_topic" value="$(var cmd_rpm_topic)"/>
+    <param name="cmd_deg_topic" value="$(var cmd_deg_topic)"/>
+    <param name="cmd_dist_topic" value="$(var cmd_dist_topic)"/>
+    <param name="publish_TF" value="$(var publish_TF)"/>
+    <param name="TF_header_frame" value="odom"/>
+    <param name="TF_child_frame" value="base_footprint"/>
+    <param name="publish_odom" value="$(var publish_odom)"/>
+    <param name="odom_header_frame" value="odom"/>
+    <param name="odom_child_frame" value="base_footprint"/>
   </node>
   <!-- hokuyo -->
   <!--<node pkg="urg_node" exec="urg_node_driver">
diff --git a/orange_bringup/orange_bringup/ekf_myself.py b/orange_bringup/orange_bringup/ekf_myself.py
index 0561458..751d74e 100644
--- a/orange_bringup/orange_bringup/ekf_myself.py
+++ b/orange_bringup/orange_bringup/ekf_myself.py
@@ -17,6 +17,7 @@ def __init__(self):
 
         self.GTheta = None
         self.GTheta0 = None
+        self.GOffset = 0
         self.GPSthetayaw0 = 0
         self.DGPStheta = 0
         self.w = None
@@ -31,11 +32,11 @@ def __init__(self):
         self.XX = None
         self.prev_time = None
         self.prev_pos = None
+        self.prev_GPSpos = None
         self.Speed = 0
         self.SmpTime = 0.1
         self.GpsXY = None
         self.GPS_conut = 0
-        self.GOffset = 0
         self.offsetyaw = 0
         self.combineyaw = 0
         self.robot_yaw = 0
@@ -43,6 +44,7 @@ def __init__(self):
         self.robot_orientationz = 0
         self.robot_orientationw = 0
         self.Number_of_satellites = 0
+        self.is_static = False
 
         self.sub_a = self.create_subscription(
             Odometry, '/odom_fast', self.sensor_a_callback, 10)
@@ -53,6 +55,13 @@ def __init__(self):
         self.ekf_publish_TF = self.get_parameter(
             "ekf_publish_TF").get_parameter_value().bool_value
 
+        self.declare_parameter("speed_limit", 3.5)
+        self.speed_limit = self.get_parameter(
+            "speed_limit").get_parameter_value().double_value
+        self.declare_parameter("speed_stop", 0.15)
+        self.speed_stop = self.get_parameter(
+            "speed_stop").get_parameter_value().double_value
+
         self.t = TransformStamped()
         self.br = tf2_ros.TransformBroadcaster(self)
 
@@ -80,17 +89,31 @@ def sensor_a_callback(self, data):
             self.SmpTime = current_time - self.prev_time
         else:
             self.SmpTime = 0.1
+
         self.prev_time = current_time
 
         current_pos = np.array([
             data.pose.pose.position.x,
             data.pose.pose.position.y
         ])
+
         if self.prev_pos is not None:
             distance = np.linalg.norm(current_pos - self.prev_pos)
             self.Speed = distance / self.SmpTime
         else:
             self.Speed = 0
+
+        if self.Speed > self.speed_limit:
+            self.Speed = 0
+            # self.get_logger().info("Speed correction")
+
+        # self.get_logger().info(f"Speed: {self.Speed}")
+
+        if self.Speed < self.speed_stop:  # 0.15
+            self.is_static = True
+        else:
+            self.is_static = False
+
         self.prev_pos = current_pos
 
         self.GTheta = self.orientation_to_yaw(
@@ -99,6 +122,17 @@ def sensor_a_callback(self, data):
     def sensor_b_callback(self, data):
         self.GpsXY = np.array(
             [data.pose.pose.position.x, data.pose.pose.position.y])
+        # self.get_logger().info(f"self.GpsXY      : {self.GpsXY}")
+
+        if self.is_static:  # stationary state(true)
+            self.get_logger().info("stationary state")
+            if self.prev_GPSpos is None:
+                self.GpsXY = np.array([0, 0])
+                # self.get_logger().info("non self.prev_GPSpos")
+            else:
+                self.GpsXY = self.prev_GPSpos
+        else:
+            self.prev_GPSpos = self.GpsXY
 
         self.GPStheta = self.orientation_to_yaw(
             data.pose.pose.orientation.z, data.pose.pose.orientation.w)
@@ -109,14 +143,10 @@ def sensor_b_callback(self, data):
 
         self.Number_of_satellites = data.pose.covariance[0]  #
 
-    def determination_of_R(self):
-        if 0 <= self.Number_of_satellites < 4:  # Bad
-            self.R1 = 1e-2  # 0.01 FAST-LIO
-            self.R2 = 9e-2  # 0.09 CLAS-movingbase
-            self.R3 = 9     # GTheta
-            self.R4 = 1     # GPStheta
+        # self.get_logger().info(f"self.Number_of_satellites: {self.Number_of_satellites}")
 
-        elif 4 <= self.Number_of_satellites < 8:  # So-so
+    def determination_of_R(self):
+        if 4 <= self.Number_of_satellites < 8:  # So-so...
             self.R1 = 6e-2  # 0.06 FAST-LIO
             self.R2 = 4e-2  # 0.04 CLAS-movingbase
             self.R3 = 4     # GTheta
@@ -242,52 +272,58 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2):
         return theta_sum
 
     def calculate_offset(self, combyaw, GTheta, GPStheta):
-        deference = abs(GTheta) + abs(GPStheta)
+        abs_GTheta = abs(GTheta)
+        abs_GPStheta = abs(GPStheta)
+        abs_combyaw = abs(combyaw)
+        pi = math.pi
+
+        deference = abs_GTheta + abs_GPStheta
 
         if GTheta > 0 and GPStheta < 0 and combyaw > 0:
             self.GOffset = -(GTheta - combyaw)
         elif GTheta > 0 and GPStheta < 0 and combyaw < 0:
-            self.GOffset = -(GTheta + abs(combyaw))
+            self.GOffset = -(GTheta + abs_combyaw)
         elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta:
-            self.GOffset = -(abs(combyaw) - abs(GTheta))
+            self.GOffset = -(abs_combyaw - abs_GTheta)
         elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta:
-            self.GOffset = -(GTheta + abs(combyaw))
+            self.GOffset = -(GTheta + abs_combyaw)
         elif GTheta < 0 and GPStheta > 0 and combyaw > 0:
-            self.GOffset = abs(GTheta) + combyaw
+            self.GOffset = abs_GTheta + combyaw
         elif GTheta < 0 and GPStheta > 0 and combyaw < 0:
-            self.GOffset = abs(GTheta) - abs(combyaw)
+            self.GOffset = abs_GTheta - abs_combyaw
         elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta:
             self.GOffset = combyaw - GTheta
         elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta:
-            self.GOffset = abs(GTheta) - abs(combyaw)
-        elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > math.pi:
+            self.GOffset = abs_GTheta - abs_combyaw
+        elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi:
             self.GOffset = combyaw - GTheta
-        elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > math.pi:
-            self.GOffset = math.pi - GTheta + math.pi - abs(combyaw)
-        elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > math.pi:
-            self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta)))
-        elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > math.pi:
-            self.GOffset = -(abs(combyaw) - abs(GTheta))
-
-        if abs(self.GOffset) > 5 * math.pi / 180:  # not -0.0872 ~ 0.0872
+        elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi:
+            self.GOffset = pi - GTheta + pi - abs_combyaw
+        elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi:
+            self.GOffset = -((pi - combyaw) + (pi - abs_GTheta))
+        elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi:
+            self.GOffset = -(abs_combyaw - abs_GTheta)
+
+        if abs(self.GOffset) > 5 * pi / 180:  # not -0.0872 ~ 0.0872
             self.GOffset = 0
             self.get_logger().warn("GOffset warning")
 
+        # self.get_logger().info(f"GOffset: {self.GOffset}") ok
+
         return self.GOffset
 
     def publish_fused_value(self):
         if self.Speed is not None and self.SmpTime is not None and self.GTheta is not None:
-            R = self.determination_of_R()
-            self.R1 = R[0]
-            self.R2 = R[1]
-            self.R3 = R[2]
-            self.R4 = R[3]
-
-            if self.GpsXY is not None:
+            if self.GpsXY is not None and self.Number_of_satellites >= 4:
+                R = self.determination_of_R()
+                self.R1 = R[0]
+                self.R2 = R[1]
+                self.R3 = R[2]
+                self.R4 = R[3]
                 fused_value = self.KalfGPSXY(
                     self.Speed, self.SmpTime, self.GTheta, self.GpsXY, self.R1, self.R2)
                 self.GPS_conut += 1
-                if self.GPS_conut % 10 == 0:
+                if self.GPS_conut % 20 == 0:
                     self.combyaw = self.combine_yaw(
                         self.DGPStheta, self.GTheta, self.GPStheta, self.R3, self.R4)
                     self.offsetyaw = self.calculate_offset(
@@ -310,6 +346,9 @@ def publish_fused_value(self):
                 self.fused_msg.pose.pose.orientation.w = float(
                     self.robot_orientationw)
             else:
+                self.R1 = 0.13**2  # ?
+                self.R2 = 0.13**2  # ?
+
                 fused_value = self.KalfXY(
                     self.Speed, self.SmpTime, self.GTheta, self.R1, self.R2)
                 self.robot_yaw = self.GTheta + self.offsetyaw
diff --git a/orange_bringup/orange_bringup/ekf_myself_noGPS.py b/orange_bringup/orange_bringup/ekf_myself_noGPS.py
index b8d32b5..72d2c0e 100755
--- a/orange_bringup/orange_bringup/ekf_myself_noGPS.py
+++ b/orange_bringup/orange_bringup/ekf_myself_noGPS.py
@@ -216,38 +216,45 @@ def combine_yaw(self, Dtheta, theta1, theta2, w1, w2):
 
         return theta_sum
 
-    def calculate_offset(self, combyaw, GTheta, odom2theta):
-        deference = abs(GTheta) + abs(odom2theta)
+    def calculate_offset(self, combyaw, GTheta, GPStheta):
+        abs_GTheta = abs(GTheta)
+        abs_GPStheta = abs(GPStheta)
+        abs_combyaw = abs(combyaw)
+        pi = math.pi
 
-        if GTheta > 0 and odom2theta < 0 and combyaw > 0:
+        deference = abs_GTheta + abs_GPStheta
+
+        if GTheta > 0 and GPStheta < 0 and combyaw > 0:
             self.GOffset = -(GTheta - combyaw)
-        elif GTheta > 0 and odom2theta < 0 and combyaw < 0:
-            self.GOffset = -(GTheta + abs(combyaw))
-        elif GTheta > 0 and odom2theta > 0 and combyaw > 0 and GTheta > odom2theta:
-            self.GOffset = -(abs(combyaw) - abs(GTheta))
-        elif GTheta < 0 and odom2theta < 0 and combyaw < 0 and GTheta > odom2theta:
-            self.GOffset = -(GTheta + abs(combyaw))
-        elif GTheta < 0 and odom2theta > 0 and combyaw > 0:
-            self.GOffset = abs(GTheta) + combyaw
-        elif GTheta < 0 and odom2theta > 0 and combyaw < 0:
-            self.GOffset = abs(GTheta) - abs(combyaw)
-        elif GTheta > 0 and odom2theta > 0 and combyaw > 0 and GTheta < odom2theta:
+        elif GTheta > 0 and GPStheta < 0 and combyaw < 0:
+            self.GOffset = -(GTheta + abs_combyaw)
+        elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta > GPStheta:
+            self.GOffset = -(abs_combyaw - abs_GTheta)
+        elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta > GPStheta:
+            self.GOffset = -(GTheta + abs_combyaw)
+        elif GTheta < 0 and GPStheta > 0 and combyaw > 0:
+            self.GOffset = abs_GTheta + combyaw
+        elif GTheta < 0 and GPStheta > 0 and combyaw < 0:
+            self.GOffset = abs_GTheta - abs_combyaw
+        elif GTheta > 0 and GPStheta > 0 and combyaw > 0 and GTheta < GPStheta:
             self.GOffset = combyaw - GTheta
-        elif GTheta < 0 and odom2theta < 0 and combyaw < 0 and GTheta < odom2theta:
-            self.GOffset = abs(GTheta) - abs(combyaw)
-        elif GTheta > 0 and odom2theta < 0 and combyaw > 0 and deference > math.pi:
+        elif GTheta < 0 and GPStheta < 0 and combyaw < 0 and GTheta < GPStheta:
+            self.GOffset = abs_GTheta - abs_combyaw
+        elif GTheta > 0 and GPStheta < 0 and combyaw > 0 and deference > pi:
             self.GOffset = combyaw - GTheta
-        elif GTheta > 0 and odom2theta < 0 and combyaw < 0 and deference > math.pi:
-            self.GOffset = math.pi - GTheta + math.pi - abs(combyaw)
-        elif GTheta < 0 and odom2theta > 0 and combyaw > 0 and deference > math.pi:
-            self.GOffset = -((math.pi - combyaw) + (math.pi - abs(GTheta)))
-        elif GTheta < 0 and odom2theta > 0 and combyaw < 0 and deference > math.pi:
-            self.GOffset = -(abs(combyaw) - abs(GTheta))
-
-        if abs(self.GOffset) > 5 * math.pi / 180:  # not -0.0872 ~ 0.0872
+        elif GTheta > 0 and GPStheta < 0 and combyaw < 0 and deference > pi:
+            self.GOffset = pi - GTheta + pi - abs_combyaw
+        elif GTheta < 0 and GPStheta > 0 and combyaw > 0 and deference > pi:
+            self.GOffset = -((pi - combyaw) + (pi - abs_GTheta))
+        elif GTheta < 0 and GPStheta > 0 and combyaw < 0 and deference > pi:
+            self.GOffset = -(abs_combyaw - abs_GTheta)
+
+        if abs(self.GOffset) > 5 * pi / 180:  # not -0.0872 ~ 0.0872
             self.GOffset = 0
             self.get_logger().warn("GOffset warning")
 
+        # self.get_logger().info(f"GOffset: {self.GOffset}") ok
+
         return self.GOffset
 
     def publish_fused_value(self):
@@ -256,7 +263,7 @@ def publish_fused_value(self):
                 fused_value = self.Kalfodom2XY(
                     self.Speed, self.SmpTime, self.GTheta, self.odom2XY, self.R1, self.R2)
                 self.conut += 1
-                if self.conut % 10 == 0:
+                if self.conut % 20 == 0:
                     self.combyaw = self.combine_yaw(
                         self.DTheta, self.GTheta, self.odom2theta, self.R3, self.R4)
                     self.offsetyaw = self.calculate_offset(
diff --git a/orange_bringup/orange_bringup/fix_to_GPSodom.py b/orange_bringup/orange_bringup/fix_to_GPSodom.py
index e676e85..3b37ece 100644
--- a/orange_bringup/orange_bringup/fix_to_GPSodom.py
+++ b/orange_bringup/orange_bringup/fix_to_GPSodom.py
@@ -153,7 +153,8 @@ def conversion(self, coordinate, origin, theta):
         r_theta = theta * degree_to_radian
         h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y
         h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y
-        point = (h_y, -h_x)
+        point = (-h_y, h_x)
+        # point = (h_y, -h_x)
 
         return point
 
diff --git a/orange_bringup/orange_bringup/lonlat_to_odom.py b/orange_bringup/orange_bringup/lonlat_to_odom.py
index 269c7e8..39e70d6 100755
--- a/orange_bringup/orange_bringup/lonlat_to_odom.py
+++ b/orange_bringup/orange_bringup/lonlat_to_odom.py
@@ -108,7 +108,8 @@ def conversion(self, coordinate, origin, theta):
         r_theta = theta * degree_to_radian
         h_x = math.cos(r_theta) * gps_x - math.sin(r_theta) * gps_y
         h_y = math.sin(r_theta) * gps_x + math.cos(r_theta) * gps_y
-        point = (h_y, -h_x)
+        point = (-h_y, h_x)
+        # point = (h_y, -h_x)
 
         return point