diff --git a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc index 2cc630532d5db9bcdb3ee9ad2cd26d00035942ab..7d93955cf102b078f91f92320c4f88d1233cc4b1 100644 --- a/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc +++ b/modules/prediction/predictor/move_sequence/move_sequence_predictor.cc @@ -555,9 +555,12 @@ double MoveSequencePredictor::CostFunction( FLAGS_cost_function_alpha * time_to_end_state; if (FLAGS_use_bell_curve_for_cost_function) { double bell_curve_weight = - 1 / std::sqrt(2 * M_PI * std::pow(FLAGS_cost_function_sigma, 2.0)) - * std::exp(- std::pow(time_to_lane_edge - bell_curve_mu, 2.0) / - (2 * std::pow(FLAGS_cost_function_sigma, 2.0))); + 1 / std::sqrt(2 * M_PI * FLAGS_cost_function_sigma * + FLAGS_cost_function_sigma) + * std::exp( + -(time_to_lane_edge - bell_curve_mu) * + (time_to_lane_edge - bell_curve_mu) / + (2 * FLAGS_cost_function_sigma * FLAGS_cost_function_sigma)); cost_of_trajectory /= (bell_curve_weight + FLAGS_double_precision); } return cost_of_trajectory;