auto ekf

src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp

@@ -33,6 +33,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 // std
 #include <atomic>
+#include <deque>
 #include <memory>
 #include <string>
 #include <vector>
@@ -117,6 +118,12 @@ private:
   std::atomic<int> ekf_detect_count_{0};
   std::atomic<int> ekf_count_threshold_{30};
 
+  // Adaptive Q based on NIS
+  std::deque<double> 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_;
   rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;

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,

src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp

@@ -70,6 +70,8 @@ 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<double, X_N, X_N> q;
@@ -89,7 +91,15 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
     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;
@@ -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<size_t>(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<double>(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<size_t>(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);

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

src/rm_utils/include/rm_utils/math/extended_kalman_filter.hpp

@@ -99,19 +99,33 @@ public:
     ceres::Jet<double, N_X> 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();
+    S_ = H * P_pri * H.transpose() + R;
-    x_post = x_post + K * (z - z_pri);
+    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