diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index c8c4e3bed2d01..2b8358eb52e4e 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -1771,11 +1771,12 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
object.kinematics.acceleration_with_covariance.accel.linear.y)
: 0.0;
const double t_h = time_horizon;
- const double λ = std::log(2) / acceleration_exponential_half_life_;
+ const double lambda = std::log(2) / acceleration_exponential_half_life_;
auto get_search_distance_with_decaying_acc = [&]() -> double {
const double acceleration_distance =
- obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
+ obj_acc * (1.0 / lambda) * t_h +
+ obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
double search_dist = acceleration_distance + obj_vel * t_h;
return search_dist;
};
@@ -1787,13 +1788,13 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
return obj_vel * t_h;
}
// Time to reach final speed
- const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
+ const double t_f = (-1.0 / lambda) * std::log(1 - ((final_speed - obj_vel) * lambda) / obj_acc);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
const double search_dist =
// Distance covered while accelerating
- obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
- obj_vel * t_f +
+ obj_acc * (1.0 / lambda) * t_f +
+ obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) + obj_vel * t_f +
// Distance covered at constant speed
final_speed * (t_h - t_f);
return search_dist;
@@ -1807,7 +1808,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
double final_speed_after_acceleration =
- obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
+ obj_vel + obj_acc * (1.0 / lambda) * (1.0 - std::exp(-lambda * t_h));
const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index aeb1b0fedd33f..dac034a6b7a7d 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
// Using a decaying acceleration model. Consult the README for more information about the model.
const double t_h = duration;
- const float λ = std::log(2) / acceleration_exponential_half_life_;
+ const float lambda = std::log(2) / acceleration_exponential_half_life_;
auto have_same_sign = [](double a, double b) -> bool {
return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0);
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
return v;
}
// Get velocity after time horizon
- const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h));
+ const auto terminal_velocity = v + a * (1.0 / lambda) * (1 - std::exp(-lambda * t_h));
// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
// most stop, not reverse its direction)
@@ -443,15 +443,16 @@ FrenetPoint PathGenerator::getFrenetPoint(
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
// time t_stop
- // 0 = Vo + acc(1/λ)(1-e^(-λt_stop))
- // e^(-λt_stop) = 1 - (-Vo* λ)/acc
- // t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc)
- // t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc)
- auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a));
+ // 0 = Vo + acc(1/lambda)(1-e^(-lambda t_stop))
+ // e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
+ // t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
+ // t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
+ auto t_stop = (-1.0 / lambda) * std::log1p(v * lambda / a);
// Calculate the distance traveled until stopping
auto distance_to_reach_zero_speed =
- v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
+ v * t_stop + a * t_stop * (1.0 / lambda) +
+ a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
// Output an equivalent constant speed
return distance_to_reach_zero_speed / t_h;
}
@@ -461,17 +462,18 @@ FrenetPoint PathGenerator::getFrenetPoint(
// assume it will continue accelerating (reckless driving)
const bool object_has_surpassed_limit_already = v > speed_limit;
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
- return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1);
+ return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
// It is assumed the vehicle accelerates until final_speed is reached and
// then continues at constant speed for the rest of the time horizon
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
// would go if it accelerated until reaching the speed limit, and then continued at a constant
// speed.
- const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a);
+ const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
const double distance_covered =
// Distance covered while accelerating
- a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f +
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) +
+ v * t_f +
// Distance covered at constant speed for the rest of the horizon time
speed_limit * (t_h - t_f);
return distance_covered / t_h;