diff --git a/README.md b/README.md index ade4f70..f04b019 100644 --- a/README.md +++ b/README.md @@ -108,7 +108,6 @@ auto barrel_direction = *fast_tf::cast( | `/auto_aim/pitch_rate` | `double` | pitch 角速度 | | `/auto_aim/yaw_acc` | `double` | yaw 角加速度 | | `/auto_aim/pitch_acc` | `double` | pitch 角加速度 | -| `/auto_aim/feedforward_valid` | `bool` | 前馈数据是否有效 | ## 项目架构 diff --git a/config/config.yaml b/config/config.yaml index 3f8a45e..daa638c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -93,13 +93,13 @@ fire_control: initial_bullet_speed: 23.5 # m/s shoot_delay: 0.04 # s mpc_enable: true - yaw_offset: -0.4 # degree - pitch_offset: +0.8 # degree + yaw_offset: -0.0 # degree + pitch_offset: +0.0 # degree armor_target_selector: coming_angle: 60.0 # degree leaving_angle: 20.0 # degree - outpost_coming_angle: 60.0 # degree + outpost_coming_angle: 40.0 # degree outpost_leaving_angle: 30.0 # degree switch_threshold: 8.0 # degree diff --git a/src/kernel/auto_aim.cpp b/src/kernel/auto_aim.cpp index c744cbe..a071695 100644 --- a/src/kernel/auto_aim.cpp +++ b/src/kernel/auto_aim.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include using namespace rmcs; diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 3eebee4..038a520 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -71,7 +71,7 @@ struct FireControl::Impl { }; } - return { }; + return {}; } auto solve(predictor::Snapshot const& snapshot, GimbalState const& gimbal_state) @@ -82,9 +82,9 @@ struct FireControl::Impl { auto aim_point_position = Eigen::Vector3d {}; auto center_position = Eigen::Vector3d {}; { - auto const target_motion = snapshot.motion(); - auto const coarse_fly_time = - target_motion.center_position.norm() / config.initial_bullet_speed; + auto const target_motion = snapshot.motion(); + auto target_center = target_motion.center_position.make(); + auto const coarse_fly_time = target_center.norm() / config.initial_bullet_speed; auto const selection_predict_time = config.shoot_delay + coarse_fly_time; auto const selection_time = snapshot.time_stamp() + std::chrono::duration_cast( @@ -100,7 +100,7 @@ struct FireControl::Impl { auto const& selected = target_candidates[*selected_index]; selected_armor_id = selected.armor.id; - center_position = target_motion.center_position; + center_position = target_center; selected.armor.translation.copy_to(aim_point_position); } @@ -116,7 +116,7 @@ struct FireControl::Impl { } last_selected_armor_id = solution->candidate.armor.id; - target_solution = std::move(*solution); + target_solution = *solution; } /// 3. 轨迹规划 @@ -167,11 +167,11 @@ struct FireControl::Impl { }; } - TrajectoryPlanner trajectory_planner { }; - TargetSolver target_solver { }; - ShootEvaluator shoot_evaluator { }; - ArmorSelector armor_selector { }; - std::optional last_selected_armor_id { }; + TrajectoryPlanner trajectory_planner {}; + TargetSolver target_solver {}; + ShootEvaluator shoot_evaluator {}; + ArmorSelector armor_selector {}; + std::optional last_selected_armor_id {}; }; FireControl::FireControl() noexcept diff --git a/src/module/fire_control/armor_selector.cpp b/src/module/fire_control/armor_selector.cpp index 71baa20..43f0785 100644 --- a/src/module/fire_control/armor_selector.cpp +++ b/src/module/fire_control/armor_selector.cpp @@ -85,8 +85,8 @@ struct ArmorSelector::Impl { auto orientation = Eigen::Quaterniond {}; candidate.armor.orientation.copy_to(orientation); const auto armor_yaw = util::eulers(orientation)[0]; - const auto center_yaw = std::atan2( - candidate.motion.center_position.y(), candidate.motion.center_position.x()); + const auto& center = candidate.motion.center_position; + const auto center_yaw = std::atan2(center.y, center.x); return util::normalize_angle(armor_yaw - center_yaw); }; diff --git a/src/module/fire_control/target_solver.cpp b/src/module/fire_control/target_solver.cpp index 40b783b..77449b8 100644 --- a/src/module/fire_control/target_solver.cpp +++ b/src/module/fire_control/target_solver.cpp @@ -40,7 +40,7 @@ struct TargetSolver::Impl { static auto solve(predictor::Snapshot const& snapshot, int armor_id, TimePoint command_time, double bullet_speed, double shoot_delay) -> std::expected { auto target_motion = snapshot.motion_at(command_time); - auto target_position = target_motion.center_position; + auto target_position = target_motion.center_position.make(); auto current_fly_time = target_position.norm() / bullet_speed; auto result = std::optional {}; diff --git a/src/module/predictor/model/outpost.cpp b/src/module/predictor/model/outpost.cpp index 17fcaea..66ba7c6 100644 --- a/src/module/predictor/model/outpost.cpp +++ b/src/module/predictor/model/outpost.cpp @@ -1,11 +1,13 @@ #include "outpost.hpp" #include "utility/math/angle.hpp" +#include "utility/math/outpost.hpp" #include "utility/robot/constant.hpp" #include using namespace rmcs; +using namespace rmcs::util; struct OutpostModel::Impl { using StateVector = Eigen::Matrix; @@ -16,74 +18,169 @@ struct OutpostModel::Impl { using ObservationJacobian = Eigen::Matrix; using KalmanGain = Eigen::Matrix; - static auto observation_jacobian() { - auto jacobian = ObservationJacobian { }; - // clang-format off - jacobian << - 1, 0, 0, 0, 0, - 0, 1, 0, 0, 0, - 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1; - // clang-format on - return jacobian; - } - struct Context { StateVector posteriors_state = StateVector::Zero(); Covariance posteriors_covariance = Covariance::Identity(); ProcessNoise noise_process = ProcessNoise::Zero(); ObservationNoise noise_observation = ObservationNoise::Zero(); - Context() noexcept { - noise_process.diagonal() << 1e-4, 1e-4, 1e-4, 1e-2, 1e-2; - noise_observation.diagonal() << 0.09, 0.09, 0.09, 9e-2; + Context() noexcept = default; + + auto set_noise(const Config& config) noexcept { + noise_process.diagonal() << config.process_noise_xy, config.process_noise_xy, + config.process_noise_z, config.process_noise_speed, config.process_noise_angle; + noise_observation.diagonal() << config.observation_noise_xy, + config.observation_noise_xy, config.observation_noise_z, + config.observation_noise_yaw; } - static_assert(sizeof(State) == sizeof(posteriors_state)); - auto state() noexcept -> State& { - auto* data_pointer = &posteriors_state; - return *reinterpret_cast(data_pointer); + auto reset_covariance() noexcept { + auto diag = Eigen::Matrix { }; + diag << 1.0, 1.0, 1.0, 64.0, 0.4; + posteriors_covariance = diag.asDiagonal(); } - auto state() const noexcept -> const State& { - const auto* data_pointer = &posteriors_state; - return *reinterpret_cast(data_pointer); + + auto get_state() const noexcept { + return State { + .x = posteriors_state[0], + .y = posteriors_state[1], + .z = posteriors_state[2], + + .rotation_speed = posteriors_state[3], + .rotation_angle = posteriors_state[4], + }; } auto update_center(const Eigen::Vector3d& point) noexcept { - state().x = point.x(); - state().y = point.y(); - state().z = point.z(); + posteriors_state[0] = point.x(); + posteriors_state[1] = point.y(); + posteriors_state[2] = point.z(); } - } context; + }; - auto initialize_from(const Armor3d& armor) noexcept -> void { - constexpr auto pitch = kPredictedOutpostArmorPitch; - constexpr auto radius = kOutpostRadius; + struct HeightBuffer { + double factor = 0.1; - const auto translation = armor.translation.make(); - const auto orientation = armor.orientation.make(); + std::array data; + int last_sign = 0; - const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; - const auto armor_to_center = Eigen::Vector3d { - Eigen::AngleAxisd { -pitch, orientation * Eigen::Vector3d::UnitY() } * backward - }; - const auto center = Eigen::Vector3d { translation + radius * armor_to_center }; + std::size_t index = 0; - context.update_center(center); - context.state().rotation_speed = 0.0; - context.state().rotation_angle = - util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); + HeightBuffer() noexcept { std::ranges::fill(data, kNaN); } - auto initial_covariance_diagonal = Eigen::Matrix { }; - initial_covariance_diagonal << 1.0, 1.0, 1.0, 64.0, 0.4; + auto go_next(int sign) noexcept { + last_sign = sign; + index = (index + sign + data.size()) % data.size(); + } - context.posteriors_covariance = initial_covariance_diagonal.asDiagonal(); + auto update(double height) noexcept { + data[index] = std::isnan(data[index]) // + ? height + : data[index] * (1. - factor) + height * factor; + } + auto clean_oldest() noexcept { + data[(index - 2 * last_sign + data.size()) % data.size()] = kNaN; + } + + auto all_observed() const noexcept -> bool { + return !std::isnan(data[0]) && !std::isnan(data[1]) && !std::isnan(data[2]); + } + + auto level_at(std::size_t pos) const noexcept { + const auto height = data[pos]; + + const auto l_height = data[(pos - 1 + data.size()) % data.size()]; + const auto r_height = data[(pos + 1 + data.size()) % data.size()]; + + /// @NOTE: is_lower / is_upper 在遇到 NaN 时返回 true 的设计 + /// + /// data 中尚未观测的装甲板对应的高度值为 NaN。当参与比较的任一 + /// 值为 NaN 时，比较结果被认定为真——该条件自动满足，不参与判定。 + /// 因此当前板的 level 仅由已知邻居的真实比较确定。 + /// + /// - 三块装甲板都被观测到后：无 NaN，所有比较均为真实比较， + /// level() 自然给出当前板正确的序号。 + /// + /// - 只观测到两块装甲板时：当前板与已知邻居的真实比较仍成立， + /// level() 能保证已观测两块板间的相对高度正确，足以支持 EKF + /// 的观测更新。但整组序号可能存在歧义：例如仅观测到两块板且 + /// 高度差为 1 step 时，无法区分是 UP→MID 还是 MID→LOW（两者 + /// 相对高度差一致），当前板的绝对序号可能错误，但不影响 EKF + /// 迭代；待第三块板被观测到后，整组序号即被完全确定。 + constexpr auto is_lower = [](double z, double other) { + return (std::isnan(z) || std::isnan(other)) ? true : z < other; + }; + constexpr auto is_upper = [](double z, double other) { + return (std::isnan(z) || std::isnan(other)) ? true : z > other; + }; + + // 带 '*' 的即为观测到的装甲板 + /*--*/ if (is_upper(height, l_height) && is_upper(height, r_height)) { + /// [*] + /// [ ] + /// [ ] + return 0; + } else if (is_lower(height, l_height) && is_upper(height, r_height)) { + /// [ ] + /// [*] + /// [ ] + return 1; // 中 + } else if (is_lower(height, l_height) && is_lower(height, r_height)) { + /// [ ] + /// [ ] + /// [*] + return 2; // 低 + } + return 0; + } + + auto level() const noexcept { return level_at(index); } + }; + HeightBuffer height_buff { }; + + int abs_ref_index = 0; // 参考板绝对序号 0/1/2（第一块观测板，三块板齐后纠正） + int rel_see_index = 0; // 当前板相对 ref_index 的位置 -1/0/+1 + + ArmorGenre armor_genre { DeviceId::OUTPOST }; + ArmorColor armor_color { ArmorColor::DARK }; + + // 切板检测：存储上一帧观测向量（xyz + yaw） + // 构造时初始化，合法帧每帧更新；噪声帧跳过不更新 + ObservationVector last_observation { }; + + static constexpr int kSignConfirmCount = 2; + + // 旋转方向确认：连续 2 次同方向切板后锁定 + int rotation_sign = 0; // +1/-1，0=未确定 + int pending_sign = 0; // 候选方向 + int sign_evidence_count = 0; // 候选方向连续命中计数 + + Context context; + Config config { }; + + auto configure(const Config& cfg) noexcept { + config = cfg; + context.set_noise(config); + context.reset_covariance(); + } + +private: + static auto make_observation_jacobian() { + auto jacobian = ObservationJacobian { }; + // clang-format off + jacobian << + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1; + // clang-format on + return jacobian; } - static auto predict_state(double dt, const State& last) -> State { - auto next = State { last }; + static auto predict_state(double dt, const StateVector& last) -> StateVector { + auto next = last; - next.rotation_angle = util::normalize_angle(last.rotation_angle + last.rotation_speed * dt); + next[4] = util::normalize_angle(last[4] + last[3] * dt); return next; } @@ -93,11 +190,11 @@ struct OutpostModel::Impl { return jacobian * covariance * jacobian.transpose() + context.noise_process; } - static auto measure_innovation(const ObservationVector& observation, const State& prior_state) - -> ObservationVector { + static auto measure_innovation( + const ObservationVector& observation, const StateVector& prior_state) -> ObservationVector { + auto predicted_observation = ObservationVector { }; - predicted_observation << prior_state.x, prior_state.y, prior_state.z, - prior_state.rotation_angle; + predicted_observation << prior_state[0], prior_state[1], prior_state[2], prior_state[4]; auto innovation = ObservationVector { observation - predicted_observation }; innovation.coeffRef(3) = util::normalize_angle(innovation.coeff(3)); @@ -105,7 +202,7 @@ struct OutpostModel::Impl { } auto calculate_kalman_gain(const Covariance& prior_covariance) const -> KalmanGain { - const auto jacobian = observation_jacobian(); + const auto jacobian = make_observation_jacobian(); const auto innovation_covariance = jacobian * prior_covariance * jacobian.transpose() + context.noise_observation; @@ -114,13 +211,13 @@ struct OutpostModel::Impl { * innovation_covariance.ldlt().solve(ObservationNoise::Identity()); } - auto a_posteriori_update(const State& prior_state, const Covariance& prior_covariance, + auto a_posteriori_update(const StateVector& prior_state, const Covariance& prior_covariance, const KalmanGain& kalman_gain, const ObservationVector& innovation, - const ObservationJacobian& jacobian) const -> std::pair { + const ObservationJacobian& jacobian) const -> std::pair { - auto posterior_state = State { prior_state }; - reinterpret_cast(posterior_state).noalias() += kalman_gain * innovation; - posterior_state.rotation_angle = util::normalize_angle(posterior_state.rotation_angle); + auto posterior_state = StateVector { prior_state }; + posterior_state.noalias() += kalman_gain * innovation; + posterior_state[4] = util::normalize_angle(posterior_state[4]); const auto complement = Covariance::Identity() - kalman_gain * jacobian; auto posterior_covariance = Covariance { }; @@ -132,60 +229,225 @@ struct OutpostModel::Impl { return { posterior_state, posterior_covariance }; } +public: + explicit Impl(const Armor3d& armor) noexcept { + constexpr auto pitch = kPredictedOutpostArmorPitch; + constexpr auto radius = kOutpostRadius; + + // 装甲板元信息 + armor_genre = armor.genre; + armor_color = armor.color; + + const auto translation = armor.translation.make(); + const auto orientation = armor.orientation.make(); + + const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; + const auto armor_to_center = Eigen::Vector3d { + Eigen::AngleAxisd { -pitch, orientation * Eigen::Vector3d::UnitY() } * backward + }; + const auto center = Eigen::Vector3d { translation + radius * armor_to_center }; + + context.update_center(center); + context.posteriors_state[3] = 0.0; + context.posteriors_state[4] = + util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); + + context.set_noise(config); + context.reset_covariance(); + + // 初始化 last_observation：从 EKF 后验 state 取（与 correct() 末尾更新方式一致） + // state[4] = atan2(-atc.y, -atc.x) = center→armor 方向角度 + last_observation << context.posteriors_state[0], context.posteriors_state[1], + context.posteriors_state[2], context.posteriors_state[4]; + + // 初始化 HeightBuff：第一块板存入 data[0]（index 初始为 0） + height_buff.update(center.z()); + } + auto predict(double dt) noexcept -> void { - const auto prior_state = predict_state(dt, context.state()); + const auto prior_state = predict_state(dt, context.posteriors_state); const auto prior_covariance = predict_covariance(dt, context.posteriors_covariance); - context.state() = prior_state; + context.posteriors_state = prior_state; context.posteriors_covariance = prior_covariance; } auto correct(const Armor3d& armor) noexcept -> void { - { // 切板检测 - } + constexpr auto kPitch = kPredictedOutpostArmorPitch; - constexpr auto pitch = kPredictedOutpostArmorPitch; + // 装甲板元信息 + armor_genre = armor.genre; + armor_color = armor.color; - const auto orientation = armor.orientation.make(); - const auto translation = armor.translation.make(); + // 观测向量构造 + auto observation = ObservationVector { }; + { + const auto orientation = armor.orientation.make(); + const auto translation = armor.translation.make(); - const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; - const auto armor_to_center = Eigen::Vector3d { - Eigen::AngleAxisd { -pitch, orientation * Eigen::Vector3d::UnitY() } * backward - }; + const auto backward = Eigen::Vector3d { orientation * Eigen::Vector3d::UnitX() }; + const auto armor_to_center = Eigen::Vector3d { + Eigen::AngleAxisd { -kPitch, orientation * Eigen::Vector3d::UnitY() } * backward + }; - const auto center = Eigen::Vector3d { translation + kOutpostRadius * armor_to_center }; + const auto center = Eigen::Vector3d { translation + kOutpostRadius * armor_to_center }; - const auto top_armor_yaw = - util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); + const auto armor_yaw = + util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x())); - auto observation = ObservationVector { }; - observation << center.x(), center.y(), center.z(), top_armor_yaw; + observation << center.x(), center.y(), center.z(), armor_yaw; + } - const auto prior_state = context.state(); + // 切板检测：判定是否发生真实切板 + // 判定依据：yaw 跳变大于配置阈值，且 center.xy 偏移落在 10cm 内 + { + constexpr auto kPlateSwitchXYTol = 0.10; // 10cm + + const auto delta_yaw_obs = util::normalize_angle(observation[3] - last_observation[3]); + const auto abs_delta = std::abs(delta_yaw_obs); + + if (abs_delta > util::deg2rad(config.plate_switch_yaw_min)) { + // 候选切板：用 center.xy 偏移做位置验证 + const double xy_error = std::hypot( + observation[0] - last_observation[0], observation[1] - last_observation[1]); + + if (xy_error > kPlateSwitchXYTol) { + // 位置验证不通过：噪声，丢弃 + return; + } + + // 真实切板：从观测符号判断方向，直接取装配标准值 + const auto in_right = delta_yaw_obs > 0.0; + + // 旋转方向确认：连续 2 次同方向切板后锁定 + const auto sign = in_right ? +1 : -1; + if (rotation_sign == 0) { + if (sign == pending_sign) { + ++sign_evidence_count; + } else { + pending_sign = sign; + sign_evidence_count = 1; + } + if (sign_evidence_count >= kSignConfirmCount) { + rotation_sign = pending_sign; + } + } + const auto go_sign = rotation_sign != 0 ? rotation_sign : sign; + + // 更新 HeightBuff + height_buff.go_next(go_sign); + height_buff.update(observation[2]); + + // obs_index：相对 ref_index 的位置 -1/0/+1 + rel_see_index += sign; + if (rel_see_index > +1) rel_see_index -= 3; + if (rel_see_index < -1) rel_see_index += 3; + + // 三块板齐后纠正 abs_ref_index（绝对序号确定） + if (height_buff.all_observed()) { + abs_ref_index = (height_buff.level() - rel_see_index + 3) % 3; + + height_buff.clean_oldest(); + } + } + } + + // 补偿观测：将当前板观测变换回参考板坐标系 + // obs_index = 0 无需补偿；否则用 outpost_relative_height 从高度符号判断 + auto delta_yaw = 0.0; + auto delta_height = 0.0; + if (rel_see_index != 0) { + const auto in_right = rel_see_index > 0; + const auto in_upper = observation[2] > context.posteriors_state[2]; + delta_yaw = in_right ? +std::numbers::pi * 2.0 / 3.0 : -std::numbers::pi * 2.0 / 3.0; + delta_height = outpost_relative_height(in_right, in_upper); + } + observation[2] -= delta_height; + observation[3] = util::normalize_angle(observation[3] - delta_yaw); + + const auto prior_state = context.posteriors_state; const auto prior_covariance = context.posteriors_covariance; const auto innovation = measure_innovation(observation, prior_state); const auto kalman_gain = calculate_kalman_gain(prior_covariance); - const auto jacobian = observation_jacobian(); + const auto jacobian = make_observation_jacobian(); const auto [posterior_state, posterior_covariance] = a_posteriori_update(prior_state, prior_covariance, kalman_gain, innovation, jacobian); - context.state() = posterior_state; + context.posteriors_state = posterior_state; context.posteriors_covariance = posterior_covariance; + + // 用 EKF 后验 state 作为 last_observation（低噪声平滑值） + // state 跟踪参考板，需反补偿回当前板坐标系，使下一帧切板检测正确 + last_observation << context.posteriors_state[0], context.posteriors_state[1], + context.posteriors_state[2] + delta_height, + util::normalize_angle(context.posteriors_state[4] + delta_yaw); + } + + auto at(std::uint8_t index) const noexcept -> Armor3d { + constexpr auto kOffsetTable = std::array { + std::tuple { 0 * std::numbers::pi * 2 / 3, -0 * kOutpostArmorHeightStep }, // 高 + std::tuple { 1 * std::numbers::pi * 2 / 3, -1 * kOutpostArmorHeightStep }, // 中 + std::tuple { 2 * std::numbers::pi * 2 / 3, -2 * kOutpostArmorHeightStep }, // 低 + }; + constexpr auto kRadius = kOutpostRadius; + constexpr auto kPitch = kPredictedOutpostArmorPitch; + + // EKF state 跟踪参考板（ref_index），求 index 相对 ref_index 的偏移 + const auto delta_yaw = util::normalize_angle( + std::get<0>(kOffsetTable[index]) - std::get<0>(kOffsetTable[abs_ref_index])); + const auto delta_height = + std::get<1>(kOffsetTable[index]) - std::get<1>(kOffsetTable[abs_ref_index]); + + // center→armor 方向的 yaw（state[4] 的约定） + const auto center_to_armor_yaw = + util::normalize_angle(context.posteriors_state[4] + delta_yaw); + + // 装甲板位置 = 旋转中心 + radius * center→armor 方向 + const auto armor_pos = Eigen::Vector3d { + context.posteriors_state[0] + kRadius * std::cos(center_to_armor_yaw), + context.posteriors_state[1] + kRadius * std::sin(center_to_armor_yaw), + context.posteriors_state[2] + delta_height, + }; + + // 装甲板朝向约定：orientation 的 X 轴 = armor→center + const auto orientation = Eigen::Quaterniond { + Eigen::AngleAxisd { center_to_armor_yaw + std::numbers::pi, Eigen::Vector3d::UnitZ() } + * Eigen::AngleAxisd { kPitch, Eigen::Vector3d::UnitY() } + }; + + return Armor3d { + .genre = armor_genre, + .color = armor_color, + .id = index, + + .translation = Translation { armor_pos }, + .orientation = Orientation { orientation }, + }; } }; OutpostModel::OutpostModel(const Armor3d& armor) noexcept - : pimpl { std::make_unique() } { - pimpl->initialize_from(armor); -} + : pimpl { std::make_unique(armor) } { } OutpostModel::~OutpostModel() noexcept = default; +auto OutpostModel::state() noexcept -> State { return pimpl->context.get_state(); } + +auto OutpostModel::configure(const Config& cfg) noexcept -> void { pimpl->configure(cfg); } + auto OutpostModel::predict(double dt) noexcept -> void { pimpl->predict(dt); } auto OutpostModel::correct(const Armor3d& armor) noexcept -> void { pimpl->correct(armor); } -auto OutpostModel::state() noexcept -> State { return pimpl->context.state(); } +auto OutpostModel::full() const -> std::array { + return std::array { + pimpl->at(0), + pimpl->at(1), + pimpl->at(2), + }; +} +auto OutpostModel::current() const -> Armor3d { + return pimpl->at((pimpl->abs_ref_index + pimpl->rel_see_index + 3) % 3); +} diff --git a/src/module/predictor/model/outpost.hpp b/src/module/predictor/model/outpost.hpp index ea0f03e..7ef21c7 100644 --- a/src/module/predictor/model/outpost.hpp +++ b/src/module/predictor/model/outpost.hpp @@ -20,11 +20,61 @@ class OutpostModel { double rotation_angle; }; + struct Config { + /// @brief 过程噪声 (Q) - 决定对物理模型的信任度 + /// @note 前哨站中心在场地上是绝对静止的，这两个值应尽可能小 + + // 中心点 XY 轴漂移噪声 + double process_noise_xy = 1e-6; + + // 中心点 Z 轴（高度）漂移噪声 + double process_noise_z = 1e-5; + + /// @note 增大 speed 会加速转速收敛速度（减小跟踪滞后），但过大会导致速度曲线随视觉高频抖动 + + // 转速变化（角速度）噪声 + double process_noise_speed = 1e-4; + + // 角度积分累积噪声 + double process_noise_angle = 1e-4; + + /// @brief 观测噪声 (R) - 决定对视觉/PnP 数据的信任度（单位：米 / 弧度） + /// @note 增大以下各值，会加大对历史轨迹的滤波平滑效果，降低对单帧视觉跳变的敏感度 + + // 视觉解算出的中心 XY 轴误差 + double observation_noise_xy = 0.005; + + // 视觉解算出的中心 Z 轴误差 + double observation_noise_z = 0.01; + + // 视觉解算出的装甲板朝向(Yaw)误差 + double observation_noise_yaw = 0.01; + + /// @brief 切板检测判定阈值 + /// @note 三块装甲板夹角 120°，考虑到观测的局限，建议设在 50.0 ~ 80.0 度之间 + + // 触发切板判定的最小 Yaw 角跳变（度） + double plate_switch_yaw_min = 50.0; + }; + explicit OutpostModel(const Armor3d& armor) noexcept; + auto configure(const Config&) noexcept -> void; + auto predict(double dt) noexcept -> void; auto correct(const Armor3d& armor) noexcept -> void; + + // 前哨站的状态向量内容喵，参考点为构造时从传入的那块 auto state() noexcept -> State; + + // 所有前哨站的装甲板，按照从高到低的顺序排列 + // 也就是说，0 是最高的那块 + auto full() const -> std::array; + + // 当前正在观测的那块装甲板（即上一个 correct 的装甲板） + // 注意纯预测不会改变 current 返回的装甲板，即使预测后准 + // 备返回的装甲板在前哨站背面 + auto current() const -> Armor3d; }; } diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp deleted file mode 100644 index 367e2fd..0000000 --- a/src/module/predictor/outpost/armor_layout.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include - -namespace rmcs::predictor { - -struct OutpostArmorSlot { - double phase_offset { 0.0 }; - double height_offset { 0.0 }; - bool assigned { false }; -}; - -struct OutpostArmorLayout { - static constexpr int kSlotCount = 3; - - std::array slots {}; -}; - -} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp deleted file mode 100644 index 098a4bc..0000000 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ /dev/null @@ -1,205 +0,0 @@ -#pragma once - -#include "module/predictor/outpost/armor_layout.hpp" -#include "utility/math/angle.hpp" -#include "utility/math/conversion.hpp" -#include "utility/math/kalman_filter/ekf.hpp" -#include "utility/robot/armor.hpp" -#include "utility/robot/constant.hpp" - -#include -#include -#include - -namespace rmcs::predictor { - -struct OutpostEKFParameters { - using EKF = util::EKF<6, 4>; - - struct ArmorObservation { - Eigen::Vector3d xyz; - Eigen::Vector3d ypr; - Eigen::Vector3d ypd; - }; - - static constexpr int kOutpostArmorCount = 3; - static constexpr double kPhaseStep = 2.0 * std::numbers::pi / kOutpostArmorCount; - - // x vx y vy z a - // x, y：前哨站旋转中心在世界坐标系下的位置 - // vx, vy：前哨站旋转中心在世界坐标系下的线速度 - // z：参考装甲板(id 0)在世界坐标系下的 z 坐标 - // a：参考装甲板(id 0)的 yaw 角 - static auto x(ArmorObservation const& obs) -> EKF::XVec { - auto const yaw = obs.ypr[0]; - auto const center_x = obs.xyz[0] + kOutpostRadius * std::cos(yaw); - auto const center_y = obs.xyz[1] + kOutpostRadius * std::sin(yaw); - - auto x = EKF::XVec { }; - x << center_x, 0.0, center_y, 0.0, obs.xyz[2], yaw; - return x; - } - - static auto P_initial_dig() -> EKF::PDig { - auto P_dig = EKF::PDig { }; - P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4; - return P_dig; - } - - static auto armor_yaw(EKF::XVec const& x, double phase_offset) -> double { - return util::normalize_angle(x[5] + phase_offset); - } - - static auto armor_yaw(EKF::XVec const& x, OutpostArmorLayout const& layout, int armor_id) - -> double { - return armor_yaw(x, layout.slots.at(armor_id).phase_offset); - } - - static auto h_armor_xyz(EKF::XVec const& x, double phase_offset, double height_offset) - -> Eigen::Vector3d { - const auto phase = armor_yaw(x, phase_offset); - const auto pos_x = x[0] - kOutpostRadius * std::cos(phase); - const auto pos_y = x[2] - kOutpostRadius * std::sin(phase); - const auto pos_z = x[4] + height_offset; - return { pos_x, pos_y, pos_z }; - } - - static auto h(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::ZVec { - const auto xyz = h_armor_xyz(x, phase_offset, height_offset); - const auto ypd = util::xyz2ypd(xyz); - const auto yaw = armor_yaw(x, phase_offset); - - auto z = EKF::ZVec { }; - z << ypd[0], ypd[1], ypd[2], yaw; - return z; - } - - static auto x_add(EKF::XVec const& a, EKF::XVec const& b) -> EKF::XVec { - auto result = EKF::XVec { a + b }; - result[5] = util::normalize_angle(result[5]); - return result; - } - - static auto z_subtract(EKF::ZVec const& a, EKF::ZVec const& b) -> EKF::ZVec { - auto result = EKF::ZVec { a - b }; - result[0] = util::normalize_angle(result[0]); - result[1] = util::normalize_angle(result[1]); - result[3] = util::normalize_angle(result[3]); - return result; - } - - static auto F(double dt) -> EKF::AMat { - auto F = EKF::AMat { }; - // clang-format off - F << - 1, dt, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; - // clang-format on - return F; - } - - static auto Q(double dt) -> EKF::QMat { - // 平面匀速模型中的未建模线加速度噪声，作用在 x/vx 与 y/vy - constexpr double linear_acc_var = 10.0; - // 高度锚点 z 的过程噪声 - constexpr double z_process_noise_var = 1e-2; - // 参考装甲板 yaw 的过程噪声 - constexpr double yaw_process_noise_var = 1e-2; - const auto v1 = linear_acc_var; - const auto v2 = z_process_noise_var; - const auto v3 = yaw_process_noise_var; - - const auto a = dt * dt * dt * dt / 4.0; - const auto b = dt * dt * dt / 2.0; - const auto c = dt * dt; - - auto Q = EKF::QMat { }; - // clang-format off - Q << a * v1, b * v1, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, - 0, 0, 0, 0, v2 * dt, 0, - 0, 0, 0, 0, 0, v3 * dt; - // clang-format on - return Q; - } - - static auto f(double dt, double angular_velocity) -> auto { - return [dt, angular_velocity](EKF::XVec const& x) { - EKF::XVec x_prior = F(dt) * x; - x_prior[5] = util::normalize_angle(x[5] + angular_velocity * dt); - return x_prior; - }; - } - - static auto observe(Armor3d const& armor) -> ArmorObservation { - auto const [pos_x, pos_y, pos_z] = armor.translation; - auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; - - auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; - auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; - - auto const ypr = util::eulers(orientation); - auto const ypd = util::xyz2ypd(xyz); - - return { xyz, ypr, ypd }; - } - - static auto z(ArmorObservation const& obs) -> EKF::ZVec { - auto z = EKF::ZVec { }; - z << obs.ypd[0], obs.ypd[1], obs.ypd[2], obs.ypr[0]; - return z; - } - - static auto R(ArmorObservation const& obs) -> EKF::RMat { - const auto center_yaw = std::atan2(obs.xyz[1], obs.xyz[0]); - const auto delta_yaw = util::normalize_angle(obs.ypr[0] - center_yaw); - const auto distance = obs.ypd[2]; - - auto R_dig = EKF::RDig { }; - // clang-format off - R_dig << 4e-3, 4e-3, std::log(std::abs(delta_yaw) + 1.0) + 1.0, - std::log(std::abs(distance) + 1.0) / 200.0 + 9e-2; - // clang-format on - - return R_dig.asDiagonal(); - } - - static auto H(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::HMat { - const auto phase = armor_yaw(x, phase_offset); - const auto cos_phase = std::cos(phase); - const auto sin_phase = std::sin(phase); - const auto dx_da = kOutpostRadius * sin_phase; - const auto dy_da = -kOutpostRadius * cos_phase; - - auto H_armor_xyza = Eigen::Matrix { }; - // clang-format off - H_armor_xyza << - 1, 0, 0, 0, 0, dx_da, - 0, 0, 1, 0, 0, dy_da, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; - // clang-format on - - const auto xyz = h_armor_xyz(x, phase_offset, height_offset); - const auto H_armor_ypd = util::xyz2ypd_jacobian(xyz); - - Eigen::Matrix H_armor_ypda; - // clang-format off - H_armor_ypda << - H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0, - H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0, - H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0, - 0, 0, 0, 1; - // clang-format on - - return H_armor_ypda * H_armor_xyza; - } -}; - -} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 9a6da53..1e0e8ab 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -1,394 +1,62 @@ #include "robot_state.hpp" +#include "module/predictor/model/outpost.hpp" #include "module/predictor/outpost/snapshot.hpp" -#include "utility/math/angle.hpp" -#include "utility/time.hpp" -#include -#include -#include #include #include -#include +#include #include namespace rmcs::predictor { struct OutpostRobotState::Impl { - using Params = OutpostEKFParameters; + std::unique_ptr model; - enum class MotionMode { STATIC, CW, CCW }; - - struct Candidate { - int slot_id { 0 }; - double phase_offset { 0.0 }; - double height_offset { 0.0 }; - bool assigned { false }; - std::optional motion_mode; - }; - - struct MatchResult { - Params::ArmorObservation observation; - Candidate candidate; - double reference_yaw { 0.0 }; - double score { std::numeric_limits::infinity() }; - }; - - struct RotationTopology { - double angular_velocity { 0.0 }; - int slot_delta { 0 }; - double phase_delta { 0.0 }; - std::array height_steps {}; - }; - - struct RotationEvidence { - std::optional mode; - int count { 0 }; - }; - - explicit Impl(TimePoint stamp) noexcept - : time_stamp { stamp } { } - - auto initialize(Armor3d const& armor, TimePoint t) -> void { - color = armor_color2camp_color(armor.color); - time_stamp = t; - update_count = 0; - motion_mode = std::nullopt; - rotation_evidence = {}; - - layout = OutpostArmorLayout {}; - assign_slot(0, 0.0, 0.0); - - ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; - initialized = true; - - auto const observation = Params::observe(armor); - mode_reference_yaw = observation.ypr[0]; - mode_reference_stamp = t; - } - - auto predict(TimePoint t) -> void { - if (t <= time_stamp) return; - - if (initialized) { - auto const dt = util::delta_time(t, time_stamp); - auto const dt_s = dt.count(); - ekf.predict( - Params::f(dt_s, angular_velocity()), - [dt_s](EKF::XVec const&) { return Params::F(dt_s); }, Params::Q(dt_s)); - } - - time_stamp = t; + auto predict(double dt) const -> void { + if (!(dt > 0.0) || !model) return; + model->predict(dt); } auto update(std::span armors) -> bool { if (armors.empty()) return false; - if (!initialized) { - initialize(armors.front(), time_stamp); - ++update_count; - return true; + if (!model) { + model = std::make_unique(armors.front()); + } else { + model->correct(armors.front()); } - auto match = select_best_match(armors); - if (!match.has_value()) return false; - - if (!apply_match(*match)) return false; - - ++update_count; return true; } - auto is_converged() const -> bool { - if (!initialized) return false; - if (!std::isfinite(distance())) return false; - if (!motion_mode.has_value()) return false; - - constexpr int min_updates = 3; - return layout.slots[0].assigned && update_count >= min_updates; - } - - auto get_snapshot() const -> std::optional { - if (!initialized) return std::nullopt; - return Snapshot { OutpostSnapshot { - ekf.x, color, time_stamp, layout, angular_velocity() } }; - } - - auto distance() const -> double { - if (!initialized) return std::numeric_limits::infinity(); - return std::hypot(ekf.x[0], ekf.x[2]); - } - -private: - static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; - static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(5.0); - static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(15.0); - static constexpr int kRotationConfirmCandidateCount = 3; - - auto assign_slot(int slot_id, double phase_offset, double height_offset) -> void { - auto& slot = layout.slots.at(slot_id); - slot.phase_offset = phase_offset; - slot.height_offset = height_offset; - slot.assigned = true; - } - - auto is_layout_consistent() const -> bool { - if (!layout.slots[0].assigned || !layout.slots[1].assigned || !layout.slots[2].assigned) - return true; - - auto height_step = [](double height_offset) { - return static_cast(std::round(height_offset / kOutpostArmorHeightStep)); - }; - - auto s0 = height_step(layout.slots[0].height_offset); - auto s1 = height_step(layout.slots[1].height_offset); - auto s2 = height_step(layout.slots[2].height_offset); - - auto min_step = std::min({ s0, s1, s2 }); - auto max_step = std::max({ s0, s1, s2 }); - - return max_step - min_step == 2 && s0 != s1 && s1 != s2 && s0 != s2; - } - - static constexpr auto topology_of(MotionMode mode) -> std::optional { - switch (mode) { - case MotionMode::CW: - return RotationTopology { - .angular_velocity = -kOutpostAngularSpeed, - .slot_delta = 1, - .phase_delta = Params::kPhaseStep, - .height_steps = { -1.0, 2.0 }, - }; - case MotionMode::CCW: - return RotationTopology { - .angular_velocity = kOutpostAngularSpeed, - .slot_delta = -1, - .phase_delta = -Params::kPhaseStep, - .height_steps = { 1.0, -2.0 }, - }; - case MotionMode::STATIC: - return std::nullopt; - } - - return std::nullopt; - } - - auto angular_velocity() const -> double { - if (!motion_mode.has_value()) return 0.0; - auto const topology = topology_of(*motion_mode); - if (!topology.has_value()) return 0.0; - return topology->angular_velocity; - } - - static constexpr auto normalize_slot_id(int slot_id) -> int { - constexpr int slot_count = Params::kOutpostArmorCount; - return (slot_id + slot_count) % slot_count; - } - - auto make_candidates() const -> std::vector { - auto candidates = std::vector {}; - candidates.reserve(Params::kOutpostArmorCount * 3); - - for (int slot_id = 0; slot_id < Params::kOutpostArmorCount; ++slot_id) { - auto const& slot = layout.slots.at(slot_id); - if (!slot.assigned) continue; - - candidates.emplace_back(Candidate { - .slot_id = slot_id, - .phase_offset = slot.phase_offset, - .height_offset = slot.height_offset, - .assigned = true, - .motion_mode = motion_mode, - }); - } - - if (motion_mode.has_value()) { - if (*motion_mode == MotionMode::STATIC) return candidates; - append_next_slot_candidates(candidates, *motion_mode); - return candidates; - } - - append_next_slot_candidates(candidates, MotionMode::CW); - append_next_slot_candidates(candidates, MotionMode::CCW); - return candidates; - } - - auto append_next_slot_candidates(std::vector& candidates, MotionMode mode) const - -> void { - auto const topology = topology_of(mode); - if (!topology.has_value()) return; - - for (int source_slot_id = 0; source_slot_id < Params::kOutpostArmorCount; - ++source_slot_id) { - auto const& source_slot = layout.slots.at(source_slot_id); - if (!source_slot.assigned) continue; - - auto const target_slot_id = normalize_slot_id(source_slot_id + topology->slot_delta); - auto const& target_slot = layout.slots.at(target_slot_id); - if (target_slot.assigned) continue; - - for (auto const height_step : topology->height_steps) { - auto const phase_offset = - util::normalize_angle(source_slot.phase_offset + topology->phase_delta); - auto const height_offset = - source_slot.height_offset + height_step * kOutpostArmorHeightStep; - - candidates.emplace_back(Candidate { - .slot_id = target_slot_id, - .phase_offset = phase_offset, - .height_offset = height_offset, - .assigned = false, - .motion_mode = mode, - }); - } - } - } - - auto select_best_match(std::span armors) const -> std::optional { - auto const candidates = make_candidates(); - if (candidates.empty()) return std::nullopt; - - auto best_match = std::optional {}; - - for (auto const& armor : armors) { - auto const observation = Params::observe(armor); - for (auto const& candidate : candidates) { - // 预测装甲板位置 - auto const predicted_xyz = - Params::h_armor_xyz(ekf.x, candidate.phase_offset, candidate.height_offset); - auto const predicted_yaw = Params::armor_yaw(ekf.x, candidate.phase_offset); - - // 计算匹配分数 - auto const position_error = (observation.xyz - predicted_xyz).norm(); - auto const yaw_error = - std::abs(util::normalize_angle(observation.ypr[0] - predicted_yaw)); - auto const score = position_error + kOutpostRadius * yaw_error; - - if (best_match.has_value() && score >= best_match->score) continue; - - best_match = MatchResult { - .observation = observation, - .candidate = candidate, - .reference_yaw = - util::normalize_angle(observation.ypr[0] - candidate.phase_offset), - .score = score, - }; - } - } - - return best_match; - } - - auto apply_match(MatchResult const& match) -> bool { - auto const& candidate = match.candidate; - - if (!motion_mode.has_value() && !candidate.assigned && candidate.motion_mode.has_value()) { - update_rotation_evidence(*candidate.motion_mode); - return false; - } - - rotation_evidence = {}; - - assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); - - if (!is_layout_consistent()) { - initialized = false; - return false; - } - - update_motion_mode(match); - - ekf.update( - Params::z(match.observation), - [candidate](EKF::XVec const& x) { - return Params::h(x, candidate.phase_offset, candidate.height_offset); - }, - [candidate](EKF::XVec const& x) { - return Params::H(x, candidate.phase_offset, candidate.height_offset); - }, - Params::R(match.observation), Params::x_add, Params::z_subtract); + auto is_converged() const noexcept -> bool { + if (!model) return false; return true; } - auto update_rotation_evidence(MotionMode candidate_mode) -> void { - if (rotation_evidence.mode == candidate_mode) { - ++rotation_evidence.count; - } else { - rotation_evidence = { - .mode = candidate_mode, - .count = 1, - }; - } + auto get_snapshot(TimePoint stamp) const -> std::optional { + if (!model) return std::nullopt; - if (rotation_evidence.count >= kRotationConfirmCandidateCount) { - motion_mode = candidate_mode; - rotation_evidence = {}; - } + const auto full = model->full(); + auto armors = std::vector { full.begin(), full.end() }; + return Snapshot { OutpostSnapshot { model->state(), std::move(armors), stamp } }; } - auto update_motion_mode(MatchResult const& match) -> void { - // 使用拓扑候选直接确认方向 - if (!motion_mode.has_value() && match.candidate.motion_mode.has_value()) { - motion_mode = *match.candidate.motion_mode; - return; - } - - // 按时间窗口判断 yaw 变化 - auto const elapsed = util::delta_time(time_stamp, mode_reference_stamp); - if (elapsed < kModeConfirmWindow) return; - - auto const mode_delta = util::normalize_angle(match.reference_yaw - mode_reference_yaw); - auto const abs_delta = std::abs(mode_delta); - - // 根据 yaw 变化推断当前观测到的运动模式 - auto observed_motion_mode = std::optional {}; - if (abs_delta < kStaticYawDeltaThreshold) observed_motion_mode = MotionMode::STATIC; - else if (abs_delta > kRotationYawDeltaThreshold) { - observed_motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; - } - - // 应用观测结果 - if (observed_motion_mode.has_value()) { - if (!motion_mode.has_value()) { - motion_mode = *observed_motion_mode; - } else if (*motion_mode != *observed_motion_mode) { - motion_mode = std::nullopt; - rotation_evidence = {}; - } - } - - mode_reference_yaw = match.reference_yaw; - mode_reference_stamp = time_stamp; + auto distance() const -> double { + if (!model) return std::numeric_limits::infinity(); + const auto state = model->state(); + return std::hypot(state.x, state.y); } - - CampColor color { CampColor::UNKNOWN }; - OutpostArmorLayout layout {}; - EKF ekf { EKF {} }; - TimePoint time_stamp; - - bool initialized { false }; - int update_count { 0 }; - - std::optional motion_mode; - RotationEvidence rotation_evidence; - double mode_reference_yaw { 0.0 }; - TimePoint mode_reference_stamp; }; OutpostRobotState::OutpostRobotState() noexcept - : OutpostRobotState(Clock::now()) { } - -OutpostRobotState::OutpostRobotState(TimePoint stamp) noexcept - : pimpl { std::make_unique(stamp) } { } + : pimpl { std::make_unique() } { } OutpostRobotState::~OutpostRobotState() noexcept = default; -auto OutpostRobotState::initialize(Armor3d const& armor, TimePoint t) -> void { - return pimpl->initialize(armor, t); -} - -auto OutpostRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } +auto OutpostRobotState::predict(double dt) -> void { return pimpl->predict(dt); } auto OutpostRobotState::update(std::span armors) -> bool { return pimpl->update(armors); @@ -396,8 +64,8 @@ auto OutpostRobotState::update(std::span armors) -> bool { auto OutpostRobotState::is_converged() const -> bool { return pimpl->is_converged(); } -auto OutpostRobotState::get_snapshot() const -> std::optional { - return pimpl->get_snapshot(); +auto OutpostRobotState::get_snapshot(TimePoint stamp) const -> std::optional { + return pimpl->get_snapshot(stamp); } auto OutpostRobotState::distance() const -> double { return pimpl->distance(); } diff --git a/src/module/predictor/outpost/robot_state.hpp b/src/module/predictor/outpost/robot_state.hpp index b8f3d92..3514146 100644 --- a/src/module/predictor/outpost/robot_state.hpp +++ b/src/module/predictor/outpost/robot_state.hpp @@ -1,8 +1,6 @@ #pragma once -#include "module/predictor/outpost/ekf_parameter.hpp" #include "module/predictor/snapshot.hpp" -#include "utility/clock.hpp" #include "utility/pimpl.hpp" #include @@ -14,17 +12,12 @@ class OutpostRobotState { RMCS_PIMPL_DEFINITION(OutpostRobotState) public: - using EKF = OutpostEKFParameters::EKF; - - explicit OutpostRobotState(TimePoint stamp) noexcept; - - auto initialize(Armor3d const& armor, TimePoint t) -> void; - auto predict(TimePoint t) -> void; + auto predict(double dt) -> void; auto update(std::span armors) -> bool; auto is_converged() const -> bool; - auto get_snapshot() const -> std::optional; + auto get_snapshot(TimePoint stamp) const -> std::optional; auto distance() const -> double; }; diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index a5e5555..21490df 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -1,75 +1,69 @@ #include "module/predictor/outpost/snapshot.hpp" -#include "utility/math/conversion.hpp" +#include "utility/math/angle.hpp" #include "utility/time.hpp" +#include #include #include +#include + namespace rmcs::predictor { struct OutpostSnapshot::Impl { - explicit Impl(EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout, - double angular_velocity) - : x { std::move(x) } - , color { color } - , stamp { stamp } - , layout { layout } - , angular_velocity { angular_velocity } { } - - static auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { - auto armor = Armor3d {}; - armor.genre = device; - armor.color = camp_color2armor_color(color); - armor.id = id; - return armor; - } + explicit Impl(OutpostModel::State state, std::vector armors, TimePoint stamp) + : state { state } + , armors { std::move(armors) } + , stamp { stamp } { } - static auto motion_of(EKF::XVec const& x, double angular_velocity) -> TargetMotion { + static auto motion_of(OutpostModel::State const& state) -> TargetMotion { return { - Eigen::Vector3d { x[0], x[2], x[4] }, - angular_velocity, + Point3d { state.x, state.y, state.z }, + state.rotation_speed, }; } - auto predict_state_at(TimePoint t) const -> EKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt, angular_velocity)(x); + auto predict_state_at(TimePoint t) const -> OutpostModel::State { + auto predicted = state; + auto const dt = util::delta_time(t, stamp).count(); + predicted.rotation_angle = + util::normalize_angle(state.rotation_angle + state.rotation_speed * dt); + return predicted; } auto predicted_armors(TimePoint t) const -> std::vector { - auto const predicted_x = predict_state_at(t); + auto const dt = util::delta_time(t, stamp).count(); + auto const angle_delta = state.rotation_speed * dt; + auto const center = Eigen::Vector3d { state.x, state.y, state.z }; + auto const rotation = Eigen::AngleAxisd { angle_delta, Eigen::Vector3d::UnitZ() }; auto armors = std::vector {}; - armors.reserve(OutpostEKFParameters::kOutpostArmorCount); + armors.reserve(this->armors.size()); - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - auto const& slot = layout.slots[id]; - if (!slot.assigned) continue; + for (auto armor : this->armors) { + auto position = armor.translation.make(); + position = center + rotation * (position - center); + armor.translation = Translation { position }; - auto armor = make_armor(DeviceId::OUTPOST, color, id); - auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, layout, id); - auto const position = OutpostEKFParameters::h_armor_xyz( - predicted_x, slot.phase_offset, slot.height_offset); + auto orientation = armor.orientation.make(); + orientation = Eigen::AngleAxisd { angle_delta, Eigen::Vector3d::UnitZ() } * orientation; + armor.orientation = Orientation { orientation.normalized() }; - armor.translation = position; - armor.orientation = util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); armors.emplace_back(armor); } return armors; } - EKF::XVec x; - CampColor color; + OutpostModel::State state; + std::vector armors; TimePoint stamp; - OutpostArmorLayout layout; - double angular_velocity { 0.0 }; }; -OutpostSnapshot::OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, - OutpostArmorLayout layout, double angular_velocity) - : pimpl { std::make_unique(std::move(x), color, stamp, layout, angular_velocity) } { } +OutpostSnapshot::OutpostSnapshot( + OutpostModel::State state, std::vector armors, TimePoint stamp) + : pimpl { std::make_unique(state, std::move(armors), stamp) } { } OutpostSnapshot::OutpostSnapshot(OutpostSnapshot&&) noexcept = default; auto OutpostSnapshot::operator=(OutpostSnapshot&&) noexcept -> OutpostSnapshot& = default; @@ -81,7 +75,7 @@ auto OutpostSnapshot::time_stamp() const -> TimePoint { return pimpl->stamp; } auto OutpostSnapshot::device_id() const -> DeviceId { return DeviceId::OUTPOST; } auto OutpostSnapshot::motion_at(TimePoint t) const -> TargetMotion { - return Impl::motion_of(pimpl->predict_state_at(t), pimpl->angular_velocity); + return Impl::motion_of(pimpl->predict_state_at(t)); } auto OutpostSnapshot::predicted_armors(TimePoint t) const -> std::vector { diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index 22f60e7..8876510 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -1,7 +1,6 @@ #pragma once -#include "module/predictor/outpost/armor_layout.hpp" -#include "module/predictor/outpost/ekf_parameter.hpp" +#include "module/predictor/model/outpost.hpp" #include "module/predictor/snapshot.hpp" #include @@ -11,10 +10,8 @@ namespace rmcs::predictor { class OutpostSnapshot { public: - using EKF = OutpostEKFParameters::EKF; - - explicit OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, - OutpostArmorLayout layout, double angular_velocity); + explicit OutpostSnapshot( + OutpostModel::State state, std::vector armors, TimePoint stamp); OutpostSnapshot(OutpostSnapshot const&) = delete; OutpostSnapshot(OutpostSnapshot&&) noexcept; OutpostSnapshot& operator=(OutpostSnapshot const&) = delete; diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index cc179bf..435bfb0 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -1,7 +1,6 @@ #include "robot_state.hpp" #include "module/predictor/regular/snapshot.hpp" -#include "utility/time.hpp" #include #include @@ -28,7 +27,6 @@ struct RegularRobotState::Impl { int armor_num { 0 }; EKF ekf { EKF {} }; - TimePoint time_stamp; bool initialized { false }; int update_count { 0 }; @@ -40,48 +38,36 @@ struct RegularRobotState::Impl { const double angle_error_threshold { 0.65 }; const double visible_angle_threshold { std::numbers::pi / 2.0 }; - explicit Impl(TimePoint stamp) noexcept - : time_stamp { stamp } { } - - auto initialize(Armor3d const& armor, TimePoint t) -> void { - device = armor.genre; - color = armor_color2camp_color(armor.color); - armor_num = EKFParameters::armor_num(armor.genre); - time_stamp = t; - update_count = 0; - last_matched_armor_id = 0; - ekf = EKF { EKFParameters::x(armor), EKFParameters::P_initial_dig(device).asDiagonal() }; - initialized = true; - } - - auto predict(TimePoint t) -> void { - if (t <= time_stamp) return; + auto predict(double dt) -> void { + if (!(dt > 0.0)) return; if (initialized) { - auto dt = util::delta_time(t, time_stamp); - if (dt > reset_interval) { + if (std::chrono::duration { dt } > reset_interval) { initialized = false; update_count = 0; last_matched_armor_id = kUnknownMatchedArmorId; - time_stamp = t; return; } - auto dt_s = dt.count(); ekf.predict( - EKFParameters::f(dt_s), [dt_s](EKF::XVec const&) { return EKFParameters::F(dt_s); }, - EKFParameters::Q(dt_s)); + EKFParameters::f(dt), [dt](EKF::XVec const&) { return EKFParameters::F(dt); }, + EKFParameters::Q(dt)); } - - time_stamp = t; } auto update(std::span armors) -> bool { if (armors.empty()) return false; if (!initialized) { - initialize(armors.front(), time_stamp); - ++update_count; + auto const& armor = armors.front(); + device = armor.genre; + color = armor_color2camp_color(armor.color); + armor_num = EKFParameters::armor_num(armor.genre); + update_count = 1; + last_matched_armor_id = 0; + ekf = + EKF { EKFParameters::x(armor), EKFParameters::P_initial_dig(device).asDiagonal() }; + initialized = true; return true; } @@ -107,9 +93,9 @@ struct RegularRobotState::Impl { return r_ok && l_ok && update_count >= min_updates; } - auto get_snapshot() const -> std::optional { + auto get_snapshot(TimePoint stamp) const -> std::optional { if (!initialized) return std::nullopt; - return Snapshot { RegularSnapshot { ekf.x, device, color, armor_num, time_stamp } }; + return Snapshot { RegularSnapshot { ekf.x, device, color, armor_num, stamp } }; } auto distance() const -> double { @@ -220,20 +206,13 @@ struct RegularRobotState::Impl { }; RegularRobotState::RegularRobotState() noexcept - : RegularRobotState(Clock::now()) { } - -RegularRobotState::RegularRobotState(TimePoint stamp) noexcept - : pimpl { std::make_unique(stamp) } { } + : pimpl { std::make_unique() } { } RegularRobotState::~RegularRobotState() noexcept = default; RegularRobotState::RegularRobotState(RegularRobotState&&) noexcept = default; auto RegularRobotState::operator=(RegularRobotState&&) noexcept -> RegularRobotState& = default; -auto RegularRobotState::initialize(Armor3d const& armor, TimePoint t) -> void { - return pimpl->initialize(armor, t); -} - -auto RegularRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } +auto RegularRobotState::predict(double dt) -> void { return pimpl->predict(dt); } auto RegularRobotState::update(std::span armors) -> bool { return pimpl->update(armors); @@ -241,8 +220,8 @@ auto RegularRobotState::update(std::span armors) -> bool { auto RegularRobotState::is_converged() const -> bool { return pimpl->is_converged(); } -auto RegularRobotState::get_snapshot() const -> std::optional { - return pimpl->get_snapshot(); +auto RegularRobotState::get_snapshot(TimePoint stamp) const -> std::optional { + return pimpl->get_snapshot(stamp); } auto RegularRobotState::distance() const -> double { return pimpl->distance(); } diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp index de7b93b..478bf7c 100644 --- a/src/module/predictor/regular/robot_state.hpp +++ b/src/module/predictor/regular/robot_state.hpp @@ -14,18 +14,16 @@ class RegularRobotState { public: using EKF = EKFParameters::EKF; - explicit RegularRobotState(TimePoint stamp) noexcept; RegularRobotState(RegularRobotState&&) noexcept; auto operator=(RegularRobotState&&) noexcept -> RegularRobotState&; - auto initialize(Armor3d const& armor, TimePoint t) -> void; - auto predict(TimePoint t) -> void; + auto predict(double dt) -> void; auto update(std::span armors) -> bool; auto is_converged() const -> bool; - auto get_snapshot() const -> std::optional; + auto get_snapshot(TimePoint stamp) const -> std::optional; auto distance() const -> double; }; diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index ba70b2c..517489c 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -17,7 +17,7 @@ struct RegularSnapshot::Impl { , stamp { stamp } { } static auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { - auto armor = Armor3d { }; + auto armor = Armor3d {}; armor.genre = device; armor.color = camp_color2armor_color(color); armor.id = id; @@ -25,7 +25,7 @@ struct RegularSnapshot::Impl { } static auto motion_of(EKF::XVec const& x) -> TargetMotion { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + return { Point3d { x[0], x[2], x[4] }, x[7] }; } auto predict_state_at(TimePoint t) const -> EKF::XVec { @@ -36,7 +36,7 @@ struct RegularSnapshot::Impl { auto predicted_armors(TimePoint t) const -> std::vector { auto const predicted_x = predict_state_at(t); - auto armors = std::vector { }; + auto armors = std::vector {}; armors.reserve(armor_num); for (int id = 0; id < armor_num; ++id) { diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index b935653..dcf3589 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -2,6 +2,7 @@ #include "module/predictor/outpost/robot_state.hpp" #include "module/predictor/regular/robot_state.hpp" +#include "utility/time.hpp" #include #include @@ -13,33 +14,33 @@ struct RobotState::Impl { using State = std::variant; std::optional state; - TimePoint pending_time_stamp { Clock::now() }; + std::optional time_stamp; - auto emplace_state(DeviceId device, TimePoint stamp) -> State& { + auto emplace_state(DeviceId device) -> State& { switch (device) { case DeviceId::OUTPOST: - return state.emplace(std::in_place_type, stamp); + return state.emplace(std::in_place_type); default: - return state.emplace(std::in_place_type, stamp); + return state.emplace(std::in_place_type); } } - auto initialize(Armor3d const& armor, TimePoint t) -> void { - pending_time_stamp = t; - auto& target_state = emplace_state(armor.genre, t); - std::visit([&](auto& model) { model.initialize(armor, t); }, target_state); - } - auto predict(TimePoint t) -> void { - pending_time_stamp = t; + if (!time_stamp.has_value()) { + time_stamp = t; + return; + } + + const auto dt = util::delta_time(t, *time_stamp).count(); + time_stamp = t; + if (!state) return; - std::visit([t](auto& model) { model.predict(t); }, *state); + std::visit([dt](auto& model) { model.predict(dt); }, *state); } auto update(std::span armors) -> bool { if (armors.empty()) return false; - auto& target_state = - state ? *state : emplace_state(armors.front().genre, pending_time_stamp); + auto& target_state = state ? *state : emplace_state(armors.front().genre); return std::visit([armors](auto& model) { return model.update(armors); }, target_state); } @@ -49,8 +50,9 @@ struct RobotState::Impl { } auto get_snapshot() const -> std::optional { - if (!state) return std::nullopt; - return std::visit([](auto const& model) { return model.get_snapshot(); }, *state); + if (!state || !time_stamp.has_value()) return std::nullopt; + return std::visit( + [this](auto const& model) { return model.get_snapshot(*time_stamp); }, *state); } auto distance() const -> double { @@ -63,10 +65,6 @@ RobotState::RobotState() noexcept : pimpl { std::make_unique() } { } RobotState::~RobotState() noexcept = default; -auto RobotState::initialize(rmcs::Armor3d const& armor, TimePoint t) -> void { - return pimpl->initialize(armor, t); -} - auto RobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } auto RobotState::update(std::span armors) -> bool { return pimpl->update(armors); } diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index 1af14e0..10c0d45 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -12,8 +12,6 @@ struct RobotState { RMCS_PIMPL_DEFINITION(RobotState) public: - auto initialize(Armor3d const&, TimePoint) -> void; - auto predict(TimePoint t) -> void; auto update(std::span armors) -> bool; diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 5cdb640..9957e39 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -4,8 +4,6 @@ #include "utility/robot/armor.hpp" #include "utility/robot/id.hpp" -#include - #include #include @@ -16,7 +14,7 @@ class OutpostSnapshot; class Snapshot; struct TargetMotion { - Eigen::Vector3d center_position; + Point3d center_position; double angular_velocity; }; diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index 2862125..2e0ae4c 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -64,8 +64,7 @@ struct Decider::Impl { for (auto& [id, grouped] : grouped_armors) { if (!trackers.contains(id)) { trackers[id] = std::make_unique(); - trackers[id]->initialize(grouped.front(), t); - last_seen_times[id] = t; + trackers[id]->predict(t); } auto grouped_span = std::span { grouped.data(), grouped.size() }; diff --git a/src/utility/math/outpost.cpp b/src/utility/math/outpost.cpp index 02f1074..f192df9 100644 --- a/src/utility/math/outpost.cpp +++ b/src/utility/math/outpost.cpp @@ -35,6 +35,18 @@ namespace details { } +auto outpost_relative_height(bool in_right, bool in_upper) noexcept -> double { + /*^^*/ if (in_right && in_upper) { + return +2 * kOutpostArmorHeightStep; + } else if (in_right && !in_upper) { + return -1 * kOutpostArmorHeightStep; + } else if (!in_right && in_upper) { + return +1 * kOutpostArmorHeightStep; + } else { + return -2 * kOutpostArmorHeightStep; + } +} + auto OutpostSolution::solve() -> void { const auto radius = kOutpostRadius + input.armor_thickness; const auto pitch = kPredictedOutpostArmorPitch; diff --git a/src/utility/math/outpost.hpp b/src/utility/math/outpost.hpp index 063aded..95c724e 100644 --- a/src/utility/math/outpost.hpp +++ b/src/utility/math/outpost.hpp @@ -1,16 +1,18 @@ #pragma once #include "utility/robot/armor.hpp" +/// 前哨站按照下面的顺序排列，其旋向确定 +/// [ UPPER ] +/// [ MIDDLE ] +/// [ LOWER ] namespace rmcs::util { +auto outpost_relative_height(bool in_right, bool in_upper) noexcept -> double; + class OutpostSolution { public: enum class ArmorLevel { UPPER, MIDDLE, LOWER }; - /// Level 按照下面的顺序排列，其旋向确定 - /// [ UPPER ] - /// [ MIDDLE ] - /// [ LOWER ] struct Input { Translation translation; Orientation orientation; diff --git a/tool/cxx/test_outpost_ekf.cpp b/tool/cxx/test_outpost_ekf.cpp index 7e4e1f5..38ec18a 100644 --- a/tool/cxx/test_outpost_ekf.cpp +++ b/tool/cxx/test_outpost_ekf.cpp @@ -5,15 +5,17 @@ #include "utility/robot/armor.hpp" #include "utility/robot/constant.hpp" +#include #include #include #include -#include #include #include #include #include -#include +#include + +#include #include using namespace rmcs; @@ -28,60 +30,85 @@ auto main() -> int { auto marker_publisher = node.details->make_pub( "/rmcs/auto_aim/outpost_test/ekf", qos::debug); - const auto true_center = Eigen::Vector3d { 2.0, 0.0, 2.0 }; - constexpr auto angular_speed = kOutpostAngularSpeed; // +0.8 pi, CCW - constexpr auto radius = kOutpostRadius; - constexpr auto pitch = kPredictedOutpostArmorPitch; - constexpr auto dt = 0.01; + const auto kTrueCenter = Eigen::Vector3d { 2.0, 0.0, 2.0 }; + constexpr auto kAngularSpeed = kOutpostAngularSpeed; // +0.8 pi, CCW + constexpr auto kRadius = kOutpostRadius; + constexpr auto kPitch = kPredictedOutpostArmorPitch; + constexpr auto kDt = 0.01; - constexpr auto noise_sigma = 0.30; - constexpr auto outlier_probability = 0.05; - constexpr auto outlier_sigma = 1.0; - constexpr auto orientation_sigma = 0.05; - constexpr auto observation_cloud_capacity = 50; + static constexpr auto kOffsetTable = std::array { + std::tuple { 0 * std::numbers::pi * 2 / 3, -0 * kOutpostArmorHeightStep }, // 高 + std::tuple { 1 * std::numbers::pi * 2 / 3, -1 * kOutpostArmorHeightStep }, // 中 + std::tuple { 2 * std::numbers::pi * 2 / 3, -2 * kOutpostArmorHeightStep }, // 低 + }; + + constexpr auto kNoiseSigma = 0.03; + constexpr auto kOutlierProbability = 0.0; + constexpr auto kOutlierSigma = 1.0; + constexpr auto kOrientationSigma = 0.05; + constexpr auto kObservationCloudCapacity = 50; auto noise_engine = std::mt19937 { 42 }; - auto noise_distribution = std::normal_distribution { 0.0, noise_sigma }; - auto outlier_distribution = std::normal_distribution { 0.0, outlier_sigma }; + auto noise_distribution = std::normal_distribution { 0.0, kNoiseSigma }; + auto outlier_distribution = std::normal_distribution { 0.0, kOutlierSigma }; auto orientation_noise_distribution = - std::normal_distribution { 0.0, orientation_sigma }; + std::normal_distribution { 0.0, kOrientationSigma }; auto uniform_distribution = std::uniform_real_distribution { 0.0, 1.0 }; auto model = std::unique_ptr { }; auto elapsed = 0.0; auto frame_index = 0; + auto last_observed_id = int { -1 }; auto observation_cloud = std::deque { }; - const auto start_time = std::chrono::steady_clock::now(); + const auto kStartTime = std::chrono::steady_clock::now(); while (rclcpp::ok()) { - elapsed += dt; + elapsed += kDt; const auto stamp = node.details->rclcpp->now(); - const auto angle = angular_speed * elapsed; - const auto true_position = Eigen::Vector3d { - true_center.x() + radius * std::cos(angle), - true_center.y() + radius * std::sin(angle), - true_center.z(), - }; - const auto direction = true_position - true_center; - const auto yaw_rotation = - Eigen::AngleAxisd { std::atan2(direction.y(), direction.x()) + std::numbers::pi, - Eigen::Vector3d::UnitZ() }; - const auto pitch_rotation = Eigen::AngleAxisd { pitch, Eigen::Vector3d::UnitY() }; - const auto true_orientation = Eigen::Quaterniond { yaw_rotation * pitch_rotation }; + const auto angle = kAngularSpeed * elapsed; + + auto true_positions = std::array { }; + auto true_orientations = std::array { }; + for (std::size_t index = 0; index < kOffsetTable.size(); ++index) { + const auto [yaw_offset, height_offset] = kOffsetTable[index]; + const auto armor_yaw = util::normalize_angle(angle + yaw_offset); + + true_positions[index] = Eigen::Vector3d { + kTrueCenter.x() - kRadius * std::cos(armor_yaw), + kTrueCenter.y() - kRadius * std::sin(armor_yaw), + kTrueCenter.z() + height_offset, + }; + true_orientations[index] = + Eigen::Quaterniond { Eigen::AngleAxisd { armor_yaw, Eigen::Vector3d::UnitZ() } + * Eigen::AngleAxisd { kPitch, Eigen::Vector3d::UnitY() } }; + } + + auto observed_id = std::size_t { 0 }; + for (std::size_t index = 1; index < true_positions.size(); ++index) { + if (true_positions[index].x() >= true_positions[observed_id].x()) continue; + observed_id = index; + } + + const auto plate_switched = + last_observed_id >= 0 && observed_id != static_cast(last_observed_id); + last_observed_id = static_cast(observed_id); + + const auto true_position = true_positions[observed_id]; + const auto true_orientation = true_orientations[observed_id]; auto distance_direction = Eigen::Vector3d { true_position.normalized() }; auto noisy_position = Eigen::Vector3d { true_position + distance_direction * noise_distribution(noise_engine) }; - auto is_outlier = uniform_distribution(noise_engine) < outlier_probability; + auto is_outlier = uniform_distribution(noise_engine) < kOutlierProbability; if (is_outlier) { noisy_position += distance_direction * outlier_distribution(noise_engine); } observation_cloud.push_back(noisy_position); - if (observation_cloud.size() > observation_cloud_capacity) { + if (observation_cloud.size() > kObservationCloudCapacity) { observation_cloud.pop_front(); } @@ -102,7 +129,7 @@ auto main() -> int { if (!model) { model = std::make_unique(noisy_armor); } else { - model->predict(dt); + model->predict(kDt); model->correct(noisy_armor); } @@ -112,15 +139,7 @@ auto main() -> int { estimated_state.y, estimated_state.z, }; - const auto estimated_armor_position = Eigen::Vector3d { - estimated_center.x() - radius * std::cos(estimated_state.rotation_angle), - estimated_center.y() - radius * std::sin(estimated_state.rotation_angle), - estimated_center.z(), - }; - const auto estimated_orientation = Eigen::Quaterniond { - Eigen::AngleAxisd { estimated_state.rotation_angle, Eigen::Vector3d::UnitZ() } - * Eigen::AngleAxisd { pitch, Eigen::Vector3d::UnitY() } - }; + const auto estimated_armors = model->full(); auto marker_array = visualization_msgs::msg::MarkerArray { }; auto marker_index = 0; @@ -155,15 +174,16 @@ auto main() -> int { marker.pose.orientation.z = orientation.z(); }; - { + for (std::size_t index = 0; index < true_positions.size(); ++index) { auto marker = make_marker("true_armor"); marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale.x = 0.003; marker.scale.y = 0.135; marker.scale.z = 0.056; marker.color.g = 1.0; - marker.color.a = 1.0; - set_pose(marker, true_position, true_orientation); + marker.color.b = index == observed_id ? 0.0 : 1.0; + marker.color.a = index == observed_id ? 1.0 : 0.25; + set_pose(marker, true_positions[index], true_orientations[index]); marker_array.markers.emplace_back(std::move(marker)); } @@ -205,7 +225,10 @@ auto main() -> int { marker_array.markers.emplace_back(std::move(marker)); } - { + for (const auto& armor : estimated_armors) { + const auto position = armor.translation.make(); + const auto orientation = armor.orientation.make(); + auto marker = make_marker("estimated_armor"); marker.type = visualization_msgs::msg::Marker::CUBE; marker.scale.x = 0.003; @@ -214,7 +237,7 @@ auto main() -> int { marker.color.r = 1.0; marker.color.g = 1.0; marker.color.a = 1.0; - set_pose(marker, estimated_armor_position, estimated_orientation); + set_pose(marker, position, orientation); marker_array.markers.emplace_back(std::move(marker)); } @@ -227,8 +250,8 @@ auto main() -> int { for (int step = 0; step <= 36; ++step) { const auto phi = 2.0 * std::numbers::pi * step / 36.0; marker.points.emplace_back(make_point(Eigen::Vector3d { - estimated_center.x() + radius * std::cos(phi), - estimated_center.y() + radius * std::sin(phi), + estimated_center.x() + kRadius * std::cos(phi), + estimated_center.y() + kRadius * std::sin(phi), estimated_center.z(), })); } @@ -237,14 +260,24 @@ auto main() -> int { marker_publisher->publish(marker_array); - if (frame_index % 10 == 0) { - const auto center_error = (estimated_center - true_center).norm(); - const auto true_yaw = util::normalize_angle(angle + std::numbers::pi); + if (plate_switched || frame_index % 10 == 0) { + const auto reference_center = Eigen::Vector3d { + kTrueCenter.x(), + kTrueCenter.y(), + kTrueCenter.z() + std::get<1>(kOffsetTable[0]), + }; + const auto center_error = (estimated_center - reference_center).norm(); + + // state.rotation_angle 约定为 center→armor 方向，true_yaw 需相应转换 + // test 的 angle 是 armor→center 方向，加 π 得到 center→armor + const auto true_yaw = + util::normalize_angle(angle + std::get<0>(kOffsetTable[0]) + std::numbers::pi); const auto angle_error = std::abs(util::normalize_angle(estimated_state.rotation_angle - true_yaw)); std::cout << std::format("elapsed={:6.2f}s center_err={:.4f}m omega_est={:.4f} " - "omega_true={:.4f} angle_err={:.4f}rad{}\n", - elapsed, center_error, estimated_state.rotation_speed, angular_speed, angle_error, + "omega_true={:.4f} angle_err={:.4f}rad obs={}{}{}\n", + elapsed, center_error, estimated_state.rotation_speed, kAngularSpeed, angle_error, + observed_id, plate_switched ? " [PLATE_SWITCH]" : "", is_outlier ? " [OUTLIER]" : ""); }