精选Autoware 定位之EKF 滤波定位（四）

敢敢のwings

个人专栏：Autoware

发布时间 2024.06.11阅读数 2901 评论数 0

0. 简介

对于AutoWare，我们已经讲了很多了，从这一篇我们开始进入实战，按照《Autoware 技术代码解读（三）》梳理的顺序，我们一个个慢慢来看与学习，首先第一个就是基于ekf的滤波定位。扩展卡尔曼滤波定位器通过将二维车辆动力学模型与输入的自我姿态和自我扭矩信息进行整合，估计出鲁棒且噪音较小的机器人姿态和扭矩。

1. 主要功能

输入消息的时间延迟补偿，可以有效整合具有不同时间延迟的输入信息。这对于高速移动的机器人，比如自动驾驶车辆，尤为重要（见下图）。
自动估计偏航误差，可以防止由于传感器安装角度误差导致的建模错误，从而提高估计精度。
马氏距离门，可以进行概率异常值检测，确定哪些输入应该被使用或忽略。
平滑更新，卡尔曼滤波器的测量更新通常在获得测量值时执行，但这可能导致估计值的大幅变化，尤其是对于低频测量。由于算法可以考虑测量时间，测量数据可以被分成多个部分，并在保持一致性的同时平滑集成（见下图）。
从俯仰角计算垂直校正量，可以减轻坡度上的定位不稳定性。例如，上坡时，由于EKF只考虑3DoF（x，y，偏航），它的行为就像被埋在地下一样（见“从俯仰角计算增量”图的左侧）。因此，EKF根据公式校正z坐标（见“从俯仰角计算增量”图的右侧）。

2. 代码阅读

2.1 covariance.cpp

/// @brief 将 EKF 的状态协方差矩阵转换为姿态消息的协方差数组。通过从输入的状态协方差矩阵 P 中提取特定索引位置的值，并将这些值存储到输出的 covariance 数组中，以构建姿态消息的协方差矩阵
/// @param P EKF 的状态协方差矩阵。
/// @return 
std::array<double, 36> ekfCovarianceToPoseMessageCovariance(const Matrix6d & P)
{
  std::array<double, 36> covariance;
  covariance.fill(0.);

  covariance[COV_IDX::X_X] = P(IDX::X, IDX::X);
  covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y);
  covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW);
  covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X);
  covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);
  covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW);
  covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X);
  covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y);
  covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);

  return covariance;
}
/// @brief 将 EKF 的状态协方差矩阵转换为扭转消息的协方差数组
/// @param P EKF 的状态协方差矩阵。
/// @return 
std::array<double, 36> ekfCovarianceToTwistMessageCovariance(const Matrix6d & P)
{
  std::array<double, 36> covariance;
  covariance.fill(0.);

  covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX);
  covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);
  covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);
  covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);

  return covariance;
}

2.2 diagnostics.cpp

/// @brief 检查进程是否已激活，并返回相应的诊断状态.根据传入的is_activated参数设置stat.values、stat.level和stat.message
/// @param is_activated 进程是否已激活
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = "is_activated";
  key_value.value = is_activated ? "True" : "False";
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_activated) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]process is not activated";
  }

  return stat;
}
/// @brief 检查测量数据是否更新，并返回相应的诊断状态。根据传入的no_update_count参数设置stat.values、stat.level和stat.message
/// @param measurement_type 测量类型
/// @param no_update_count 没有更新的计数
/// @param no_update_count_threshold_warn 警告阈值
/// @param no_update_count_threshold_error 错误阈值
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(
  const std::string & measurement_type, const size_t no_update_count,
  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;
    //将各种数据转换为字符串形式，存入 stat.values 中。
  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_no_update_count";
  key_value.value = std::to_string(no_update_count);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_no_update_count_threshold_warn";
  key_value.value = std::to_string(no_update_count_threshold_warn);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_no_update_count_threshold_error";
  key_value.value = std::to_string(no_update_count_threshold_error);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  //根据 no_update_count 是否超过阈值设置 stat.level 和 stat.message。
  if (no_update_count >= no_update_count_threshold_warn) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]" + measurement_type + " is not updated";
  }
  if (no_update_count >= no_update_count_threshold_error) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
    stat.message = "[ERROR]" + measurement_type + " is not updated";
  }

  return stat;
}
/// @brief 检查测量队列的大小，并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param queue_size 队列大小
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize(
  const std::string & measurement_type, const size_t queue_size)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_queue_size";
  key_value.value = std::to_string(queue_size);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";

  return stat;
}
/// @brief 检查测量是否通过延迟门，并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param is_passed_delay_gate 表示延迟门是否通过
/// @param delay_time 延迟时间
/// @param delay_time_threshold 延迟时间阈值 
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(
  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
  const double delay_time_threshold)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_is_passed_delay_gate";
  key_value.value = is_passed_delay_gate ? "True" : "False";
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_delay_time";
  key_value.value = std::to_string(delay_time);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_delay_time_threshold";
  key_value.value = std::to_string(delay_time_threshold);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_passed_delay_gate) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]" + measurement_type + " topic is delay";
  }

  return stat;
}
/// @brief 检查测量是否通过马氏距离门，并返回相应的诊断状态
/// @param measurement_type 测量类型
/// @param is_passed_mahalanobis_gate 表示马氏距离门是否通过 
/// @param mahalanobis_distance 马氏距离
/// @param mahalanobis_distance_threshold  马氏距离阈值 
/// @return 
diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate(
  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
  const double mahalanobis_distance, const double mahalanobis_distance_threshold)
{
  diagnostic_msgs::msg::DiagnosticStatus stat;

  diagnostic_msgs::msg::KeyValue key_value;
  key_value.key = measurement_type + "_is_passed_mahalanobis_gate";
  key_value.value = is_passed_mahalanobis_gate ? "True" : "False";
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_mahalanobis_distance";
  key_value.value = std::to_string(mahalanobis_distance);
  stat.values.push_back(key_value);
  key_value.key = measurement_type + "_mahalanobis_distance_threshold";
  key_value.value = std::to_string(mahalanobis_distance_threshold);
  stat.values.push_back(key_value);

  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  stat.message = "OK";
  if (!is_passed_mahalanobis_gate) {
    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
    stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large";
  }

  return stat;
}

