diff --git a/CMakeLists.txt b/CMakeLists.txt
index fc56aa9..231175b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -22,7 +22,7 @@ option(CHECK_TIDY "Adds clang-tidy tests" OFF)
 
 option(ENABLE_NATIVE_OPTIMIZATION
   "Enable CPU-specific optimizations (-march=native)"
-  OFF
+  ON
 )
 
 # Default to release build
diff --git a/include/crisp_controllers/utils/fiters.hpp b/include/crisp_controllers/utils/fiters.hpp
index 1a59200..3a90842 100644
--- a/include/crisp_controllers/utils/fiters.hpp
+++ b/include/crisp_controllers/utils/fiters.hpp
@@ -10,13 +10,13 @@
   * @brief Compute the exponential moving average of a value
   * @param output Previous output value
   * @param current Current value measured to be filtered
-   * @param alpha Smoothing factor (0 < alpha < 1). The closer to 1, the more it
+   * @param alpha Smoothing factor (0 < alpha < 1). The closer to 0, the more it
   * smooths the value
   * @return returns the filtered value
   */
 template <typename T>
-inline T exponential_moving_average(const T output, const T current, const double alpha) {
-  return (1.0 - alpha) * current + alpha * output;
+inline T exponential_moving_average(const T prev, const T current, const double alpha) {
+  return (1.0 - alpha) * prev + alpha * current;
 }
 
 /**
diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index c16a7c4..99a9978 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -86,7 +86,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
   desired_position_ =
     exponential_moving_average(desired_position_, target_position_, params_.filter.target_pose);
   desired_orientation_ =
-    target_orientation_.slerp(params_.filter.target_pose, desired_orientation_);
+    desired_orientation_.slerp(params_.filter.target_pose, target_orientation_);
 
   /*target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(),
    * target_position_);*/
@@ -183,10 +183,11 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
   tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + tau_gravity + tau_joint_limits +
       tau_wrench;
 
+  tau_d = exponential_moving_average(tau_previous, tau_d, params_.filter.output_torque);
+
   if (params_.limit_torques) {
     tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau);
   }
-  tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque);
 
   if (!params_.stop_commands) {
     for (size_t i = 0; i < params_.joints.size(); ++i) {
@@ -320,7 +321,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   new_target_wrench_ = false;
 
   multiple_publishers_detected_ = false;
-  max_allowed_publishers_ = 1;
+  max_allowed_publishers_ = 2;
 
   auto target_pose_callback =
     [this](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {