第 8 章 扩展卡尔曼滤波与目标跟踪¶

帧 t-1:  检测器 → (x₁, y₁, z₁, yaw₁)
帧 t:    检测器 → (x₂, y₂, z₂, yaw₂)
帧 t+1:  检测器 → (x₃, y₃, z₃, yaw₃)

// Tracker::predict() 中构建 F
Eigen::MatrixXd F = Eigen::MatrixXd::Identity(11, 11);
F(0, 1) = dt;   // xc += vxc * dt
F(2, 3) = dt;   // yc += vyc * dt
F(4, 5) = dt;   // zc += vzc * dt
F(6, 7) = dt;   // yaw += vyaw * dt

// cartesian → spherical (yaw, pitch, distance)
Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
{
  double x = xyz[0], y = xyz[1], z = xyz[2];
  double yaw = std::atan2(y, x);
  double pitch = std::atan2(z, std::sqrt(x * x + y * y));
  double distance = std::sqrt(x * x + y * y + z * z);
  return {yaw, pitch, distance};
}

Eigen::Vector3d Tracker::h_armor_xyz(const Eigen::VectorXd & x, int id) const
{
  int armor_num = static_cast<int>(tracked_armors_num);
  double angle = limit_rad(x[6] + id * 2 * M_PI / armor_num);

!!! info "`limit_rad()` 函数定义"
    这个函数将角度归一化到 `[-π, π]` 范围：
    ```cpp
    inline double limit_rad(double angle) {
        angle = fmod(angle + M_PI, 2 * M_PI);
        if (angle < 0) angle += 2 * M_PI;
        return angle - M_PI;
    }
    ```
    在 EKF 中，yaw 角会不断增长（可能超过 2π），但比较和显示时需要归一化到 [-π, π]。
  bool use_l_h = (armor_num == 4) && (id == 1 || id == 3);

  double r = use_l_h ? x[8] + x[9] : x[8];
  double armor_x = x[0] - r * std::cos(angle);
  double armor_y = x[2] - r * std::sin(angle);
  double armor_z = use_l_h ? x[4] + x[10] : x[4];

  return {armor_x, armor_y, armor_z};
}

Eigen::MatrixXd Tracker::h_jacobian(const Eigen::VectorXd & x, int id) const
{
  int armor_num = static_cast<int>(tracked_armors_num);
  double angle = limit_rad(x[6] + id * 2 * M_PI / armor_num);
  bool use_l_h = (armor_num == 4) && (id == 1 || id == 3);

  double r = use_l_h ? x[8] + x[9] : x[8];
  double dx_da = r * std::sin(angle);
  double dy_da = -r * std::cos(angle);
  double dx_dr = -std::cos(angle);
  double dy_dr = -std::sin(angle);
  double dx_dl = use_l_h ? -std::cos(angle) : 0.0;
  double dy_dl = use_l_h ? -std::sin(angle) : 0.0;
  double dz_dh = use_l_h ? 1.0 : 0.0;

  // Jacobian of armor_xyz w.r.t state -> (4x11)
  Eigen::MatrixXd H_xyza(4, 11);
  H_xyza << 1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl,     0,
            0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl,     0,
            0, 0, 0, 0, 1, 0,     0, 0,     0,     0, dz_dh,
            0, 0, 0, 0, 0, 0,     1, 0,     0,     0,     0;
  // ...

Eigen::MatrixXd xyz2ypd_jacobian(const Eigen::Vector3d & xyz)
{
  double x = xyz[0], y = xyz[1], z = xyz[2];
  double x2y2 = x * x + y * y;
  double sqrt_x2y2 = std::sqrt(x2y2);
  double x2y2z2 = x2y2 + z * z;
  double sqrt_all = std::sqrt(x2y2z2);

  double dyaw_dx = -y / x2y2;
  double dyaw_dy = x / x2y2;

  double dpitch_dx = -(x * z) / ((z * z / x2y2 + 1) * std::pow(x2y2, 1.5));
  double dpitch_dy = -(y * z) / ((z * z / x2y2 + 1) * std::pow(x2y2, 1.5));
  double dpitch_dz = 1.0 / ((z * z / x2y2 + 1) * sqrt_x2y2);

  double dd_dx = x / sqrt_all;
  double dd_dy = y / sqrt_all;
  double dd_dz = z / sqrt_all;

  Eigen::MatrixXd J(3, 3);
  J << dyaw_dx, dyaw_dy, 0,
       dpitch_dx, dpitch_dy, dpitch_dz,
       dd_dx, dd_dy, dd_dz;
  return J;
}

  Eigen::Vector3d armor_xyz = h_armor_xyz(x, id);
  Eigen::MatrixXd H_ypd = xyz2ypd_jacobian(armor_xyz);

  // Chain rule: d(ypd) / d(state) = d(ypd)/d(xyz) * d(xyz)/d(state)
  Eigen::MatrixXd H_full(4, 11);
  H_full << H_ypd(0, 0), H_ypd(0, 1), H_ypd(0, 2), 0,
            H_ypd(1, 0), H_ypd(1, 1), H_ypd(1, 2), 0,
            H_ypd(2, 0), H_ypd(2, 1), H_ypd(2, 2), 0,
                       0,            0,            0, 1;

  return H_full * H_xyza;
}

