refactor: outpost ekf - #63
Merged
Merged
Conversation
- remove explicit initialize from robot state models - switch regular/outpost prediction APIs to dt-based updates - lazy-initialize models from first observation update - keep snapshot timestamps managed by RobotState
- replace tracker outpost state internals with OutpostModel adapter - snapshot OutpostModel state and predicted armors - use Point3d for TargetMotion center position - adapt fire-control consumers to Point3d motion centers
Contributor
代码概览本次 PR 对前哨站目标预测的 EKF 实现进行全面重构，将内部状态改为 变更内容前哨站 EKF 与预测器时间 API 重构
时序图sequenceDiagram
participant Decider
participant RobotState
participant OutpostRobotState
participant OutpostModel
participant OutpostSnapshot
Decider->>RobotState: predict(TimePoint t)
RobotState->>RobotState: delta_time(t, time_stamp) → dt
RobotState->>OutpostRobotState: predict(dt)
OutpostRobotState->>OutpostModel: predict(dt)
Decider->>RobotState: update(armors)
RobotState->>OutpostRobotState: update(armors)
OutpostRobotState->>OutpostModel: correct(armor)
OutpostModel->>OutpostModel: 切板检测 + delta_yaw/delta_height 补偿
OutpostModel->>OutpostModel: measure_innovation → calculate_kalman_gain → a_posteriori_update
Decider->>RobotState: get_snapshot(t)
RobotState->>OutpostRobotState: get_snapshot(t)
OutpostRobotState->>OutpostModel: full() → vector~Armor3d~
OutpostRobotState->>OutpostModel: state() → State
OutpostRobotState->>OutpostSnapshot: OutpostSnapshot(State, armors, stamp)
OutpostSnapshot-->>Decider: Snapshot
预估代码审查工作量🎯 5 (Critical) | ⏱️ ~120 分钟 相关 PR
建议标签
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
Contributor
There was a problem hiding this comment.
Actionable comments posted: 4
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/module/predictor/model/outpost.cpp`:
- Around line 444-453: `OutpostModel::full()` currently assumes
`pimpl->at(0..2)` is already mapped to “highest to lowest,” but `abs_ref_index`
is still provisional until all three plates are observed, so early snapshots can
violate the `full()` contract and mislead callers. Update `OutpostModel` to
track whether the absolute plate order is resolved (for example via a dedicated
readiness state tied to `abs_ref_index`/`rel_see_index`), and prevent `full()`
from exposing ordered output until that state is true; alternatively, initialize
`abs_ref_index` from a reliable prior so the ordering is correct from the first
frame. Keep `current()` consistent with the same resolution state.
- Around line 335-344: The index updates in outpost.cpp are using two different
directions: `height_buff.go_next()` follows `go_sign`, but `rel_see_index` is
still advanced with the raw `sign`, which can desynchronize the board state when
`rotation_sign` is already locked. Update the `rel_see_index` adjustment in the
same block to use the same confirmed direction variable as
`height_buff.go_next()` (the `go_sign` logic), so all cut-sheet indices advance
consistently and the downstream `delta_yaw`/`delta_height` compensation stays
aligned.
In `@src/module/predictor/outpost/robot_state.cpp`:
- Around line 21-30: `OutpostRobotState::update` is only using `armors.front()`,
which drops same-frame observations and makes EKF correction depend on detection
order. Update the method to process the full `std::span<Armor3d const>` from
`Decider`, applying all valid armors in the batch when initializing or
correcting the `OutpostModel`, rather than only the first element.
In `@src/module/predictor/robot_state.cpp`:
- Around line 34-38: In `RobotState::predict`, the `time_stamp` is updated
before validating whether `dt` is usable, which can cause the parent timestamp
to move backward on out-of-order input and lead to over-prediction. Reorder the
logic so the `dt` check happens first, and only assign `time_stamp = t` when
`dt` is valid and the prediction will actually proceed; keep the existing
`std::visit` model prediction path unchanged.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: eee57bcd-0b1d-419f-9787-0f5ff65a8b1c
📒 Files selected for processing (24)
README.mdconfig/config.yamlsrc/kernel/auto_aim.cppsrc/kernel/fire_control.cppsrc/module/fire_control/armor_selector.cppsrc/module/fire_control/target_solver.cppsrc/module/predictor/model/outpost.cppsrc/module/predictor/model/outpost.hppsrc/module/predictor/outpost/armor_layout.hppsrc/module/predictor/outpost/ekf_parameter.hppsrc/module/predictor/outpost/robot_state.cppsrc/module/predictor/outpost/robot_state.hppsrc/module/predictor/outpost/snapshot.cppsrc/module/predictor/outpost/snapshot.hppsrc/module/predictor/regular/robot_state.cppsrc/module/predictor/regular/robot_state.hppsrc/module/predictor/regular/snapshot.cppsrc/module/predictor/robot_state.cppsrc/module/predictor/robot_state.hppsrc/module/predictor/snapshot.hppsrc/module/tracker/decider.cppsrc/utility/math/outpost.cppsrc/utility/math/outpost.hpptool/cxx/test_outpost_ekf.cpp
💤 Files with no reviewable changes (4)
- src/module/predictor/outpost/ekf_parameter.hpp
- src/module/predictor/outpost/armor_layout.hpp
- README.md
- src/module/predictor/robot_state.hpp
Comment on lines
21
to
30
| auto update(std::span<Armor3d const> armors) -> bool { | ||
| if (armors.empty()) return false; | ||
|
|
||
| if (!initialized) { | ||
| initialize(armors.front(), time_stamp); | ||
| ++update_count; | ||
| return true; | ||
| if (!model) { | ||
| model = std::make_unique<OutpostModel>(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; |
Contributor
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
不要丢弃同帧的其余前哨站观测。
Decider 会把同一 DeviceId 的装甲板作为 span 传入；这里只用 armors.front() 会让 EKF 修正依赖检测顺序，并忽略同帧的额外有效观测。
建议修改
if (armors.empty()) return false;
+ auto next = armors.begin();
if (!model) {
- model = std::make_unique<OutpostModel>(armors.front());
- } else {
- model->correct(armors.front());
+ model = std::make_unique<OutpostModel>(*next++);
+ }
+
+ for (; next != armors.end(); ++next) {
+ model->correct(*next);
}
return true;📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| auto update(std::span<Armor3d const> armors) -> bool { | |
| if (armors.empty()) return false; | |
| if (!initialized) { | |
| initialize(armors.front(), time_stamp); | |
| ++update_count; | |
| return true; | |
| if (!model) { | |
| model = std::make_unique<OutpostModel>(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 update(std::span<Armor3d const> armors) -> bool { | |
| if (armors.empty()) return false; | |
| auto next = armors.begin(); | |
| if (!model) { | |
| model = std::make_unique<OutpostModel>(*next++); | |
| } | |
| for (; next != armors.end(); ++next) { | |
| model->correct(*next); | |
| } | |
| return true; |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/module/predictor/outpost/robot_state.cpp` around lines 21 - 30,
`OutpostRobotState::update` is only using `armors.front()`, which drops
same-frame observations and makes EKF correction depend on detection order.
Update the method to process the full `std::span<Armor3d const>` from `Decider`,
applying all valid armors in the batch when initializing or correcting the
`OutpostModel`, rather than only the first element.
Comment on lines
+34
to
+38
| 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); |
Contributor
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
先校验 dt 再推进 time_stamp。
Line 35 会在子模型忽略 dt <= 0 之前更新父级时间戳；如果输入时间戳乱序，下一帧会从回退后的时间重新计算 dt，导致过度预测。
建议修改
const auto dt = util::delta_time(t, *time_stamp).count();
+ if (!(dt > 0.0)) return;
+
time_stamp = t;
if (!state) return;📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| 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); | |
| const auto dt = util::delta_time(t, *time_stamp).count(); | |
| if (!(dt > 0.0)) return; | |
| time_stamp = t; | |
| if (!state) return; | |
| std::visit([dt](auto& model) { model.predict(dt); }, *state); |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/module/predictor/robot_state.cpp` around lines 34 - 38, In
`RobotState::predict`, the `time_stamp` is updated before validating whether
`dt` is usable, which can cause the parent timestamp to move backward on
out-of-order input and lead to over-prediction. Reorder the logic so the `dt`
check happens first, and only assign `time_stamp = t` when `dt` is valid and the
prediction will actually proceed; keep the existing `std::visit` model
prediction path unchanged.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
概述
完全重构了前哨站的 EKF，结合先前的前哨站专属距离优化，大幅度提升了收敛速度和稳定程度，同时具有强大的自纠正能力
实机效果
实测距离在 7m 内能做到 90% 左右的命中率，这个距离下弹道的 yaw 散布为半块装甲板，pitch 散布为 1/3 块装甲板
8m.mp4
概述
本次提交对 outpost EKF 进行了整体重构，并顺带统一了预测/快照接口的时间推进方式，移除了旧的初始化路径与部分过时的 outpost 布局实现。
主要变更
OutpostModel：Configfull()/current()等接口OutpostRobotState/RegularRobotState/RobotState：dtinitialize(...)初始化入口OutpostSnapshot：OutpostModel::State和装甲集合TargetMotion.center_position改为Point3darmor_layout.hppconfig.yaml中 outpost/火控参数README.md中对外 Topic 说明结果
整体上，这次修改将 outpost 目标跟踪从“单装甲、旧初始化路径”升级为“多装甲、自校正更强、时间推进更统一”的实现，并同步清理了相关接口与文档。