// The highest level within the stat_array will be reflected in the merged_stat.
// When all stat_array entries are 'OK,' the message of merged_stat will be "OK"

/// @brief 合并诊断状态数组，并返回合并后的诊断状态
/// @param stat_array 根据每个诊断状态的级别和消息进行合并
/// @return 
diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(
  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array)
{
  diagnostic_msgs::msg::DiagnosticStatus merged_stat;

  for (const auto & stat : stat_array) {
    if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
      if (!merged_stat.message.empty()) {
        merged_stat.message += "; ";
      }
      merged_stat.message += stat.message;
    }
    if (stat.level > merged_stat.level) {
      merged_stat.level = stat.level;
    }
    for (const auto & value : stat.values) {
      merged_stat.values.push_back(value);
    }
  }

  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
    merged_stat.message = "OK";
  }

  return merged_stat;
}

2.3 ekf_localizer.cpp

// clang-format off
#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl //打印矩阵
#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} //打印调试信息
// clang-format on

using std::placeholders::_1;  // 占位符

/// @brief 构造函数
/// @param node_name 节点名称
/// @param node_options 节点选项
EKFLocalizer::EKFLocalizer(const std::string& node_name, const rclcpp::NodeOptions& node_options)
    : rclcpp::Node(node_name, node_options),
      warning_(std::make_shared<Warning>(
          this)),  // 警告管理器(warning_)、参数配置对象(params_)、EKF的更新频率(ekf_rate_)、时间步长(ekf_dt_)以及姿态和速度队列
      params_(this),
      ekf_rate_(params_.ekf_rate),
      ekf_dt_(params_.ekf_dt),
      pose_queue_(params_.pose_smoothing_steps),
      twist_queue_(params_.twist_smoothing_steps) {
    /* convert to continuous to discrete */
    proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
    proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
    proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);

    is_activated_ = false;

    // ROS系统的初始化
    /* initialize ros system */
    timer_control_ = rclcpp::create_timer(this,
                                          get_clock(),
                                          rclcpp::Duration::from_seconds(ekf_dt_),
                                          std::bind(&EKFLocalizer::timerCallback, this));

    timer_tf_ = rclcpp::create_timer(this,
                                     get_clock(),
                                     rclcpp::Rate(params_.tf_rate_).period(),
                                     std::bind(&EKFLocalizer::timerTFCallback, this));

    pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
    pub_pose_cov_ =
        create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("ekf_pose_with_covariance",
                                                                        1);
    pub_odom_ = create_publisher<nav_msgs::msg::Odometry>("ekf_odom", 1);
    pub_twist_ = create_publisher<geometry_msgs::msg::TwistStamped>("ekf_twist", 1);
    pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
        "ekf_twist_with_covariance",
        1);
    pub_yaw_bias_ =
        create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
    pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
    pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "ekf_biased_pose_with_covariance",
        1);
    pub_diag_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
    sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "initialpose",
        1,
        std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
    sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
        "in_pose_with_covariance",
        1,
        std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1));
    sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
        "in_twist_with_covariance",
        1,
        std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1));
    service_trigger_node_ =
        create_service<std_srvs::srv::SetBool>("trigger_node_srv",
                                               std::bind(&EKFLocalizer::serviceTriggerNode,
                                                         this,
                                                         std::placeholders::_1,
                                                         std::placeholders::_2),
                                               rclcpp::ServicesQoS().get_rmw_qos_profile());

    tf_br_ = std::make_shared<tf2_ros::TransformBroadcaster>(
        std::shared_ptr<rclcpp::Node>(this, [](auto) {}));

    ekf_module_ = std::make_unique<EKFModule>(warning_, params_);
    logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);

    z_filter_.set_proc_dev(1.0);
    roll_filter_.set_proc_dev(0.01);
    pitch_filter_.set_proc_dev(0.01);
}

/// @brief 更新预测频率,根据上次预测时间来更新EKF的预测频率
void EKFLocalizer::updatePredictFrequency() {
    if (last_predict_time_) {
        // 如果检测到时间向后跳跃，则发出警告
        if (get_clock()->now() < *last_predict_time_) {
            warning_->warn("Detected jump back in time");
        } else {
            // 根据时间间隔计算新的EKF频率和时间步长，并更新离散过程噪声方差。
            ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds();
            DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_);
            ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);

            /* Update discrete proc_cov*/
            proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
            proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);
            proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);
        }
    }
    last_predict_time_ = std::make_shared<const rclcpp::Time>(get_clock()->now());
}

/// @brief
/// 执行定时回调操作，用于更新预测频率、执行EKF模型的预测和姿态与速度的测量更新，并发布EKF结果。
void EKFLocalizer::timerCallback() {
    // 检查节点是否已激活，若未激活则发出警告并返回
    if (!is_activated_) {
        warning_->warnThrottle(
            "The node is not activated. Provide initial pose to pose_initializer",
            2000);
        publishDiagnostics();
        return;
    }

    DEBUG_INFO(get_logger(), "========================= timer called =========================");

    /* update predict frequency with measured timer rate */
    // 更新预测频率
    updatePredictFrequency();

    /* predict model in EKF */
    // 执行EKF模型的预测
    stop_watch_.tic();
    DEBUG_INFO(get_logger(),
               "------------------------- start prediction -------------------------");
    ekf_module_->predictWithDelay(ekf_dt_);
    DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc());
    DEBUG_INFO(get_logger(),
               "------------------------- end prediction -------------------------\n");

    /* pose measurement update */
    pose_diag_info_.queue_size = pose_queue_.size();
    pose_diag_info_.is_passed_delay_gate = true;
    pose_diag_info_.delay_time = 0.0;
    pose_diag_info_.delay_time_threshold = 0.0;
    pose_diag_info_.is_passed_mahalanobis_gate = true;
    pose_diag_info_.mahalanobis_distance = 0.0;

    bool pose_is_updated = false;

    if (!pose_queue_.empty()) {
        DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------");
        stop_watch_.tic();

        // save the initial size because the queue size can change in the loop
        const auto t_curr = this->now();
        const size_t n = pose_queue_.size();
        for (size_t i = 0; i < n; ++i) {
            const auto pose = pose_queue_.pop_increment_age();
            // 对姿态和速度进行测量更新,并返回是否更新成功的标志
            bool is_updated =
                ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_);
            if (is_updated) {
                pose_is_updated = true;

                // Update Simple 1D filter with considering change of z value due to measurement
                // pose
                const double delay_time = (t_curr - pose->header.stamp).seconds() +
                                          params_.pose_additional_delay;  // 计算延迟时间
                // 根据测量姿态的时间戳和当前时间戳计算延迟时间，然后使用延迟时间对姿态进行补偿
                const auto pose_with_z_delay =
                    ekf_module_->compensatePoseWithZDelay(*pose, delay_time);
                updateSimple1DFilters(pose_with_z_delay,
                                      params_.pose_smoothing_steps);  // 更新滤波器
            }
        }
        DEBUG_INFO(get_logger(),
                   "[EKF] measurementUpdatePose calc time = %f [ms]",
                   stop_watch_.toc());
        DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");
    }
    pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1);

    /* twist measurement update */
    twist_diag_info_.queue_size = twist_queue_.size();
    twist_diag_info_.is_passed_delay_gate = true;
    twist_diag_info_.delay_time = 0.0;
    twist_diag_info_.delay_time_threshold = 0.0;
    twist_diag_info_.is_passed_mahalanobis_gate = true;
    twist_diag_info_.mahalanobis_distance = 0.0;

    bool twist_is_updated = false;

    if (!twist_queue_.empty()) {
        DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------");
        stop_watch_.tic();

        // 保存初始大小，因为循环中队列大小可能会改变
        const auto t_curr = this->now();
        const size_t n = twist_queue_.size();
        for (size_t i = 0; i < n; ++i) {
            const auto twist = twist_queue_.pop_increment_age();
            bool is_updated = ekf_module_->measurementUpdateTwist(
                *twist,
                ekf_dt_,
                t_curr,
                twist_diag_info_);  // 对姿态和速度进行测量更新,并返回是否更新成功的标志
            if (is_updated) {
                twist_is_updated = true;
            }
        }
        DEBUG_INFO(get_logger(),
                   "[EKF] measurementUpdateTwist calc time = %f [ms]",
                   stop_watch_.toc());
        DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n");
    }
    twist_diag_info_.no_update_count =
        twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1);  // 更新测量更新次数

    const double z = z_filter_.get_x();
    const double roll = roll_filter_.get_x();
    const double pitch = pitch_filter_.get_x();
    // 获取当前的姿态和速度信息，并发布EKF结果
    const geometry_msgs::msg::PoseStamped current_ekf_pose =
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false);
    const geometry_msgs::msg::PoseStamped current_biased_ekf_pose =
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true);
    const geometry_msgs::msg::TwistStamped current_ekf_twist =
        ekf_module_->getCurrentTwist(this->now());

    /* publish ekf result */
    publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);
    publishDiagnostics();
}

/// @brief 执行TF变换的定时回调操作，用于获取当前姿态信息并发送TF变换
void EKFLocalizer::timerTFCallback() {
    // 检查节点是否已激活或者姿态框架ID是否为空，若是则直接返回
    if (!is_activated_) {
        return;
    }

    if (params_.pose_frame_id == "") {
        return;
    }

    const double z = z_filter_.get_x();
    const double roll = roll_filter_.get_x();
    const double pitch = pitch_filter_.get_x();
    // 获取当前姿态信息，并将其转换为TF消息并发送
    geometry_msgs::msg::TransformStamped transform_stamped;
    transform_stamped = tier4_autoware_utils::pose2transform(
        ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false),
        "base_link");
    transform_stamped.header.stamp = this->now();
    tf_br_->sendTransform(transform_stamped);
}

