diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
index 48917e892d..172c0c090b 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
@@ -45,27 +45,23 @@ FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth(control::SuperBlo
 	_smoothingZ(_velocity(2), _sticks(2))
 {}
 
-
-bool FlightTaskManualPositionSmooth::activate()
-{
-	_vel_sp_prev_z = _velocity(2);
-	return FlightTaskManualPosition::activate();
-}
-
 void FlightTaskManualPositionSmooth::_updateSetpoints()
 {
-	/* Get yaw setpont, unsmoothed position setpoints */
+	/* Get yaw setpont, un-smoothed position setpoints.*/
 	FlightTaskManualPosition::_updateSetpoints();
 
-	/* Smooth velocity setpoint in xy */
+	/* Smooth velocity setpoint in xy.*/
 	matrix::Vector2f vel(&_velocity(0));
-	_smoothingXY.smoothVelocity(_vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
+	Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
+	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
+	_vel_sp(0) = vel_sp_xy(0);
+	_vel_sp(1) = vel_sp_xy(1);
 
-	/* Check for altitude lock*/
+	/* Check for altitude lock.*/
 	_updateXYlock();
 
-	/* Smooth velocity in z*/
-	_smoothingZ.smoothVelFromSticks(_vel_sp_z, _deltatime);
+	/* Smooth velocity in z.*/
+	_smoothingZ.smoothVelFromSticks(_vel_sp(2), _deltatime);
 
 	/* Check for altitude lock*/
 	_updateAltitudeLock();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp
index e225a32113..b8abc1f90b 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp
@@ -49,14 +49,9 @@ public:
 
 	virtual ~FlightTaskManualPositionSmooth() = default;
 
-	bool activate() override;
-
 protected:
 
 	virtual void _updateSetpoints() override;
-	ManualSmoothingXY _smoothingXY; // Smoothing for velocity setpoints.
-	ManualSmoothingZ _smoothingZ; //Smoothing for velocity in z.
-	float _vel_sp_prev_z{}; // Velocity setpoint from previous iteration.
-
-
+	ManualSmoothingXY _smoothingXY; /**< smoothing for velocity setpoints in xy */
+	ManualSmoothingZ _smoothingZ; /**< smoothing for velocity in z */
 };