diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index c3a7539c5a..d9d95bf8b5 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio
   }
   else
   {
-    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double front_right_steer_position =
-      state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
-    const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+    const double traction_right_wheel_value =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+    const double traction_left_wheel_value =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
+    const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
+    const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
     if (
-      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
-      !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
-          front_left_steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
+          steering_left_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_velocity(
-          rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
-          front_left_steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_right_position,
+          steering_left_position, period.seconds());
       }
     }
   }
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 5f013d7d7a..95eaf1965c 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
@@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
   }
   else
   {
-    const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
-    const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
-    if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position))
+    const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
+    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
-        odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds());
+        odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds());
       }
     }
   }
diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 25d0235818..b0488a7d4a 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -56,7 +56,7 @@ class SteeringOdometry
   /**
    * \brief Updates the odometry class with latest wheels position
    * \param traction_wheel_pos  traction wheel position [rad]
-   * \param steer_pos Front Steer position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -67,7 +67,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest wheels position
    * \param right_traction_wheel_pos  Right traction wheel velocity [rad]
    * \param left_traction_wheel_pos  Left traction wheel velocity [rad]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -91,7 +91,7 @@ class SteeringOdometry
   /**
    * \brief Updates the odometry class with latest wheels position
    * \param traction_wheel_vel  Traction wheel velocity [rad/s]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -102,7 +102,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest wheels position
    * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
    * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -125,11 +125,11 @@ class SteeringOdometry
 
   /**
    * \brief Updates the odometry class with latest velocity command
-   * \param linear  Linear velocity [m/s]
-   * \param angular Angular velocity [rad/s]
-   * \param time    Current time
+   * \param v_bx  Linear velocity   [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt      time difference to last call
    */
-  void update_open_loop(const double linear, const double angular, const double dt);
+  void update_open_loop(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Set odometry type
@@ -170,22 +170,23 @@ class SteeringOdometry
   /**
    * \brief Sets the wheel parameters: radius, separation and wheelbase
    */
-  void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0);
+  void set_wheel_params(
+    const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0);
 
   /**
    * \brief Velocity rolling window size setter
    * \param velocity_rolling_window_size Velocity rolling window size
    */
-  void set_velocity_rolling_window_size(size_t velocity_rolling_window_size);
+  void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size);
 
   /**
    * \brief Calculates inverse kinematics for the desired linear and angular velocities
-   * \param Vx  Desired linear velocity [m/s]
-   * \param theta_dot Desired angular velocity [rad/s]
+   * \param v_bx     Desired linear velocity of the robot in x_b-axis direction
+   * \param omega_bz Desired angular velocity of the robot around x_z-axis
    * \return Tuple of velocity commands and steering commands
    */
   std::tuple<std::vector<double>, std::vector<double>> get_commands(
-    const double Vx, const double theta_dot);
+    const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset poses, heading, and accumulators
@@ -194,35 +195,35 @@ class SteeringOdometry
 
 private:
   /**
-   * \brief Uses precomputed linear and angular velocities to compute dometry and update
-   * accumulators \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt)
-   * computed by previous odometry method \param angular Angular velocity [rad] (angular
-   * displacement, i.e. m/s * dt) computed by previous odometry method
+   * \brief Uses precomputed linear and angular velocities to compute odometry
+   * \param v_bx  Linear  velocity   [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt      time difference to last call
    */
-  bool update_odometry(const double linear_velocity, const double angular, const double dt);
+  bool update_odometry(const double v_bx, const double omega_bz, const double dt);
 
   /**
    * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
-   * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by
-   * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
-   * by encoders
+   * \param v_bx Linear velocity [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt time difference to last call
    */
-  void integrate_runge_kutta_2(double linear, double angular);
+  void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
 
   /**
-   * \brief Integrates the velocities (linear and angular) using exact method
-   * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by
-   * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed
-   * by encoders
+   * \brief Integrates the velocities (linear and angular)
+   * \param v_bx Linear velocity [m/s]
+   * \param omega_bz Angular velocity [rad/s]
+   * \param dt time difference to last call
    */
