Skip to content

Commit

Permalink
Use a lowercase theta for heading
Browse files Browse the repository at this point in the history
Co-authored-by: Sai Kishor Kothakota <[email protected]>
  • Loading branch information
christophfroehlich and saikishor authored May 27, 2024
1 parent e115836 commit 6942c1a
Showing 1 changed file with 7 additions and 7 deletions.
14 changes: 7 additions & 7 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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));
}
Expand Down

0 comments on commit 6942c1a

Please sign in to comment.