diff --git a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp index c4df676..12943f5 100644 --- a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp +++ b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp @@ -33,6 +33,7 @@ #include // std #include +#include #include #include #include @@ -116,6 +117,12 @@ private: std::atomic shoot_rate_max_{13}; std::atomic ekf_detect_count_{0}; std::atomic ekf_count_threshold_{30}; + + // Adaptive Q based on NIS + std::deque nis_history_; + int nis_window_size_{20}; + double adaptive_q_scale_{1.0}; + double nis_adapt_range_{2.0}; // Enable/Disable Armor Solver bool enable_; diff --git a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp index 1fd82fe..740b654 100644 --- a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp +++ b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp @@ -52,6 +52,9 @@ public: void setMatchThreshold(double max_match_distance, double max_match_yaw_diff) noexcept; + // Get NIS (Normalized Innovation Squared) from EKF for adaptive Q tuning + double getNIS() const noexcept { return ekf->getNIS(); } + enum State { LOST, DETECTING, diff --git a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp index de2a95e..6f465d8 100644 --- a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp +++ b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp @@ -70,27 +70,37 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options) s2qyaw_min_ = declare_parameter("ekf.sigma2_q_yaw_min", 50.0); s2qr_ = declare_parameter("ekf.sigma2_q_r", 800.0); s2qd_zc_ = declare_parameter("ekf.sigma2_q_d_zc", 800.0); + nis_window_size_ = declare_parameter("ekf.nis_window_size", 20); + nis_adapt_range_ = declare_parameter("ekf.nis_adapt_range", 2.0); auto u_q = [this]() { Eigen::Matrix q; double t = dt_; - + // Get current state for adaptive calculation const auto &state = tracker_->target_state; double vx = state(1), vy = state(3), v_yaw = state(7); - + // Adaptive noise calculation: // - High angular velocity (spinning) -> lower position noise (trust position measurement more) // - High linear velocity (moving fast) -> lower yaw noise (trust yaw measurement more) double linear_vel = std::sqrt(vx * vx + vy * vy); double angular_vel = std::abs(v_yaw); - + // Exponential decay: when velocity is high, noise approaches min value double s2q_xyz = std::exp(-angular_vel) * (s2qxyz_max_ - s2qxyz_min_) + s2qxyz_min_; double s2q_yaw = std::exp(-linear_vel) * (s2qyaw_max_ - s2qyaw_min_) + s2qyaw_min_; - - double r = s2qr_, d_zc = s2qd_zc_; - + + // Apply NIS-based adaptive scaling + // If NIS > Z_N (4), increase Q (trust model less) + // If NIS < Z_N (4), decrease Q (trust model more) + double q_scale = adaptive_q_scale_; + + s2q_xyz *= q_scale; + s2q_yaw *= q_scale; + + double r = s2qr_ * q_scale, d_zc = s2qd_zc_ * q_scale; + // White noise integral model for position-velocity state double q_x_x = pow(t, 4) / 4 * s2q_xyz, q_x_vx = pow(t, 3) / 2 * s2q_xyz, q_vx_vx = pow(t, 2) * s2q_xyz; double q_y_y = pow(t, 4) / 4 * s2q_xyz, q_y_vy = pow(t, 3) / 2 * s2q_xyz, q_vy_vy = pow(t, 2) * s2q_xyz; @@ -382,6 +392,9 @@ void ArmorSolverNode::armorsCallback(const rm_interfaces::msg::Armors::SharedPtr target_msg.tracking = false; // Reset EKF detect count when tracker is lost ekf_detect_count_.store(0, std::memory_order_relaxed); + // Reset NIS history and adaptive Q scale + nis_history_.clear(); + adaptive_q_scale_ = 1.0; } else { // Increment EKF detect count when successfully tracking (not lost) ekf_detect_count_.fetch_add(1, std::memory_order_relaxed); @@ -393,6 +406,37 @@ void ArmorSolverNode::armorsCallback(const rm_interfaces::msg::Armors::SharedPtr tracker_->ekf->setPredictFunc(Predict{dt_, MotionModel::CONSTANT_VEL_ROT}); } tracker_->update(armors_msg); + + // Update adaptive Q scale based on NIS (Normalized Innovation Squared) + // NIS follows chi-square distribution with Z_N degrees of freedom + // Expected NIS ≈ Z_N (4 for our 4D measurement) + double nis = tracker_->getNIS(); + if (nis > 0 && std::isfinite(nis)) { + nis_history_.push_back(nis); + if (nis_history_.size() > static_cast(nis_window_size_)) { + nis_history_.pop_front(); + } + + // Compute average NIS from history + double avg_nis = 0; + for (double v : nis_history_) { + avg_nis += v; + } + avg_nis /= nis_history_.size(); + + // Adaptive scale: if avg_nis > Z_N, increase Q; if avg_nis < Z_N, decrease Q + // NIS_adapt_range controls sensitivity (nis_adapt_range_ = 2.0 means NIS > 6 or NIS < 2 triggers adaptation) + double nis_ratio = avg_nis / static_cast(Z_N); + double adapt_factor = 1.0 + (nis_ratio - 1.0) * 0.1; // gradual adaptation (10% per step) + adapt_factor = std::max(0.5, std::min(3.0, adapt_factor)); // clamp to [0.5, 3.0] + adaptive_q_scale_ = adapt_factor; + + if (debug_mode_ && nis_history_.size() >= static_cast(nis_window_size_)) { + FYT_INFO("armor_solver", "NIS: avg={:.2f}, ratio={:.2f}, Q_scale={:.2f}", + avg_nis, nis_ratio, adaptive_q_scale_); + } + } + // Publish measurement measure_msg.x = tracker_->measurement(0); measure_msg.y = tracker_->measurement(1); diff --git a/src/rm_bringup/config/node_params/armor_solver_params.yaml b/src/rm_bringup/config/node_params/armor_solver_params.yaml index 589982b..bdb3881 100644 --- a/src/rm_bringup/config/node_params/armor_solver_params.yaml +++ b/src/rm_bringup/config/node_params/armor_solver_params.yaml @@ -16,6 +16,12 @@ sigma2_q_r: 4.0 # Radius noise (fixed) sigma2_q_d_zc: 8.0 # Height offset noise (fixed) + # NIS-based adaptive Q parameters + # NIS (Normalized Innovation Squared) follows chi-square with Z_N DOF + # If NIS > Z_N, increase Q; if NIS < Z_N, decrease Q + nis_window_size: 20 # Window size for averaging NIS + nis_adapt_range: 2.0 # Sensitivity for NIS adaptation + # R matrix parameters (measurement noise, scaled by distance) r_x: 2e-3 r_y: 2e-3 diff --git a/src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp b/src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp index cfbe443..5f2bb3f 100644 --- a/src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp +++ b/src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp @@ -99,19 +99,33 @@ public: ceres::Jet z_p_jet[N_Z]; h(x_p_jet, z_p_jet); - MatrixZ1 z_pri; + z_pri_ = MatrixZ1::Zero(); for (int i = 0; i < N_Z; i++) { - z_pri[i] = z_p_jet[i].a; + z_pri_[i] = z_p_jet[i].a; H.block(i, 0, 1, N_X) = z_p_jet[i].v.transpose(); } R = update_R(z); - K = P_pri * H.transpose() * (H * P_pri * H.transpose() + R).inverse(); - x_post = x_post + K * (z - z_pri); + S_ = H * P_pri * H.transpose() + R; + K = P_pri * H.transpose() * S_.inverse(); + innovation_ = z - z_pri_; + x_post = x_post + K * innovation_; P_post = (MatrixXX::Identity() - K * H) * P_pri; return x_post; } + // Get innovation (z - z_pri) + const MatrixZ1 & getInnovation() const { return innovation_; } + + // Get innovation covariance S = H * P_pri * H^T + R + const MatrixZZ & getInnovationCovariance() const { return S_; } + + // Get normalized innovation squared (NIS) + double getNIS() const { + if (S_.determinant() < 1e-10) return 1.0; + return innovation_.transpose() * S_.inverse() * innovation_; + } + private: // Process nonlinear vector function PredicFunc f; @@ -138,6 +152,12 @@ private: MatrixX1 x_pri; // Posteriori state MatrixX1 x_post; + + // Innovation (measurement residual) + MatrixZ1 z_pri_; + MatrixZ1 innovation_; + // Innovation covariance + MatrixZZ S_; }; } // namespace fyt