/// @brief 从TF缓存中获取两个坐标系之间的变换关系。
/// @param parent_frame 父坐标系名称
/// @param child_frame  子坐标系名称
/// @param transform   变换关系
/// @return 
bool EKFLocalizer::getTransformFromTF(std::string parent_frame,
                                      std::string child_frame,
                                      geometry_msgs::msg::TransformStamped& transform) {
    tf2::BufferCore tf_buffer;
    tf2_ros::TransformListener tf_listener(tf_buffer);//创建TF缓存对象和监听器
    rclcpp::sleep_for(std::chrono::milliseconds(100));

    parent_frame = eraseLeadingSlash(parent_frame);
    child_frame = eraseLeadingSlash(child_frame);

    for (int i = 0; i < 50; ++i) {
        try {
            transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero);//循环尝试50次获取父坐标系到子坐标系的变换关系，如果成功则返回true
            return true;
        } catch (tf2::TransformException& ex) {
            // 否则打印警告信息，等待100ms后再次尝试，共循环50次
            warning_->warn(ex.what());
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    return false;
}

/// @brief 处理初始姿态的回调函数，用于初始化EKF模型
/// @param initialpose 初始姿态
void EKFLocalizer::callbackInitialPose(
    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) {
    geometry_msgs::msg::TransformStamped transform;
    // 调用getTransformFromTF()函数获取父坐标系到初始姿态坐标系的变换关系
    if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) {
        RCLCPP_ERROR(get_logger(),
                     "[EKF] TF transform failed. parent = %s, child = %s",
                     params_.pose_frame_id.c_str(),
                     initialpose->header.frame_id.c_str());
    }
    //获取的变换关系和初始姿态信息初始化EKF模型
    ekf_module_->initialize(*initialpose, transform);
    initSimple1DFilters(*initialpose);
}

/// @brief 当接收到包含姿态与协方差信息的消息时，将其存入姿态队列中
/// @param msg 包含姿态与协方差信息的指针
void EKFLocalizer::callbackPoseWithCovariance(
    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
    if (!is_activated_) {
        return;
    }
    //将收到的消息存入姿态队列中,并等待timer回调函数处理
    pose_queue_.push(msg);
}

/// @brief 当接收到包含速度与协方差信息的消息时，根据速度大小判断是否忽略该消息，并在需要时修改协方差信息。
/// @param msg 包含速度与协方差信息的指针
void EKFLocalizer::callbackTwistWithCovariance(
    geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
    // 忽略速度太小的消息。注意，这个不等式不能包含“等于”。
    if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) {
        // 根据速度大小判断是否忽略消息，并在需要时修改协方差信息
        msg->twist.covariance[0 * 6 + 0] = 10000.0;
    }
    twist_queue_.push(msg);
}

/// @brief 发布定位结果，包括姿态、带偏置的姿态、速度以及相应的协方差信息等
/// @param current_ekf_pose 当前估计的姿态
/// @param current_biased_ekf_pose 当前估计的带偏置的姿态
/// @param current_ekf_twist 当前估计的速度
void EKFLocalizer::publishEstimateResult(
    const geometry_msgs::msg::PoseStamped& current_ekf_pose,
    const geometry_msgs::msg::PoseStamped& current_biased_ekf_pose,
    const geometry_msgs::msg::TwistStamped& current_ekf_twist) {
    rclcpp::Time current_time = this->now();

    /* publish latest pose */
    pub_pose_->publish(current_ekf_pose);
    pub_biased_pose_->publish(current_biased_ekf_pose);

    /* publish latest pose with covariance */
    geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;
    pose_cov.header.stamp = current_time;
    pose_cov.header.frame_id = current_ekf_pose.header.frame_id;
    pose_cov.pose.pose = current_ekf_pose.pose;
    pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance();
    pub_pose_cov_->publish(pose_cov);

    geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;
    biased_pose_cov.pose.pose = current_biased_ekf_pose.pose;
    pub_biased_pose_cov_->publish(biased_pose_cov);

    /* publish latest twist */
    pub_twist_->publish(current_ekf_twist);

    /* publish latest twist with covariance */
    geometry_msgs::msg::TwistWithCovarianceStamped twist_cov;
    twist_cov.header.stamp = current_time;
    twist_cov.header.frame_id = current_ekf_twist.header.frame_id;
    twist_cov.twist.twist = current_ekf_twist.twist;
    twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance();
    pub_twist_cov_->publish(twist_cov);

    /* publish yaw bias */
    tier4_debug_msgs::msg::Float64Stamped yawb;
    yawb.stamp = current_time;
    yawb.data = ekf_module_->getYawBias();
    pub_yaw_bias_->publish(yawb);

    /* publish latest odometry */
    nav_msgs::msg::Odometry odometry;
    odometry.header.stamp = current_time;
    odometry.header.frame_id = current_ekf_pose.header.frame_id;
    odometry.child_frame_id = "base_link";
    odometry.pose = pose_cov.pose;
    odometry.twist = twist_cov.twist;
    pub_odom_->publish(odometry);
}
/// @brief 发布诊断信息，包括激活状态检查、测量更新情况、队列大小、延迟情况、Mahalanobis门限等
void EKFLocalizer::publishDiagnostics() {
    std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;

    diag_status_array.push_back(checkProcessActivated(is_activated_));

    if (is_activated_) {
        diag_status_array.push_back(
            checkMeasurementUpdated("pose",
                                    pose_diag_info_.no_update_count,
                                    params_.pose_no_update_count_threshold_warn,
                                    params_.pose_no_update_count_threshold_error));
        diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size));
        diag_status_array.push_back(
            checkMeasurementDelayGate("pose",
                                      pose_diag_info_.is_passed_delay_gate,
                                      pose_diag_info_.delay_time,
                                      pose_diag_info_.delay_time_threshold));
        diag_status_array.push_back(
            checkMeasurementMahalanobisGate("pose",
                                            pose_diag_info_.is_passed_mahalanobis_gate,
                                            pose_diag_info_.mahalanobis_distance,
                                            params_.pose_gate_dist));

        diag_status_array.push_back(
            checkMeasurementUpdated("twist",
                                    twist_diag_info_.no_update_count,
                                    params_.twist_no_update_count_threshold_warn,
                                    params_.twist_no_update_count_threshold_error));
        diag_status_array.push_back(
            checkMeasurementQueueSize("twist", twist_diag_info_.queue_size));
        diag_status_array.push_back(
            checkMeasurementDelayGate("twist",
                                      twist_diag_info_.is_passed_delay_gate,
                                      twist_diag_info_.delay_time,
                                      twist_diag_info_.delay_time_threshold));
        diag_status_array.push_back(
            checkMeasurementMahalanobisGate("twist",
                                            twist_diag_info_.is_passed_mahalanobis_gate,
                                            twist_diag_info_.mahalanobis_distance,
                                            params_.twist_gate_dist));
    }

    diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;
    diag_merged_status = mergeDiagnosticStatus(diag_status_array);
    diag_merged_status.name = "localization: " + std::string(this->get_name());
    diag_merged_status.hardware_id = this->get_name();

    diagnostic_msgs::msg::DiagnosticArray diag_msg;
    diag_msg.header.stamp = this->now();
    diag_msg.status.push_back(diag_merged_status);
    pub_diag_->publish(diag_msg);
}
/// @brief 更新一维滤波器
/// @param pose 位置和方向的带协方差的姿态消息
/// @param smoothing_step 平滑步数
void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped& pose,
                                         const size_t smoothing_step) {
    // 从姿态消息中提取高度z和姿态的滚转、俯仰角
    double z = pose.pose.pose.position.z;

    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);

    //根据姿态消息的协方差矩阵和平滑步数计算高度z、滚转、俯仰角的偏差
    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
    double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast<double>(smoothing_step);
    double roll_dev =
        pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
    double pitch_dev =
        pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(smoothing_step);

    z_filter_.update(z, z_dev, pose.header.stamp);
    roll_filter_.update(rpy.x, roll_dev, pose.header.stamp);
    pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp);
}