void Tracker::predict(double dt)
{
  // State transition matrix F (11x11 constant velocity model)
  Eigen::MatrixXd F = Eigen::MatrixXd::Identity(11, 11);
  F(0, 1) = dt;   // xc += vxc * dt
  F(2, 3) = dt;   // yc += vyc * dt
  F(4, 5) = dt;   // zc += vzc * dt
  F(6, 7) = dt;   // yaw += vyaw * dt

  // Process noise Q (Piecewise White Noise Model)
  double v1, v2;
  if (tracked_id == "outpost") {
    v1 = 10;   // outpost accel variance
    v2 = 0.1;  // outpost angular accel variance
  } else {
    v1 = 100;  // accel variance
    v2 = 400;  // angular accel variance
  }
  double a = dt * dt * dt * dt / 4;
  double b = dt * dt * dt / 2;
  double c = dt * dt;

  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(11, 11);
  Q(0, 0) = a * v1; Q(0, 1) = b * v1;
  Q(1, 0) = b * v1; Q(1, 1) = c * v1;
  Q(2, 2) = a * v1; Q(2, 3) = b * v1;
  Q(3, 2) = b * v1; Q(3, 3) = c * v1;
  Q(4, 4) = a * v1; Q(4, 5) = b * v1;
  Q(5, 4) = b * v1; Q(5, 5) = c * v1;
  Q(6, 6) = a * v2; Q(6, 7) = b * v2;
  Q(7, 6) = b * v2; Q(7, 7) = c * v2;
  // r1, r2_diff, z_diff have zero process noise (static parameters)
  // ...

  // Outpost speed clamping
  static constexpr double outpost_max_v_yaw = 2.51;  // ~144 deg/s

  if (is_converged() && tracked_id == "outpost" && std::abs(ekf.x[7]) > 2)
    ekf.x[7] = ekf.x[7] > 0 ? outpost_max_v_yaw : -outpost_max_v_yaw;

void Tracker::update_ypda(const Armor & armor, int id)
{
  // Measurement Jacobian: d(ypd, angle) / d(state)
  Eigen::MatrixXd H = h_jacobian(ekf.x, id);

  // Adaptive measurement noise R
  double delta_angle = limit_rad(orientationToYaw(armor.pose.orientation) -
                                 std::atan2(ekf.x[2], ekf.x[0]));
  Eigen::Vector3d armor_ypd = xyz2ypd(Eigen::Vector3d(
    armor.pose.position.x, armor.pose.position.y, armor.pose.position.z));
  Eigen::Vector4d R_dig;
  R_dig << 4e-3, 4e-3,
           std::log(std::abs(delta_angle) + 1) + 1,
           std::log(std::abs(armor_ypd[2]) + 1) / 200 + 9e-2;
  Eigen::Matrix4d R = R_dig.asDiagonal();
  // ...

  // Nonlinear observation function: state -> [ypd_yaw, ypd_pitch, ypd_dist, angle]
  auto h = [&](const Eigen::VectorXd & x) -> Eigen::Vector4d {
    Eigen::Vector3d xyz = h_armor_xyz(x, id);
    Eigen::Vector3d ypd = xyz2ypd(xyz);
    double angle = limit_rad(x[6] + id * 2 * M_PI / static_cast<int>(tracked_armors_num));
    return {ypd[0], ypd[1], ypd[2], angle};
  };

  // Angular-aware subtraction
  auto z_subtract = [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) -> Eigen::VectorXd {
    Eigen::VectorXd c = a - b;
    c[0] = limit_rad(c[0]);  // yaw
    c[1] = limit_rad(c[1]);  // pitch
    c[3] = limit_rad(c[3]);  // angle
    return c;
  };

class ExtendedKalmanFilter
{
public:
  Eigen::VectorXd x;
  Eigen::MatrixXd P;

  ExtendedKalmanFilter() = default;

  ExtendedKalmanFilter(
    const Eigen::VectorXd & x0, const Eigen::MatrixXd & P0,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> x_add =
      [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) { return a + b; });

  Eigen::VectorXd predict(const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q);

  Eigen::VectorXd predict(
    const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f);

  Eigen::VectorXd update(
    const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> h,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract);

private:
  Eigen::MatrixXd I;
  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> x_add;
};

Eigen::VectorXd ExtendedKalmanFilter::predict(
  const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q,
  std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f)
{
  P = F * P * F.transpose() + Q;
  x = f(x);
  return x;
}

ekf.predict(F, Q, [&](const Eigen::VectorXd & x) -> Eigen::VectorXd {
  Eigen::VectorXd x_prior = F * x;
  x_prior[6] = limit_rad(x_prior[6]);
  return x_prior;
});

Eigen::VectorXd ExtendedKalmanFilter::update(
  const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
  std::function<Eigen::VectorXd(const Eigen::VectorXd &)> h,
  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract)
{
  Eigen::VectorXd x_prior = x;
  Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

  // Joseph form for numerical stability
  P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();

  x = x_add(x, K * z_subtract(z, h(x)));

  // Chi-squared NIS/NEES tests
  Eigen::VectorXd residual = z_subtract(z, h(x));
  Eigen::MatrixXd S = H * P * H.transpose() + R;
  double nis = residual.transpose() * S.inverse() * residual;
  double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior);

  // Chi-squared threshold (df=4, 95% confidence)
  constexpr double nis_threshold = 0.711;
  constexpr double nees_threshold = 0.711;
  // ...

void Tracker::initEKF(const Armor & a)
{
  double xa = a.pose.position.x;
  double ya = a.pose.position.y;
  double za = a.pose.position.z;
  last_yaw_ = 0;
  double yaw = orientationToYaw(a.pose.orientation);

  double r = 0.26;
  dz = 0;
  another_r = r;

  if (a.type == "large" && (tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) {
    r = 0.2;
  } else if (tracked_id == "outpost") {
    r = 0.2765;
  }

  // 11D state: [xc, vxc, yc, vyc, zc, vzc, yaw, vyaw, r1, r2_diff, z_diff]
  double xc = xa + r * cos(yaw);
  double yc = ya + r * sin(yaw);
  Eigen::VectorXd x0(11);
  x0 << xc, 0, yc, 0, za, 0, yaw, 0, r, 0, 0;

  // Initial covariance diagonal
  Eigen::VectorXd P0_dig(11);
  P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1;

  if (tracked_id == "outpost") {
    P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0;
  }

  Eigen::MatrixXd P0 = P0_dig.asDiagonal();

  // Angle-aware state addition
  auto x_add = [](const Eigen::VectorXd & a_vec, const Eigen::VectorXd & b) -> Eigen::VectorXd {
    Eigen::VectorXd c = a_vec + b;
    c[6] = limit_rad(c[6]);
    return c;
  };

  ekf = ExtendedKalmanFilter(x0, P0, x_add);
  target_state = ekf.x;
}

void Tracker::init(const Armors::SharedPtr & armors_msg)
{
  if (armors_msg->armors.empty()) {
    RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "No armor detected!");
    return;
  }

  // Choose armor closest to image center
  double min_distance = DBL_MAX;
  tracked_armor = armors_msg->armors[0];
  for (const auto & armor : armors_msg->armors) {
    if (armor.distance_to_image_center < min_distance) {
      min_distance = armor.distance_to_image_center;
      tracked_armor = armor;
    }
  }

  initEKF(tracked_armor);
  tracked_id = tracked_armor.number;
  tracker_state = DETECTING;
  detect_count_ = 1;
  update_count = 0;
  jumped = false;
  last_id_ = 0;
  is_converged_ = false;
  switch_count_ = 0;

  updateArmorsNum(tracked_armor);
}

void Tracker::update(const Armors::SharedPtr & armors_msg, double dt)
{
  // ---- EKF predict ----
  predict(dt);

  bool matched = false;
  target_state = ekf.x;

  if (!armors_msg->armors.empty()) {
    // Find the closest armor with matching number
    Armor same_id_armor;
    int same_id_armors_count = 0;
    auto predicted_position = getArmorPositionFromState(ekf.x);
    double min_position_diff = DBL_MAX;
    double yaw_diff = DBL_MAX;

    for (const auto & armor : armors_msg->armors) {
      if (armor.number == tracked_id) {
        same_id_armor = armor;
        same_id_armors_count++;
        auto p = armor.pose.position;
        Eigen::Vector3d position_vec(p.x, p.y, p.z);
        double position_diff = (predicted_position - position_vec).norm();
        if (position_diff < min_position_diff) {
          min_position_diff = position_diff;
          yaw_diff = std::abs(orientationToYaw(armor.pose.orientation) - ekf.x(6));
          tracked_armor = armor;
        }
      }
    }

    info_position_diff = min_position_diff;
    info_yaw_diff = yaw_diff;

    if (min_position_diff < max_match_distance_ && yaw_diff < max_match_yaw_diff_) {
      matched = true;
      update_count++;
      // ... armor plate ID matching and EKF update (see section 8.10.1)
    } else if (same_id_armors_count == 1 && yaw_diff > max_match_yaw_diff_) {
      handleArmorJump(same_id_armor);
    } else {
      RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "No matched armor found!");
    }
  }
  // ...

      // Match armor to expected plate id
      int id = 0;
      double min_angle_error = 1e10;
      const auto xyza_list = armor_xyza_list();
      int armor_num = static_cast<int>(tracked_armors_num);

      std::vector<std::pair<Eigen::Vector4d, int>> xyza_i_list;
      for (int i = 0; i < armor_num; i++) {
        xyza_i_list.push_back({xyza_list[i], i});
      }
      std::sort(xyza_i_list.begin(), xyza_i_list.end(),
        [](const auto & a, const auto & b) {
          Eigen::Vector3d ypd1 = xyz2ypd(a.first.head(3));
          Eigen::Vector3d ypd2 = xyz2ypd(b.first.head(3));
          return ypd1[2] < ypd2[2];
        });

      double measured_yaw = orientationToYaw(tracked_armor.pose.orientation);
      for (int i = 0; i < std::min(3, armor_num); i++) {
        const auto & xyza = xyza_i_list[i].first;
        Eigen::Vector3d ypd = xyz2ypd(xyza.head(3));
        double angle_error = std::abs(limit_rad(measured_yaw - xyza[3])) +
                             std::abs(limit_rad(
                               std::atan2(tracked_armor.pose.position.y,
                                          tracked_armor.pose.position.x) -
                               ypd[0]));
        if (std::abs(angle_error) < std::abs(min_angle_error)) {
          id = xyza_i_list[i].second;
          min_angle_error = angle_error;
        }
      }

      if (id != 0) jumped = true;

  // Prevent radius from spreading (physical constraints)
  if (ekf.x(8) < 0.12) {
    ekf.x(8) = 0.12;
  } else if (ekf.x(8) > 0.4) {
    ekf.x(8) = 0.4;
  }
  target_state = ekf.x;

  // Tracking state machine
  if (tracker_state == DETECTING) {
    if (matched) {
      detect_count_++;
      if (detect_count_ > tracking_thres) {
        detect_count_ = 0;
        tracker_state = TRACKING;
        RCLCPP_INFO(rclcpp::get_logger("armor_tracker"), "Target tracking confirmed!");
      }
    } else {
      detect_count_ = 0;
      tracker_state = LOST;
    }
  } else if (tracker_state == TRACKING) {
    if (!matched) {
      tracker_state = TEMP_LOST;
      lost_count_ = 1;
    }
  } else if (tracker_state == TEMP_LOST) {
    if (!matched) {
      lost_count_++;
      if (lost_count_ > lost_thres) {
        lost_count_ = 0;
        tracker_state = LOST;
        RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "Target lost!");
      }
    } else {
      tracker_state = TRACKING;
      lost_count_ = 0;
    }
  }
}

