diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
index ddd0f1c..ff17434 100644
--- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
+++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
@@ -53,7 +53,7 @@ protected:
   rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
   std::shared_mutex mutex_;
-  RollingAverage avg_ = RollingAverage(5);
+  RollingAverage avg_ = RollingAverage(10);
 
   //Goal
   geometry_msgs::msg::Pose new_target_pose_;
diff --git a/toid_behaviors/include/toid_behaviors/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp
index 9c5b2dd..1a9050f 100644
--- a/toid_behaviors/include/toid_behaviors/rolling_average.hpp
+++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp
@@ -18,10 +18,10 @@ public:
 
   Pose2D push(geometry_msgs::msg::Pose & pose)
   {
-    if(size_ > 0) {
+    if(size_ == 0) {
         return {};
     }
-    if (size_ == data_count_) {
+    if (size_-1 == data_count_) {
       accum_x_ -= poses_[front_idx_].position.x;
       accum_y_ -= poses_[front_idx_].position.y;
       accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);
diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp
index 85c0a5d..b3f0f3f 100644
--- a/toid_behaviors/src/approach_acorns.cpp
+++ b/toid_behaviors/src/approach_acorns.cpp
@@ -111,21 +111,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
 
 
-  if(debug_marker_) {
-    visualization_msgs::msg::Marker marker;
-    marker.lifetime.sec = 1.0;
-    marker.header = pose_global.header;
-    marker.pose = pose_global.pose;
-    marker.type = visualization_msgs::msg::Marker::ARROW;
-    marker.scale.x = 0.02;
-    marker.scale.y = 0.02;
-    marker.scale.z = 0.02;
-    marker.color.a = 1.0;
-    marker.color.r = 1.0;
-    marker.color.g = 0;
-    marker.color.b = 0;
-    target_pose_pub_->publish(marker);
-  }
 
   yaw = angles::normalize_angle(yaw);
 
@@ -136,6 +121,23 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   new_target_pose_.position.y = b; 
   tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation);
   new_target_angle_ = c;
+
+  if(debug_marker_) {
+    visualization_msgs::msg::Marker marker;
+    marker.lifetime.sec = 1.0;
+    marker.header = pose_global.header;
+    marker.pose = new_target_pose_;
+    marker.type = visualization_msgs::msg::Marker::ARROW;
+    marker.scale.x = 0.02;
+    marker.scale.y = 0.02;
+    marker.scale.z = 0.02;
+    marker.color.a = 1.0;
+    marker.color.r = 1.0;
+    marker.color.g = 0;
+    marker.color.b = 0;
+    target_pose_pub_->publish(marker);
+  }
+  
 }
 
 ResultStatus ApproachAcorns::onStart(