/// @brief 初始化一维滤波器
/// @param pose 位置和方向的带协方差的姿态消息
void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) {
    //从姿态消息中提取高度z和姿态的滚转、俯仰角
    double z = pose.pose.pose.position.z;

    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);

    // 根据姿态消息的协方差矩阵计算高度z、滚转、俯仰角的偏差
    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
    double z_dev = pose.pose.covariance[COV_IDX::Z_Z];
    double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL];
    double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH];

    z_filter_.init(z, z_dev, pose.header.stamp);
    roll_filter_.init(rpy.x, roll_dev, pose.header.stamp);
    pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp);
}

/// @brief 触发节点服务
/// @param req 请求
/// @param res 响应
void EKFLocalizer::serviceTriggerNode(const std_srvs::srv::SetBool::Request::SharedPtr req,
                                      std_srvs::srv::SetBool::Response::SharedPtr res) {
    if (req->data) {
        pose_queue_.clear();
        twist_queue_.clear();
        is_activated_ = true;
    } else {
        is_activated_ = false;
    }
    res->success = true;
    return;
}

2.4 ekf_module.cpp

// clang-format off
#define DEBUG_PRINT_MAT(X) {\
  if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\
}// 调试模式下打印矩阵的值
// clang-format on

/// @brief 初始化 EKF 模块
/// @param warning 警告模块
/// @param params 超参数
EKFModule::EKFModule(std::shared_ptr<Warning> warning, const HyperParameters params)
    : warning_(std::move(warning)),
      dim_x_(6),  // x, y, yaw, yaw_bias, vx, wz
      params_(params) {
    Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);
    Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15;  // for x & y
    P(IDX::YAW, IDX::YAW) = 50.0;                                            // for yaw
    if (params_.enable_yaw_bias_estimation) {
        P(IDX::YAWB, IDX::YAWB) = 50.0;  // for yaw bias
    }
    P(IDX::VX, IDX::VX) = 1000.0;  // for vx
    P(IDX::WZ, IDX::WZ) = 50.0;    // for wz

    kalman_filter_.init(
        X,
        P,
        params_
            .extend_state_step);  // 初始化卡尔曼滤波器,kalman_filter_的代码在common/kalman_filter/src/time_delay_kalman_filter.cpp中
}

/// @brief 初始化函数，用于设置初始姿态和协方差矩阵
/// @param initial_pose 初始姿态
/// @param transform 初始姿态的变换
void EKFModule::initialize(const PoseWithCovariance& initial_pose,
                           const geometry_msgs::msg::TransformStamped& transform) {
    Eigen::MatrixXd X(dim_x_, 1);
    Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);

    // 设置状态向量 X 和状态协方差矩阵 P，然后调用 kalman_filter_.init 进行初始化。
    X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x;
    X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y;
    X(IDX::YAW) =
        tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation);
    X(IDX::YAWB) = 0.0;
    X(IDX::VX) = 0.0;
    X(IDX::WZ) = 0.0;

    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
    P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X];
    P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y];
    P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW];

    if (params_.enable_yaw_bias_estimation) {
        P(IDX::YAWB, IDX::YAWB) = 0.0001;
    }
    P(IDX::VX, IDX::VX) = 0.01;
    P(IDX::WZ, IDX::WZ) = 0.01;

    kalman_filter_.init(X, P, params_.extend_state_step);
}

