diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 2747767ba52..430c7d54d66 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -379,30 +379,31 @@ auto DetectionSensor::update( | p_00 p_01 | == | p0 + phi * p1 p1 (1 - phi) | | p_10 p_11 | == | p0 (1 - phi) p1 - phi * p0 | - - In the code, we use `phi` for the above autocorrelation coefficients - `phi`, which is calculated from the time_interval `dt` by the - following formula: - - phi(dt) = amplitude * exp(-decay * dt) + offset */ auto markov_process_noise = - [this](bool is_previous_noise_1, auto p1, auto autocorrelation_coefficient) { - const auto rate = (is_previous_noise_1 ? 1.0 : 0.0) * autocorrelation_coefficient + + [this](bool previous_noise, auto p1, auto autocorrelation_coefficient) { + const auto rate = (previous_noise ? 1.0 : 0.0) * autocorrelation_coefficient + (1 - autocorrelation_coefficient) * p1; return std::uniform_real_distribution()(random_engine_) < rate; }; + /* + We use `phi` for the above autocorrelation coefficients `phi`, which + is calculated from the time_interval `dt` by the following formula: + + phi(dt) = amplitude * exp(-decay * dt) + offset + */ auto autocorrelation_coefficient = [&](const std::string & name) { static const auto amplitude = parameter(name + ".autocorrelation_coefficient.amplitude"); static const auto decay = parameter(name + ".autocorrelation_coefficient.decay"); static const auto offset = parameter(name + ".autocorrelation_coefficient.offset"); - return std::clamp(amplitude * std::exp(-interval * decay) + offset, 0.0, 1.0); + return std::clamp(amplitude * std::exp(-decay * interval) + offset, 0.0, 1.0); }; - auto selector = [&](auto ellipse_normalized_x_radius, const auto & targets) { + auto selector = [&](const std::string & name) { static const auto ellipse_y_radii = parameters("ellipse_y_radii"); - return [&, ellipse_normalized_x_radius, targets]() { + return [&, ellipse_normalized_x_radius = parameter(name + ".ellipse_normalized_x_radius"), + values = parameters(name + ".values")]() { /* If the parameter `.noise.v2.ellipse_y_radii` contains the value 0.0, division by zero will occur here. @@ -414,7 +415,7 @@ auto DetectionSensor::update( const auto distance = std::hypot(x / ellipse_normalized_x_radius, y); for (auto i = std::size_t(0); i < ellipse_y_radii.size(); ++i) { if (distance < ellipse_y_radii[i]) { - return targets[i]; + return values[i]; } } return 0.0; @@ -422,22 +423,16 @@ auto DetectionSensor::update( }; noise_output->second.distance_noise = [&]() { - static const auto mean = selector( - parameter("distance.ellipse_normalized_x_radius.mean"), parameters("distance.means")); - static const auto standard_deviation = selector( - parameter("distance.ellipse_normalized_x_radius.standard_deviation"), - parameters("distance.standard_deviations")); + static const auto mean = selector("distance.mean"); + static const auto standard_deviation = selector("distance.standard_deviation"); return autoregressive_noise( noise_output->second.distance_noise, mean(), standard_deviation(), autocorrelation_coefficient("distance")); }(); noise_output->second.yaw_noise = [&]() { - static const auto mean = - selector(parameter("yaw.ellipse_normalized_x_radius.mean"), parameters("yaw.means")); - static const auto standard_deviation = selector( - parameter("yaw.ellipse_normalized_x_radius.standard_deviation"), - parameters("yaw.standard_deviations")); + static const auto mean = selector("yaw.mean"); + static const auto standard_deviation = selector("yaw.standard_deviation"); return autoregressive_noise( noise_output->second.yaw_noise, mean(), standard_deviation(), autocorrelation_coefficient("yaw")); @@ -452,9 +447,7 @@ auto DetectionSensor::update( }(); noise_output->second.true_positive = [&]() { - static const auto rate = selector( - parameter("true_positive.ellipse_normalized_x_radius"), - parameters("true_positive.rates")); + static const auto rate = selector("true_positive.rate"); return markov_process_noise( noise_output->second.true_positive, rate(), autocorrelation_coefficient("true_positive")); diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml index 9750bb82630..77768f2c52d 100644 --- a/test_runner/scenario_test_runner/config/parameters.yaml +++ b/test_runner/scenario_test_runner/config/parameters.yaml @@ -140,21 +140,23 @@ simulation: amplitude: 0.32 decay: 0.45 offset: 0.26 - ellipse_normalized_x_radius: - mean: 1.1 - standard_deviation: 1.0 - means: [-0.06, -0.04, -0.04, -0.07, -0.26, -0.56, -1.02, -1.05, 0.0] - standard_deviations: [0.17, 0.20, 0.27, 0.40, 0.67, 0.94, 1.19, 1.17, 0.0] + mean: + ellipse_normalized_x_radius: 1.1 + values: [-0.06, -0.04, -0.04, -0.07, -0.26, -0.56, -1.02, -1.05, 0.0] + standard_deviation: + ellipse_normalized_x_radius: 1.0 + values: [0.17, 0.20, 0.27, 0.40, 0.67, 0.94, 1.19, 1.17, 0.0] yaw: autocorrelation_coefficient: amplitude: 0.22 decay: 0.52 offset: 0.21 - ellipse_normalized_x_radius: - mean: 1.9 - standard_deviation: 0.8 - means: [0.0, 0.01, 0.0, 0.0, 0.0, -0.07, 0.18, 0.06, 0.0] - standard_deviations: [0.09, 0.15, 0.21, 0.18, 0.17, 0.19, 0.39, 0.15, 0.0] + mean: + ellipse_normalized_x_radius: 1.9 + values: [0.0, 0.01, 0.0, 0.0, 0.0, -0.07, 0.18, 0.06, 0.0] + standard_deviation: + ellipse_normalized_x_radius: 0.8 + values: [0.09, 0.15, 0.21, 0.18, 0.17, 0.19, 0.39, 0.15, 0.0] yaw_flip: autocorrelation_coefficient: amplitude: 0.29 @@ -167,5 +169,6 @@ simulation: amplitude: 0.32 decay: 0.26 offset: 0.40 - ellipse_normalized_x_radius: 0.7 - rates: [0.96, 0.78, 0.57, 0.48, 0.27, 0.07, 0.01, 0.0, 0.0] + rate: + ellipse_normalized_x_radius: 0.7 + values: [0.96, 0.78, 0.57, 0.48, 0.27, 0.07, 0.01, 0.0, 0.0]