diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index a2d34479f3..ec85a9d6df 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -48,6 +48,8 @@ void VelocitySmoothing::reset(float accel, float vel, float pos)
 	_state.a = accel;
 	_state.v = vel;
 	_state.x = pos;
+
+	_state_init = _state;
 }
 
 float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max)
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index 1930b3aaea..ba817a8b98 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -100,11 +100,11 @@ public:
 	void setMaxVel(float max_vel) { _max_vel = max_vel; }
 
 	float getCurrentJerk() const { return _state.j; }
-	void setCurrentAcceleration(const float accel) { _state.a = accel; }
+	void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; }
 	float getCurrentAcceleration() const { return _state.a; }
-	void setCurrentVelocity(const float vel) { _state.v = vel; }
+	void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; }
 	float getCurrentVelocity() const { return _state.v; }
-	void setCurrentPosition(const float pos) { _state.x = pos; }
+	void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; }
 	float getCurrentPosition() const { return _state.x; }
 
 	float getVelSp() const { return _vel_sp; }