refactor: ekf and debug tool

AGENTS.md

@@ -13,3 +13,8 @@ build-rmcs --packages-up-to rmcs_auto_aim_v2
 cmake -B build
 cmake --build build -j
 ```
+
+
+2. 规范
+
+- 项目头文件遵循最小引入原则，如果某个类型通过头文件间接引入了，那就不需要再引入该头文件

src/kernel/auto_aim.cpp

@@ -142,18 +142,26 @@ struct AutoAim::Impl {
                     cmd.pitch          = result->pitch;
                     cmd.robot_center   = result->center_position;
 
-                    if (result->feedforward.has_value()) {
-                        cmd.yaw_rate   = result->feedforward->yaw_rate;
-                        cmd.pitch_rate = result->feedforward->pitch_rate;
-                        cmd.yaw_acc    = result->feedforward->yaw_acc;
-                        cmd.pitch_acc  = result->feedforward->pitch_acc;
+                    if (auto feedforward = result->feedforward) {
+                        cmd.yaw_rate   = feedforward->yaw_rate;
+                        cmd.pitch_rate = feedforward->pitch_rate;
+                        cmd.yaw_acc    = feedforward->yaw_acc;
+                        cmd.pitch_acc  = feedforward->pitch_acc;
                     }
 
                     auto impact_armors = snapshot->predicted_armors(result->impact_time);
                     visual.publish(impact_armors, "impact_robot");
                     visual.update_aiming_direction(cmd.yaw, cmd.pitch);
                     visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate,
                         cmd.yaw_acc, cmd.pitch_acc);
+
+                    if (auto aim_2d = estimator.make_point2d(result->aim_point)) {
+                        visual.draw_later(Canvas::Point {
+                            .origin = aim_2d->make<cv::Point2i>(),
+                            .radius = 5,
+                            .color  = result->shoot_permitted ? kRed : kGreen,
+                        });
+                    }
                 }
             }
             visual.draw_later(

src/kernel/fire_control.cpp

@@ -162,6 +162,7 @@ struct FireControl::Impl {
             .feedforward     = feedforward,
             .shoot_permitted = shoot_permitted,
             .center_position = Point3d { center_position },
+            .aim_point       = Point3d { aim_point_position },
             .impact_time     = target_solution.impact_time,
         };
     }

src/kernel/fire_control.hpp

@@ -30,6 +30,7 @@ class FireControl {
         std::optional<Feedforward> feedforward;
         bool shoot_permitted;
         Point3d center_position;
+        Point3d aim_point;
         TimePoint impact_time;
     };
 

src/kernel/pose_estimator.cpp

@@ -117,11 +117,11 @@ struct PoseEstimator::Impl {
 
         auto transformed = armor;
 
-        auto position = Eigen::Vector3d {};
+        auto position = Eigen::Vector3d { };
         transformed.translation.copy_to(position);
         transformed.translation = camera_orientation.inverse() * (position - camera_translation);
 
-        auto quat = Eigen::Quaterniond {};
+        auto quat = Eigen::Quaterniond { };
         transformed.orientation.copy_to(quat);
         transformed.orientation = camera_orientation.inverse() * quat;
 
@@ -271,6 +271,21 @@ struct PoseEstimator::Impl {
         }
         return armor3ds;
     }
+
+    auto make_point2d(const Point3d& point_odom) const -> std::optional<Point2d> {
+        const auto t = camera_feature.translation.make<Eigen::Vector3d>();
+        const auto q = camera_feature.orientation.make<Eigen::Quaterniond>();
+
+        // odom (ROS) -> camera (OpenCV)
+        const auto point_ros_odom   = point_odom.make<Eigen::Vector3d>();
+        const auto point_ros_camera = Eigen::Vector3d { q.inverse() * (point_ros_odom - t) };
+
+        const auto point_opencv = ros2opencv_position(point_ros_camera);
+        const auto point_cv     = Point3d { point_opencv };
+
+        // reproject
+        return util::reproject_point(point_cv, camera_feature);
+    }
 };
 
 auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept
@@ -298,6 +313,10 @@ auto PoseEstimator::update_camera_transform(const Transform& transform) -> void
     return pimpl->update_camera_transform(transform);
 }
 
+auto PoseEstimator::make_point2d(const Point3d& point_odom) const -> std::optional<Point2d> {
+    return pimpl->make_point2d(point_odom);
+}
+
 PoseEstimator::PoseEstimator() noexcept
     : pimpl { std::make_unique<Impl>() } { }
 

src/kernel/pose_estimator.hpp

@@ -33,6 +33,8 @@ class PoseEstimator {
     auto estimate_armor(const std::vector<Armor2d>&) const -> Armor3ds;
     auto estimate_armor(const std::vector<Armor2d>&, Image&) const -> Armor3ds;
 
+    auto make_point2d(const Point3d& point_odom) const -> std::optional<Point2d>;
+
     auto addition() -> const Addition&;
 };
 

src/module/predictor/model/outpost.cpp

@@ -0,0 +1,191 @@
+#include "outpost.hpp"
+
+#include "utility/math/angle.hpp"
+#include "utility/robot/constant.hpp"
+
+#include <eigen3/Eigen/Geometry>
+
+using namespace rmcs;
+
+struct OutpostModel::Impl {
+    using StateVector         = Eigen::Matrix<double, 5, 1>;
+    using Covariance          = Eigen::Matrix<double, 5, 5>;
+    using ProcessNoise        = Eigen::Matrix<double, 5, 5>;
+    using ObservationNoise    = Eigen::Matrix<double, 4, 4>;
+    using ObservationVector   = Eigen::Matrix<double, 4, 1>;
+    using ObservationJacobian = Eigen::Matrix<double, 4, 5>;
+    using KalmanGain          = Eigen::Matrix<double, 5, 4>;
+
+    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;
+        }
+
+        static_assert(sizeof(State) == sizeof(posteriors_state));
+        auto state() noexcept -> State& {
+            auto* data_pointer = &posteriors_state;
+            return *reinterpret_cast<State*>(data_pointer);
+        }
+        auto state() const noexcept -> const State& {
+            const auto* data_pointer = &posteriors_state;
+            return *reinterpret_cast<const State*>(data_pointer);
+        }
+
+        auto update_center(const Eigen::Vector3d& point) noexcept {
+            state().x = point.x();
+            state().y = point.y();
+            state().z = point.z();
+        }
+    } context;
+
+    auto initialize_from(const Armor3d& armor) noexcept -> void {
+        constexpr auto pitch  = kPredictedOutpostArmorPitch;
+        constexpr auto radius = kOutpostRadius;
+
+        const auto translation = armor.translation.make<Eigen::Vector3d>();
+        const auto orientation = armor.orientation.make<Eigen::Quaterniond>();
+
+        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.state().rotation_speed = 0.0;
+        context.state().rotation_angle =
+            util::normalize_angle(std::atan2(-armor_to_center.y(), -armor_to_center.x()));
+
+        auto initial_covariance_diagonal = Eigen::Matrix<double, 5, 1> { };
+        initial_covariance_diagonal << 1.0, 1.0, 1.0, 64.0, 0.4;
+
+        context.posteriors_covariance = initial_covariance_diagonal.asDiagonal();
+    }
+
+    static auto predict_state(double dt, const State& last) -> State {
+        auto next = State { last };
+
+        next.rotation_angle = util::normalize_angle(last.rotation_angle + last.rotation_speed * dt);
+        return next;
+    }
+
+    auto predict_covariance(double dt, const Covariance& covariance) const -> Covariance {
+        auto jacobian  = Covariance { Covariance::Identity() };
+        jacobian(4, 3) = dt;
+        return jacobian * covariance * jacobian.transpose() + context.noise_process;
+    }
+
+    static auto measure_innovation(const ObservationVector& observation, const State& prior_state)
+        -> ObservationVector {
+        auto predicted_observation = ObservationVector { };
+        predicted_observation << prior_state.x, prior_state.y, prior_state.z,
+            prior_state.rotation_angle;
+
+        auto innovation        = ObservationVector { observation - predicted_observation };
+        innovation.coeffRef(3) = util::normalize_angle(innovation.coeff(3));
+        return innovation;
+    }
+
+    auto calculate_kalman_gain(const Covariance& prior_covariance) const -> KalmanGain {
+        const auto jacobian = observation_jacobian();
+
+        const auto innovation_covariance =
+            jacobian * prior_covariance * jacobian.transpose() + context.noise_observation;
+
+        return prior_covariance * jacobian.transpose()
+            * innovation_covariance.ldlt().solve(ObservationNoise::Identity());
+    }
+
+    auto a_posteriori_update(const State& prior_state, const Covariance& prior_covariance,
+        const KalmanGain& kalman_gain, const ObservationVector& innovation,
+        const ObservationJacobian& jacobian) const -> std::pair<State, Covariance> {
+
+        auto posterior_state = State { prior_state };
+        reinterpret_cast<StateVector&>(posterior_state).noalias() += kalman_gain * innovation;
+        posterior_state.rotation_angle = util::normalize_angle(posterior_state.rotation_angle);
+
+        const auto complement          = Covariance::Identity() - kalman_gain * jacobian;
+        auto posterior_covariance      = Covariance { };
+        posterior_covariance.noalias() = complement * prior_covariance * complement.transpose();
+        posterior_covariance +=
+            (kalman_gain * context.noise_observation * kalman_gain.transpose()).eval();
+        posterior_covariance = 0.5 * (posterior_covariance + posterior_covariance.transpose());
+
+        return { posterior_state, posterior_covariance };
+    }
+
+    auto predict(double dt) noexcept -> void {
+        const auto prior_state      = predict_state(dt, context.state());
+        const auto prior_covariance = predict_covariance(dt, context.posteriors_covariance);
+
+        context.state()               = prior_state;
+        context.posteriors_covariance = prior_covariance;
+    }
+
+    auto correct(const Armor3d& armor) noexcept -> void {
+        { // 切板检测
+        }
+
+        constexpr auto pitch = kPredictedOutpostArmorPitch;
+
+        const auto orientation = armor.orientation.make<Eigen::Quaterniond>();
+        const auto translation = armor.translation.make<Eigen::Vector3d>();
+
+        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 + kOutpostRadius * armor_to_center };
+
+        const auto top_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;
+
+        const auto prior_state      = context.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 [posterior_state, posterior_covariance] =
+            a_posteriori_update(prior_state, prior_covariance, kalman_gain, innovation, jacobian);
+
+        context.state()               = posterior_state;
+        context.posteriors_covariance = posterior_covariance;
+    }
+};
+
+OutpostModel::OutpostModel(const Armor3d& armor) noexcept
+    : pimpl { std::make_unique<Impl>() } {
+    pimpl->initialize_from(armor);
+}
+
+OutpostModel::~OutpostModel() noexcept = default;
+
+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(); }

src/module/predictor/model/outpost.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "utility/pimpl.hpp"
+#include "utility/robot/armor.hpp"
+
+namespace rmcs {
+
+class OutpostModel {
+    RMCS_PIMPL_DEFINITION(OutpostModel)
+
+public:
+    struct State {
+        // 前哨站旋转中心在世界坐标系下的位置
+        double x;
+        double y;
+        double z;
+
+        // 以参考装甲板朝向为基准的角速度与 yaw 角
+        double rotation_speed;
+        double rotation_angle;
+    };
+
+    explicit OutpostModel(const Armor3d& armor) noexcept;
+
+    auto predict(double dt) noexcept -> void;
+    auto correct(const Armor3d& armor) noexcept -> void;
+    auto state() noexcept -> State;
+};
+
+}

src/module/predictor/model/robot.hpp

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace rmcs {
+
+class RobotModel { };
+
+}