dt_ = (time - last_time_).seconds();
tracker_->lost_thres = static_cast<int>(lost_time_thres_ / dt_);

void Tracker::handleArmorJump(const Armor & current_armor)
{
  double yaw = orientationToYaw(current_armor.pose.orientation);
  ekf.x(6) = yaw;
  updateArmorsNum(current_armor);

  if (tracked_armors_num == ArmorsNum::NORMAL_4) {
    dz = ekf.x(4) - current_armor.pose.position.z;
    ekf.x(4) = current_armor.pose.position.z;
    std::swap(ekf.x(8), another_r);
  }
  RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "Armor jump!");

  // If position diff is large, reset diverged state
  auto p = current_armor.pose.position;
  Eigen::Vector3d current_p(p.x, p.y, p.z);
  Eigen::Vector3d infer_p = getArmorPositionFromState(ekf.x);
  if ((current_p - infer_p).norm() > max_match_distance_) {
    double r_val = ekf.x(8);
    ekf.x(0) = p.x + r_val * cos(yaw);
    ekf.x(1) = 0;
    ekf.x(2) = p.y + r_val * sin(yaw);
    ekf.x(3) = 0;
    ekf.x(4) = p.z;
    ekf.x(5) = 0;
    ekf.x(9) = 0;
    ekf.x(10) = 0;
    RCLCPP_WARN(rclcpp::get_logger("armor_tracker"), "Reset diverged state!");
  }

  target_state = ekf.x;
}