/// @brief 获取当前姿态的函数。从 EKF 中获取当前的位置和姿态信息，并根据需要计算偏航角
/// @param current_time 当前时间
/// @param z 高度
/// @param roll 滚动角
/// @param pitch 俯仰角
/// @param get_biased_yaw 是否获取偏航角
/// @return
geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose(const rclcpp::Time& current_time,
                                                          const double z,
                                                          const double roll,
                                                          const double pitch,
                                                          bool get_biased_yaw) const {
    // 从 EKF 中获取当前的x、y、偏航角和偏航角偏差
    const double x = kalman_filter_.getXelement(IDX::X);
    const double y = kalman_filter_.getXelement(IDX::Y);
    const double biased_yaw = kalman_filter_.getXelement(IDX::YAW);
    const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB);
    const double yaw = biased_yaw + yaw_bias;
    Pose current_ekf_pose;
    current_ekf_pose.header.frame_id = params_.pose_frame_id;
    current_ekf_pose.header.stamp = current_time;
    current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z);
    // 根据需要计算偏航角
    if (get_biased_yaw) {
        current_ekf_pose.pose.orientation =
            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);
    } else {
        current_ekf_pose.pose.orientation =
            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);
    }
    return current_ekf_pose;
}

/// @brief 获取当前速度的函数
/// @param current_time 当前时间
/// @return
geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(
    const rclcpp::Time& current_time) const {
    // 从卡尔曼滤波器中获取当前的速度信息
    const double vx = kalman_filter_.getXelement(IDX::VX);
    const double wz = kalman_filter_.getXelement(IDX::WZ);

    Twist current_ekf_twist;
    current_ekf_twist.header.frame_id = "base_link";
    current_ekf_twist.header.stamp = current_time;
    current_ekf_twist.twist.linear.x = vx;
    current_ekf_twist.twist.angular.z = wz;
    return current_ekf_twist;
}

/// @brief 获取当前姿态的协方差矩阵
/// @return
std::array<double, 36> EKFModule::getCurrentPoseCovariance() const {
    // 卡尔曼滤波器中最新的协方差矩阵转换成数组形式返回
    return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP());
}

/// @brief 获取当前速度的协方差矩阵
/// @return
std::array<double, 36> EKFModule::getCurrentTwistCovariance() const {
    // 调用 ekfCovarianceToTwistMessageCovariance
    // 函数将卡尔曼滤波器中最新的速度协方差矩阵转换成数组形式返回。
    return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP());
}
/// @brief 获取偏航角的偏差
/// @return
double EKFModule::getYawBias() const {
    // 从卡尔曼滤波器中获取最新的偏航角偏差并返回
    return kalman_filter_.getLatestX()(
        IDX::YAWB);  // kalman_filter_的代码在common/kalman_filter/src/time_delay_kalman_filter.cpp中
}

/// @brief 根据延迟预测下一个状态
/// @param dt 时间间隔
void EKFModule::predictWithDelay(const double dt) {
    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();
    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();

    const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0);
    const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0);
    const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0);

    const Vector6d X_next = predictNextState(X_curr, dt);        // 预测下一个状态
    const Matrix6d A = createStateTransitionMatrix(X_curr, dt);  // 创建状态转移矩阵
    const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d,
                                              proc_cov_vx_d,
                                              proc_cov_wz_d);  // 创建过程噪声协方差矩阵
    kalman_filter_.predictWithDelay(
        X_next,
        A,
        Q);  // 预测下一个状态,kalman_filter_的代码在common/kalman_filter/src/time_delay_kalman_filter.cpp中
}

/// @brief 这个函数用于处理姿态测量的更新
/// @param pose 当前姿态测量值及其协方差矩阵
/// @param dt 时间间隔
/// @param t_curr 当前时间
/// @param pose_diag_info 用于记录姿态测量更新诊断信息的结构体
/// @return
bool EKFModule::measurementUpdatePose(const PoseWithCovariance& pose,
                                      const double dt,
                                      const rclcpp::Time& t_curr,
                                      EKFDiagnosticInfo& pose_diag_info) {
    // 函数会检查pose的坐标系是否与设定的姿态坐标系一致
    if (pose.header.frame_id != params_.pose_frame_id) {
        warning_->warnThrottle(
            fmt::format("pose frame_id is %s, but pose_frame is set as %s. They must be same.",
                        pose.header.frame_id.c_str(),
                        params_.pose_frame_id.c_str()),
            2000);
    }
    // 从卡尔曼滤波器中获取当前状态估计值
    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();
    DEBUG_PRINT_MAT(X_curr.transpose());

    constexpr int dim_y = 3;  // pos_x, pos_y, yaw, depending on Pose output

    /* Calculate delay step */
    // 计算姿态测量的延迟步数，并更新延迟时间相关的诊断信息
    double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay;
    if (delay_time < 0.0) {
        warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000);
    }

    delay_time = std::max(delay_time, 0.0);

    const int delay_step = std::roundf(delay_time / dt);

    pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time);
    pose_diag_info.delay_time_threshold = params_.extend_state_step * dt;
    if (delay_step >= params_.extend_state_step) {
        pose_diag_info.is_passed_delay_gate = false;
        warning_->warnThrottle(
            poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt),
            2000);
        return false;
    }

    /* Set yaw */
    // 从姿态测量中提取姿态信息，并进行一些异常值检测和警告输出
    double yaw = tf2::getYaw(pose.pose.pose.orientation);
    const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW);
    const double yaw_error = normalizeYaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
    yaw = yaw_error + ekf_yaw;

    /* Set measurement matrix */
    Eigen::MatrixXd y(dim_y, 1);
    y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw;

    if (hasNan(y) || hasInf(y)) {
        warning_->warn(
            "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose "
            "message.");
        return false;
    }

    /* Gate */
    const Eigen::Vector3d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X),
                                kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y),
                                ekf_yaw);
    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();
    const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y);
    // 计算马氏距离，并进行门限判断，若超出门限则输出警告信息。
    const double distance = mahalanobis(y_ekf, y, P_y);
    pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance);
    if (distance > params_.pose_gate_dist) {
        pose_diag_info.is_passed_mahalanobis_gate = false;
        warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000);
        warning_->warnThrottle("Ignore the measurement data.", 2000);
        return false;
    }

    DEBUG_PRINT_MAT(y.transpose());
    DEBUG_PRINT_MAT(y_ekf.transpose());
    DEBUG_PRINT_MAT((y - y_ekf).transpose());

    // 构造姿态测量的测量矩阵C和协方差矩阵R。
    const Eigen::Matrix<double, 3, 6> C = poseMeasurementMatrix();
    const Eigen::Matrix3d R =
        poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps);

    // 调用卡尔曼滤波器的updateWithDelay函数进行状态更新
    kalman_filter_.updateWithDelay(y, C, R, delay_step);

    // debug
    const Eigen::MatrixXd X_result = kalman_filter_.getLatestX();
    DEBUG_PRINT_MAT(X_result.transpose());
    DEBUG_PRINT_MAT((X_result - X_curr).transpose());

    return true;
}

