diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
index 71b5b7c5..bbb862e5 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -181,6 +181,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   double publish_rate_{};
   bool state_changed_{};
   int loop_count_{};
+  double last_pos_des_[3]{ 0. };
 
   // Transform
   geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_;
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index e98d628b..1eaed2e3 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -452,7 +452,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
         tf2::doTransform(target_vel, target_vel, transform);
         tf2::fromMsg(target_pos, target_pos_tf);
         tf2::fromMsg(target_vel, target_vel_tf);
-        vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2);
+        vel_des[2] = angles::shortest_angular_distance(last_pos_des_[2], pos_des[2]) / period.toSec();
+        last_pos_des_[2] = pos_des[2];
       }
       if (joint_urdfs_.find(1) != joint_urdfs_.end())
       {