double Tracker::orientationToYaw(const geometry_msgs::msg::Quaternion & q)
{
  tf2::Quaternion tf_q;
  tf2::fromMsg(q, tf_q);
  double roll, pitch, yaw;
  tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw);
  yaw = last_yaw_ + angles::shortest_angular_distance(last_yaw_, yaw);
  last_yaw_ = yaw;
  return yaw;
}

  // Prevent radius from spreading (physical constraints)
  if (ekf.x(8) < 0.12) {
    ekf.x(8) = 0.12;
  } else if (ekf.x(8) > 0.4) {
    ekf.x(8) = 0.4;
  }

bool Tracker::is_diverged() const
{
  double r = ekf.x[8];
  bool r_ok = r > 0.05 && r < 0.5;

  if (static_cast<int>(tracked_armors_num) == 4) {
    double l = ekf.x[8] + ekf.x[9];
    bool l_ok = l > 0.05 && l < 0.5;
    return !(r_ok && l_ok);
  }

  return !r_ok;
}

  constexpr double nis_threshold = 0.711;
  constexpr double nees_threshold = 0.711;

  if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1;
  if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1;
  total_count_++;
  last_nis = nis;

  recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0);
  if (recent_nis_failures.size() > window_size) {
    recent_nis_failures.pop_front();
  }

  int recent_failures = std::accumulate(
    recent_nis_failures.begin(), recent_nis_failures.end(), 0);
  double recent_rate = static_cast<double>(recent_failures) / recent_nis_failures.size();