/// @brief 这个函数用于进行姿态测量的高度延迟补偿
/// @param pose 当前姿态测量值
/// @param delay_time 延迟时间
/// @return
geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay(
    const PoseWithCovariance& pose,
    const double delay_time) {
    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);
    const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time *
                            std::sin(-rpy.y);  // 函数会根据姿态的方向角计算出高度延迟的补偿值
    PoseWithCovariance pose_with_z_delay;
    pose_with_z_delay = pose;
    pose_with_z_delay.pose.pose.position.z +=
        dz_delay;  // 姿态的高度值进行延迟补偿，并返回补偿后的姿态测量值
    return pose_with_z_delay;
}

/// @brief 这个函数用于处理速度测量的更新
/// @param twist 当前速度测量值及其协方差矩阵
/// @param dt 时间间隔
/// @param t_curr 当前时间
/// @param twist_diag_info 用于记录速度测量更新诊断信息的结构体
/// @return
bool EKFModule::measurementUpdateTwist(const TwistWithCovariance& twist,
                                       const double dt,
                                       const rclcpp::Time& t_curr,
                                       EKFDiagnosticInfo& twist_diag_info) {
    // 函数会检查速度测量的坐标系是否为"base_link"
    if (twist.header.frame_id != "base_link") {
        warning_->warnThrottle("twist frame_id must be base_link", 2000);
    }

    // 从卡尔曼滤波器中获取当前状态估计值，并进行一些调试信息的打印
    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();
    DEBUG_PRINT_MAT(X_curr.transpose());

    constexpr int dim_y = 2;  // vx, wz

    /* Calculate delay step */
    // 计算速度测量的延迟时间
    double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay;
    if (delay_time < 0.0) {
        warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000);
    }
    delay_time = std::max(delay_time, 0.0);

    const int delay_step = std::roundf(delay_time / dt);

    twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time);
    twist_diag_info.delay_time_threshold = params_.extend_state_step * dt;
    if (delay_step >= params_.extend_state_step) {
        twist_diag_info.is_passed_delay_gate = false;
        warning_->warnThrottle(
            twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt),
            2000);
        return false;
    }

    /* Set measurement matrix */
    Eigen::MatrixXd y(dim_y, 1);
    y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;

    if (hasNan(y) || hasInf(y)) {
        warning_->warn(
            "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist "
            "message.");
        return false;
    }
    // 从速度测量中提取速度信息
    const Eigen::Vector2d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX),
                                kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ));

    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();
    const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y);

    // 计算马氏距离，并进行门限判断，若超出门限则输出警告信息
    const double distance = mahalanobis(y_ekf, y, P_y);
    twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance);
    if (distance > params_.twist_gate_dist) {
        twist_diag_info.is_passed_mahalanobis_gate = false;
        warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000);
        warning_->warnThrottle("Ignore the measurement data.", 2000);
        return false;
    }

    DEBUG_PRINT_MAT(y.transpose());
    DEBUG_PRINT_MAT(y_ekf.transpose());
    DEBUG_PRINT_MAT((y - y_ekf).transpose());

    // 构造速度测量的测量矩阵C和协方差矩阵R
    const Eigen::Matrix<double, 2, 6> C = twistMeasurementMatrix();
    const Eigen::Matrix2d R =
        twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps);

    kalman_filter_.updateWithDelay(y, C, R, delay_step);

    // debug
    const Eigen::MatrixXd X_result = kalman_filter_.getLatestX();
    DEBUG_PRINT_MAT(X_result.transpose());
    DEBUG_PRINT_MAT((X_result - X_curr).transpose());

    return true;
}

3. 重点内容解析

3.1 输入时间延迟补偿

这个操作其实比较简单基本，那当前时间 t_curr 与 pose 的时间戳之差。并与预设的延迟时间，得到一个延迟的时间，这个时间会加入到pose中完成更新，然后就是根据时间差去拿到z轴上的高度。