-  void integrate_exact(double linear, double angular);
+  void integrate_fk(const double v_bx, const double omega_bz, const double dt);
 
   /**
-   * \brief Calculates steering angle from the desired translational and rotational velocity
-   * \param Vx   Linear  velocity   [m]
-   * \param theta_dot Angular velocity [rad]
+   * \brief Calculates steering angle from the desired twist
+   * \param v_bx     Linear velocity of the robot in x_b-axis direction
+   * \param omega_bz Angular velocity of the robot around x_z-axis
    */
-  double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot);
+  double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
   /**
    *  \brief Reset linear and angular accumulators
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 352ba5c792..e663eddf91 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size)
   wheelbase_(0.0),
   wheel_radius_(0.0),
   traction_wheel_old_pos_(0.0),
+  traction_right_wheel_old_pos_(0.0),
+  traction_left_wheel_old_pos_(0.0),
   velocity_rolling_window_size_(velocity_rolling_window_size),
   linear_acc_(velocity_rolling_window_size),
   angular_acc_(velocity_rolling_window_size)
@@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time)
 }
 
 bool SteeringOdometry::update_odometry(
-  const double linear_velocity, const double angular, const double dt)
+  const double linear_velocity, const double angular_velocity, const double dt)
 {
   /// Integrate odometry:
-  SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt);
+  integrate_fk(linear_velocity, angular_velocity, dt);
 
   /// We cannot estimate the speed with very small time intervals:
   if (dt < 0.0001)
@@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry(
 
   /// Estimate speeds using a rolling mean to filter them out:
   linear_acc_.accumulate(linear_velocity);
-  angular_acc_.accumulate(angular);
+  angular_acc_.accumulate(angular_velocity);
 
   linear_ = linear_acc_.getRollingMean();
   angular_ = angular_acc_.getRollingMean();
@@ -73,70 +75,47 @@ bool SteeringOdometry::update_odometry(
 bool SteeringOdometry::update_from_position(
   const double traction_wheel_pos, const double steer_pos, const double dt)
 {
-  /// Get current wheel joint positions:
-  const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_;
-  const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_;
+  const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_wheel_old_pos_ = traction_wheel_cur_pos;
+  traction_wheel_old_pos_ = traction_wheel_pos;
 
-  /// Compute linear and angular diff:
-  const double linear_velocity = traction_wheel_est_pos_diff / dt;
-  steer_pos_ = steer_pos;
-  const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
-
-  return update_odometry(linear_velocity, angular, dt);
+  return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt);
 }
 
 bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double steer_pos, const double dt)
 {
-  /// Get current wheel joint positions:
-  const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_;
-  const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_;
-
   const double traction_right_wheel_est_pos_diff =
-    traction_right_wheel_cur_pos - traction_right_wheel_old_pos_;
+    traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
-    traction_left_wheel_cur_pos - traction_left_wheel_old_pos_;
+    traction_left_wheel_pos - traction_left_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos;
-  traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos;
-
-  const double linear_velocity =
-    (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
-  steer_pos_ = steer_pos;
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  traction_right_wheel_old_pos_ = traction_right_wheel_pos;
+  traction_left_wheel_old_pos_ = traction_left_wheel_pos;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_from_velocity(
+    traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt);
 }
 
 bool SteeringOdometry::update_from_position(
   const double traction_right_wheel_pos, const double traction_left_wheel_pos,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-  /// Get current wheel joint positions:
-  const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_;
-  const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_;
-
   const double traction_right_wheel_est_pos_diff =
-    traction_right_wheel_cur_pos - traction_right_wheel_old_pos_;
+    traction_right_wheel_pos - traction_right_wheel_old_pos_;
   const double traction_left_wheel_est_pos_diff =
-    traction_left_wheel_cur_pos - traction_left_wheel_old_pos_;
+    traction_left_wheel_pos - traction_left_wheel_old_pos_;
 
   /// Update old position with current:
-  traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos;
-  traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos;
-
-  /// Compute linear and angular diff:
-  const double linear_velocity =
-    (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt;
-  steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  traction_right_wheel_old_pos_ = traction_right_wheel_pos;
+  traction_left_wheel_old_pos_ = traction_left_wheel_pos;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_from_velocity(
+    traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos,
+    left_steer_pos, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -144,9 +123,9 @@ bool SteeringOdometry::update_from_velocity(
 {
   steer_pos_ = steer_pos;
   double linear_velocity = traction_wheel_vel * wheel_radius_;
-  const double angular = tan(steer_pos) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -157,9 +136,9 @@ bool SteeringOdometry::update_from_velocity(
     (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
   steer_pos_ = steer_pos;
 
-  const double angular = tan(steer_pos_) * linear_velocity / wheelbase_;
+  const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
 bool SteeringOdometry::update_from_velocity(
@@ -169,19 +148,19 @@ bool SteeringOdometry::update_from_velocity(
   steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
   double linear_velocity =
     (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
-  const double angular = steer_pos_ * linear_velocity / wheelbase_;
+  const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_;
 
-  return update_odometry(linear_velocity, angular, dt);
+  return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
-void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt)
+void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt)
 {
   /// Save last linear and angular velocity:
-  linear_ = linear;
-  angular_ = angular;
+  linear_ = v_bx;
+  angular_ = omega_bz;
 
   /// Integrate odometry:
-  SteeringOdometry::integrate_exact(linear * dt, angular * dt);
+  integrate_fk(v_bx, omega_bz, dt);
 }
 
 void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
@@ -200,36 +179,37 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_
 
 void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; }
 
-double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot)
+double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
 {
-  if (theta_dot == 0 || Vx == 0)
+  if (omega_bz == 0 || v_bx == 0)
   {
     return 0;
   }
-  return std::atan(theta_dot * wheelbase_ / Vx);
+  return std::atan(omega_bz * wheelbase_ / v_bx);
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
-  const double Vx, const double theta_dot)
+  const double v_bx, const double omega_bz)
 {
-  // desired velocity and steering angle of the middle of traction and steering axis
-  double Ws, alpha;
+  // desired wheel speed and steering angle of the middle of traction and steering axis
+  double Ws, phi;
 
-  if (Vx == 0 && theta_dot != 0)
+  if (v_bx == 0 && omega_bz != 0)
   {
-    alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2;
-    Ws = abs(theta_dot) * wheelbase_ / wheel_radius_;
+    // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller
+    phi = omega_bz > 0 ? M_PI_2 : -M_PI_2;
+    Ws = abs(omega_bz) * wheelbase_ / wheel_radius_;
   }
   else
   {
-    alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot);
-    Ws = Vx / (wheel_radius_ * std::cos(steer_pos_));
+    phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz);
+    Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_));
   }
 
   if (config_type_ == BICYCLE_CONFIG)
   {
     std::vector<double> traction_commands = {Ws};
-    std::vector<double> steering_commands = {alpha};
+    std::vector<double> steering_commands = {phi};
     return std::make_tuple(traction_commands, steering_commands);
   }
   else if (config_type_ == TRICYCLE_CONFIG)
@@ -242,12 +222,12 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     }
     else
     {
-      double turning_radius = wheelbase_ / std::tan(steer_pos_);
-      double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
-      double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
+      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
+      const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
     }
-    steering_commands = {alpha};
+    steering_commands = {phi};
     return std::make_tuple(traction_commands, steering_commands);
   }
   else if (config_type_ == ACKERMANN_CONFIG)
@@ -257,21 +237,23 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     if (fabs(steer_pos_) < 1e-6)
     {
       traction_commands = {Ws, Ws};
-      steering_commands = {alpha, alpha};
+      steering_commands = {phi, phi};
     }
     else
     {
-      double turning_radius = wheelbase_ / std::tan(steer_pos_);
-      double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
-      double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
+      const double turning_radius = wheelbase_ / std::tan(steer_pos_);
+      const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius;
+      const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius;
       traction_commands = {Wr, Wl};
 
-      double numerator = 2 * wheelbase_ * std::sin(alpha);
-      double denominator_first_member = 2 * wheelbase_ * std::cos(alpha);
-      double denominator_second_member = wheel_track_ * std::sin(alpha);
+      const double numerator = 2 * wheelbase_ * std::sin(phi);
+      const double denominator_first_member = 2 * wheelbase_ * std::cos(phi);
+      const double denominator_second_member = wheel_track_ * std::sin(phi);
 
-      double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
-      double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);
+      const double alpha_r =
+        std::atan2(numerator, denominator_first_member + denominator_second_member);
+      const double alpha_l =
+        std::atan2(numerator, denominator_first_member - denominator_second_member);
       steering_commands = {alpha_r, alpha_l};
     }
     return std::make_tuple(traction_commands, steering_commands);
@@ -290,35 +272,36 @@ void SteeringOdometry::reset_odometry()
   reset_accumulators();
 }
 
-void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular)
+void SteeringOdometry::integrate_runge_kutta_2(
+  const double v_bx, const double omega_bz, const double dt)
 {
-  const double direction = heading_ + angular * 0.5;
+  // Compute intermediate value of the heading
+  const double theta_mid = heading_ + omega_bz * 0.5 * dt;
 
-  /// Runge-Kutta 2nd order integration:
-  x_ += linear * cos(direction);
-  y_ += linear * sin(direction);
-  heading_ += angular;
+  // Use the intermediate values to update the state
+  x_ += v_bx * cos(theta_mid) * dt;
+  y_ += v_bx * sin(theta_mid) * dt;
+  heading_ += omega_bz * dt;
 }
 
-/**
- * \brief Other possible integration method provided by the class
- * \param linear
- * \param angular
- */
-void SteeringOdometry::integrate_exact(double linear, double angular)
+void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt)
 {
-  if (fabs(angular) < 1e-6)
+  const double delta_x_b = v_bx * dt;
+  const double delta_theta = omega_bz * dt;
+
+  if (fabs(delta_theta) < 1e-6)
   {
-    integrate_runge_kutta_2(linear, angular);
+    /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero):
+    integrate_runge_kutta_2(v_bx, omega_bz, dt);
   }
   else
   {
-    /// Exact integration (should solve problems when angular is zero):
+    /// Exact integration
     const double heading_old = heading_;
-    const double r = linear / angular;
-    heading_ += angular;
-    x_ += r * (sin(heading_) - sin(heading_old));
-    y_ += -r * (cos(heading_) - cos(heading_old));
+    const double R = delta_x_b / delta_theta;
+    heading_ += delta_theta;
+    x_ += R * (sin(heading_) - sin(heading_old));
+    y_ += -R * (cos(heading_) - cos(heading_old));
   }
 }
 
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index f89d78a52c..03be6b88f0 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -60,24 +60,28 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
   }
   else
   {
-    const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    const double traction_right_wheel_value =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
+    const double traction_left_wheel_value =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
+    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
     if (
-      !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
-      !std::isnan(steer_position))
+      std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
+      std::isfinite(steering_position))
     {
       if (params_.position_feedback)
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_position(
-          rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_position,
+          period.seconds());
       }
       else
       {
         // Estimate linear and angular velocity using joint information
         odometry_.update_from_velocity(
-          rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds());
+          traction_right_wheel_value, traction_left_wheel_value, steering_position,
+          period.seconds());
       }
     }
   }