diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h
index 39a5eb820..f01b57c6e 100644
--- a/diff_drive_controller/include/diff_drive_controller/odometry.h
+++ b/diff_drive_controller/include/diff_drive_controller/odometry.h
@@ -198,6 +198,7 @@ namespace diff_drive_controller
     /// Previou wheel position/state [rad]:
     double left_wheel_old_pos_;
     double right_wheel_old_pos_;
+    bool old_pos_valid_;
 
     /// Rolling mean accumulators for the linar and angular velocities:
     size_t velocity_rolling_window_size_;
diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp
index d086d5ccf..e89542fcb 100644
--- a/diff_drive_controller/src/odometry.cpp
+++ b/diff_drive_controller/src/odometry.cpp
@@ -59,6 +59,7 @@ namespace diff_drive_controller
   , right_wheel_radius_(0.0)
   , left_wheel_old_pos_(0.0)
   , right_wheel_old_pos_(0.0)
+  , old_pos_valid_(false)
   , velocity_rolling_window_size_(velocity_rolling_window_size)
   , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
   , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
@@ -79,6 +80,15 @@ namespace diff_drive_controller
     const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
     const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
 
+    if (!old_pos_valid_)
+    {
+      /// Set old position with current to retain true velocities when initial wheel position is non-zero:
+      left_wheel_old_pos_ = left_wheel_cur_pos;
+      right_wheel_old_pos_ = right_wheel_cur_pos;
+      old_pos_valid_ = true;
+      return false;
+    }
+
     /// Estimate velocity of wheels using old and current position:
     const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
     const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
@@ -171,6 +181,7 @@ namespace diff_drive_controller
   {
     linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
     angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+    old_pos_valid_ = false;
   }
 
 } // namespace diff_drive_controller