const double delay_time = (t_curr - pose->header.stamp).seconds() +
                          params_.pose_additional_delay;  // 计算延迟时间
// 根据测量姿态的时间戳和当前时间戳计算延迟时间，然后使用延迟时间对姿态进行补偿
const auto pose_with_z_delay =
    ekf_module_->compensatePoseWithZDelay(*pose, delay_time);

3.2 自动估计偏航误差

这个里面的估算其实都在common/kalman_filter/src/time_delay_kalman_filter.cpp中，主要根据yaw以及yaw_bias来完成偏航角状态估算

    const double biased_yaw = kalman_filter_.getXelement(IDX::YAW);
    const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB);
    const double yaw = biased_yaw + yaw_bias;
    Pose current_ekf_pose;
    current_ekf_pose.header.frame_id = params_.pose_frame_id;
    current_ekf_pose.header.stamp = current_time;
    current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z);
    // 根据需要计算偏航角
    if (get_biased_yaw) {
        current_ekf_pose.pose.orientation =
            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);
    } else {
        current_ekf_pose.pose.orientation =
            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);
    }

3.3 马氏距离门

第一行计算了观测值y_ekf相对于实际观测值y的马氏距离，使用了之前定义的P_y作为协方差矩阵。
接着更新了诊断信息pose_diag_info中的mahalanobis_distance，确保记录下最大的马氏距离。
最后，通过比较马氏距离和门限值params_.pose_gate_dist来进行门限判断，若超出门限则设置pose_diag_info.is_passed_mahalanobis_gate为false，并输出警告信息。

需要注意的是我们拿到的值是加入延迟的，即代码中的 delay_step * dim_x_ 表示一个延迟步长乘以维度X的大小，而 IDX::X 则表示在X方向上的索引。 dim_x_是一个6元素的数组，分别存储着：x, y, yaw, yaw_bias, vx, wz。

const Eigen::Vector3d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X),
                            kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y),
                            ekf_yaw);
const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();
const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y);
// 计算马氏距离，并进行门限判断，若超出门限则输出警告信息。
const double distance = mahalanobis(y_ekf, y, P_y);
pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance);
if (distance > params_.pose_gate_dist) {
    pose_diag_info.is_passed_mahalanobis_gate = false;
    warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000);
    warning_->warnThrottle("Ignore the measurement data.", 2000);
    return false;
}

3.4 平滑更新

平滑更新其实里面没有什么操作，在3.1延迟补偿下面就是平滑更新，主要虎牙股根据平滑参数与协方差矩阵完成轨迹的平滑

            if (is_updated) {
                pose_is_updated = true;

                // Update Simple 1D filter with considering change of z value due to measurement
                // pose
                const double delay_time = (t_curr - pose->header.stamp).seconds() +
                                          params_.pose_additional_delay;  // 计算延迟时间
                // 根据测量姿态的时间戳和当前时间戳计算延迟时间，然后使用延迟时间对姿态进行补偿
                const auto pose_with_z_delay =
                    ekf_module_->compensatePoseWithZDelay(*pose, delay_time);
                updateSimple1DFilters(pose_with_z_delay,
                                      params_.pose_smoothing_steps);  // 更新滤波器
            }

3.5 俯仰角计算垂直校正量

这部分在《3.1 输入时间延迟补偿》已经讲到，示意图如下

定位

原创文章作者：敢敢のwings。如若转载，请注明出处：古月居 http://admin.guyuehome.com/47204

打赏 0

上一篇：经典文献阅读之--FlashOcc(快速且内存高效的占用预测模块)

下一篇：RAG与知识库搭建

精选Autoware 定位之EKF 滤波定位（四）

敢敢のwings

0. 简介

1. 主要功能

2. 代码阅读

2.1 covariance.cpp

2.2 diagnostics.cpp

2.3 ekf_localizer.cpp

2.4 ekf_module.cpp

3. 重点内容解析

3.1 输入时间延迟补偿

3.2 自动估计偏航误差

3.3 马氏距离门

3.4 平滑更新

3.5 俯仰角计算垂直校正量

为你推荐

精选四足机器人雷达-视觉导航3：Ego Planning 面向四足机器人的修改

精选在gazebo里搭建一个livox mid360 + 惯导仿真平台测试 FAST-LIO2

ALOAM:异常点剔除机制代码解析

【动态SLAM】Dyna-SLAM原理介绍

点云配准方法---ICP升级版本NICP(Normal ICP)

经典文献阅读之--On Degeneracy of Optimization-based State Estimation Problems(防止非线性优化退化)

评论（0）

关于作者

敢敢のwings

395

2

1368

17

精选代码浅析DLIO(四)---位姿更新

FAST-LIO,FAST-LIO2与FASTER-LIO

精选经典文献阅读之--Light-LOAM( 基于图匹配的轻量级激光雷达里程计和地图构建)

相关推荐

基于ROS通信机制的多点导航实验

【自动驾驶】高精度地图车道线三次样条插值算法（一）

IMU预积分在优化问题中的建模及外参标定

机器人学习--定位、建图、SLAM（声呐、激光等扫描束方案）的发展史

Udacity机器人软件工程师课程笔记（一）-样本搜索和找回-基于漫游者号模拟器

不可错过的gmapping算法使用与详细解释

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

Autoware

dlo系列

轨迹规划

大模型

Matlab-ros

深度学习

CUDA

FAST-LIO系列

规范学习

机器人仿真

传感器感知

ROS基础以及进阶

无人驾驶

从零到一的SLAM

机器人基础知识

经典论文阅读

C++模式

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板