| 164 | namespace beluga::tutorial { |
| 165 | |
| 166 | int run(const std::filesystem::path& path) { |
| 167 | const auto parameters = YAML::LoadFile(path).as<beluga::tutorial::Parameters>(); |
| 168 | |
| 169 | std::normal_distribution<double> initial_position_distribution( |
| 170 | parameters.initial_position, parameters.initial_position_sigma); |
| 171 | auto particles = beluga::views::sample(initial_position_distribution) | // |
| 172 | ranges::views::transform(beluga::make_from_state<Particle>) | // |
| 173 | ranges::views::take_exactly(parameters.number_of_particles) | // |
| 174 | ranges::to<std::vector>; |
| 175 | |
| 176 | std::vector<RobotRecord> records; |
| 177 | records.reserve(parameters.number_of_cycles); |
| 178 | |
| 179 | double current_position{parameters.initial_position}; |
| 180 | for (std::size_t n = 0; n < parameters.number_of_cycles; ++n) { |
| 181 | RobotRecord record; |
| 182 | |
| 183 | current_position += parameters.velocity * parameters.dt; |
| 184 | record.ground_truth = current_position; |
| 185 | |
| 186 | if (current_position > static_cast<double>(parameters.map_size)) { |
| 187 | break; |
| 188 | } |
| 189 | |
| 190 | const auto motion_model = [&](double position, auto& random_engine) { |
| 191 | std::normal_distribution<double> motion_distribution( |
| 192 | parameters.velocity * parameters.dt, parameters.motion_model_sigma * parameters.dt); |
| 193 | return position + motion_distribution(random_engine); |
| 194 | }; |
| 195 | |
| 196 | const auto range_measurements = |
| 197 | parameters.landmark_map | // |
| 198 | ranges::views::transform([&](double landmark_position) { return landmark_position - current_position; }) | // |
| 199 | ranges::views::remove_if([&](double range) { return std::abs(range) > parameters.sensor_range; }) | // |
| 200 | ranges::to<std::vector>; |
| 201 | |
| 202 | const auto sensor_model = [&](double position) { |
| 203 | auto range_map = |
| 204 | parameters.landmark_map | // |
| 205 | ranges::views::transform([&](double landmark_position) { return landmark_position - position; }) | // |
| 206 | ranges::to<std::vector>; |
| 207 | |
| 208 | return parameters.min_particle_weight + |
| 209 | std::transform_reduce( |
| 210 | range_measurements.begin(), range_measurements.end(), 1.0, std::multiplies<>{}, |
| 211 | [&](double range_measurement) { |
| 212 | const auto distances = range_map | ranges::views::transform([&](double range) { |
| 213 | return std::abs(range - range_measurement); |
| 214 | }); |
| 215 | const auto min_distance = ranges::min(distances); |
| 216 | return std::exp((-1 * std::pow(min_distance, 2)) / (2 * parameters.sensor_model_sigma)); |
| 217 | }); |
| 218 | }; |
| 219 | |
| 220 | record.current = particles; |
| 221 | |
| 222 | particles |= beluga::actions::propagate(std::execution::seq, motion_model); |
| 223 | record.prediction = particles; |