diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 45323c3f82..e07b988dd4 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -295,20 +295,20 @@ void SteeringOdometry::integrate_runge_kutta_2( const double v_bx, const double omega_bz, const double dt) { // Compute intermediate value of the heading - const double Theta_mid = heading_ + omega_bz * 0.5 * dt; + const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(Theta_mid) * dt; - y_ += v_bx * sin(Theta_mid) * dt; + x_ += v_bx * cos(theta_mid) * dt; + y_ += v_bx * sin(theta_mid) * dt; heading_ += omega_bz * dt; } void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) { const double delta_x_b = v_bx * dt; - const double delta_Theta = omega_bz * dt; + const double delta_theta = omega_bz * dt; - if (fabs(delta_Theta) < 1e-6) + if (fabs(delta_theta) < 1e-6) { /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): integrate_runge_kutta_2(v_bx, omega_bz, dt); @@ -317,8 +317,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co { /// Exact integration const double heading_old = heading_; - const double R = delta_x_b / delta_Theta; - heading_ += delta_Theta; + 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)); }