无迹卡尔曼

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 # 添加 SDK 库路径
 set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
@@ -41,6 +42,7 @@ ament_target_dependencies(imu_receiver_node
   std_msgs
   geometry_msgs
   sensor_msgs
+  Eigen3
 )
 
 # ==================== 安装目标 ====================

docs/IMU/packet/MSG_IMU.md

@@ -0,0 +1,35 @@
+# MSG_IMU | FDISYSTEMS支持中心
+
+1. 机体坐标系及坐标系方向如图所示。在FDI系列产品中，模块表面都会标注准确的机体坐标系方向。具体以实际产品为准。
+
+2. 对于角速度，加速度的标定解释：
+   - (1) 转台标定：测量并补偿了陀螺仪和加速度计的零偏、尺度误差及三轴不垂直度。
+   - (2) 温箱标定：在宽温范围（如-40℃~85℃）下，测量了传感器误差随温度的变化规律，并生成了补偿模型。对陀螺和加表的输出数据进行了补偿。
+
+3. 对于加速度中未分离重力加速度解释：
+   加速度计测量的是载体运动与地球重力的"混合"结果。简单来说，当模块静止时，加速度读数并不为零，而会显示出一个重力值（例如，平放时Z轴约为+9.8 m/s²）。
+
+## 基本信息
+
+| 属性 | 值 |
+|------|-----|
+| **Packet ID** | 0x40 |
+| **Length** | 56 |
+| **Read / Write** | Read |
+
+## 数据字段
+
+| Offset | Size | Format | Field | Unit | Description |
+|--------|------|--------|-------|------|-------------|
+| 0 | 4 | float32_t | Gyroscope_X | rad/s | 机体系X轴角速度 |
+| 4 | 4 | float32_t | Gyroscope_Y | rad/s | 机体系Y轴角速度 |
+| 8 | 4 | float32_t | Gyroscope_Z | rad/s | 机体系Z轴角速度 |
+| 12 | 4 | float32_t | Accelerometer_X | m/s² | 机体系X轴加速度(未分离重力加速度) |
+| 16 | 4 | float32_t | Accelerometer_Y | m/s² | 机体系Y轴加速度(未分离重力加速度) |
+| 20 | 4 | float32_t | Accelerometer_Z | m/s² | 机体系Z轴加速度(未分离重力加速度) |
+| 24 | 4 | float32_t | Magnetometer_X | mG | 机体系X轴磁感应强度 |
+| 28 | 4 | float32_t | Magnetometer_Y | mG | 机体系Y轴磁感应强度 |
+| 32 | 4 | float32_t | Magnetometer_Z | mG | 机体系Z轴磁感应强度 |
+| 36 | 4 | float32_t | IMU_Temperature | deg C | 如果IMU数据由多个传感器组成则该值为这些传感器的平均温度 |
+| 40 | 4 | float32_t | Pressure | Pa | 气压值，如果没装气压计，默认为一个标准大气压101325Pa |
+| 48 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 |

src/imu_receiver_node.cpp

@@ -1,21 +1,24 @@
 /**
  * @file imu_receiver_node.cpp
- * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本
+ * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF
  *
  * 通过串口读取 FDI DETA10 IMU 模块的数据
  * 波特率：921600
  * 协议：FDILink
  *
  * 转发数据包：
- * - MSG_AHRS (0x41): 航姿参考系统数据
+ * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力）
- * - MSG_BODY_VEL (0x60): 机体系速度数据 (IMU卡尔曼滤波后)
  *
- * 特性：使用环形缓冲区避免数据堆积爆发
+ * 特性：
+ * - 使用环形缓冲区避免数据堆积爆发
+ * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波
  */
 
 #include <atomic>
+#include <cmath>
 #include <condition_variable>
 #include <cstring>
+#include <eigen3/Eigen/Dense>
 #include <errno.h>
 #include <fcntl.h>
 #include <geometry_msgs/msg/quaternion.hpp>
@@ -38,9 +41,8 @@
 constexpr uint8_t FRAME_START = 0xFC;
 constexpr uint8_t FRAME_END = 0xFD;
 
-// 数据包 ID
+// 数据包 ID - 只使用 MSG_BODY_ACCELERATION
-constexpr uint8_t MSG_AHRS = 0x41;     // 航姿参考系统数据
+constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度
-constexpr uint8_t MSG_BODY_VEL = 0x60; // 机体系速度数据 (卡尔曼滤波后)
 
 // 默认串口设备
 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -56,6 +58,11 @@ constexpr size_t MAX_PUBLISH_RATE_HZ = 100; // 最大发布频率 100Hz
 constexpr auto PUBLISH_PERIOD =
     std::chrono::microseconds(1000000 / MAX_PUBLISH_RATE_HZ); // 10ms
 
+// UKF 参数
+constexpr int UKF_STATE_DIM = 2; // 状态维度 [位置, 速度]
+constexpr int UKF_MEAS_DIM = 1;  // 测量维度 [位置]
+constexpr int UKF_SIGMA_POINTS = 2 * UKF_STATE_DIM + 1; // 2n+1 个 Sigma 点
+
 // CRC8 查找表
 static const uint8_t CRC8Table[] = {
     0,   94,  188, 226, 97,  63,  221, 131, 194, 156, 126, 32,  163, 253, 31,
@@ -109,16 +116,221 @@ static const uint16_t CRC16Table[256] = {
     0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74,
     0x2E93, 0x3EB2, 0x0ED1, 0x1EF0};
 
-// IMU 数据结构
+/**
-struct ImuDataFrame {
+ * @brief 无迹卡尔曼滤波器 (UKF) 用于加速度滤波
-  bool ahrs_valid = false;
+ *
-  float roll_speed = 0, pitch_speed = 0, heading_speed = 0;
+ * 状态向量: [位置, 速度]^T
-  float roll = 0, pitch = 0, heading = 0;
+ * 测量: 位置
-  float q1 = 0, q2 = 0, q3 = 0, q4 = 0;
+ *
-  int64_t timestamp_us = 0;
+ * 使用 Sigma 点变换处理非线性
+ */
+class UKF {
+public:
+  UKF()
+      : x_(Eigen::Vector2d::Zero()), P_(Eigen::Matrix2d::Identity()),
+        Q_(Eigen::Matrix2d::Identity() * 0.001),
+        R_(Eigen::Matrix<double, 1, 1>::Identity() * 0.03), dt_(0.01),
+        initialized_(false) {
+    // 初始化权重
+    computeWeights();
+  }
 
-  bool vel_valid = false;
+  /**
-  float vel_x = 0, vel_y = 0, vel_z = 0;
+   * @brief 设置过程噪声
+   */
+  void setProcessNoise(double q) {
+    Q_ = Eigen::Matrix2d::Identity() * q;
+    Q_(1, 1) = q * 10; // 速度噪声更大一些
+  }
+
+  /**
+   * @brief 设置测量噪声
+   */
+  void setMeasurementNoise(double r) { R_(0, 0) = r; }
+
+  /**
+   * @brief 更新滤波器 - 简单的一维位置滤波
+   * @param measurement 测量值 (加速度/位置)
+   * @return 滤波后的值
+   */
+  double update(double measurement) {
+    if (!initialized_) {
+      x_(0) = measurement;
+      x_(1) = 0; // 初始速度为0
+      initialized_ = true;
+      return measurement;
+    }
+
+    // === 预测步骤 ===
+    // 生成 Sigma 点
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points;
+    generateSigmaPoints(sigma_points);
+
+    // 传播 Sigma 点通过状态转移方程
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points_pred;
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      // 状态转移: 恒定速度模型
+      // x_k+1 = x_k + v_k * dt
+      // v_k+1 = v_k
+      sigma_points_pred(0, i) = sigma_points(0, i) + sigma_points(1, i) * dt_;
+      sigma_points_pred(1, i) = sigma_points(1, i);
+    }
+
+    // 计算预测均值
+    x_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      x_pred_ += weights_m_[i] * sigma_points_pred.col(i);
+    }
+
+    // 计算预测协方差
+    P_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Vector2d diff = sigma_points_pred.col(i) - x_pred_;
+      P_pred_ += weights_c_[i] * diff * diff.transpose();
+    }
+    P_pred_ += Q_;
+
+    // === 更新步骤 ===
+    // 传播 Sigma 点到测量空间
+    Eigen::Matrix<double, UKF_MEAS_DIM, UKF_SIGMA_POINTS> Z_sigma;
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      // 测量模型: 只测量位置
+      Z_sigma(0, i) = sigma_points_pred(0, i);
+    }
+
+    // 计算预测测量均值
+    z_pred_.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      z_pred_ += weights_m_[i] * Z_sigma.col(i);
+    }
+
+    // 计算预测测量协方差
+    Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> S;
+    S.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff = Z_sigma.col(i) - z_pred_;
+      S += weights_c_[i] * diff * diff.transpose();
+    }
+    S += R_;
+
+    // 计算互协方差
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> T;
+    T.setZero();
+    for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
+      Eigen::Vector2d diff_x = sigma_points_pred.col(i) - x_pred_;
+      Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff_z = Z_sigma.col(i) - z_pred_;
+      T += weights_c_[i] * diff_x * diff_z.transpose();
+    }
+
+    // 计算卡尔曼增益
+    Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> K = T * S.inverse();
+
+    // 更新状态
+    Eigen::Matrix<double, UKF_MEAS_DIM, 1> z;
+    z(0) = measurement;
+    x_ = x_pred_ + K * (z - z_pred_);
+
+    // 更新协方差
+    P_ = P_pred_ - K * S * K.transpose();
+
+    return x_(0);
+  }
+
+  /**
+   * @brief 设置采样时间
+   */
+  void setDt(double dt) { dt_ = dt; }
+
+  /**
+   * @brief 获取当前估计值
+   */
+  double getEstimate() const { return x_(0); }
+
+  /**
+   * @brief 获取当前速度估计
+   */
+  double getVelocity() const { return x_(1); }
+
+private:
+  /**
+   * @brief 计算 Sigma 点权重
+   */
+  void computeWeights() {
+    // 使用标准 UKF 参数
+    // alpha = 1e-3, beta = 2, kappa = 0
+    double alpha = 1e-3;
+    double beta = 2.0;
+    double kappa = 0.0;
+
+    lambda_ = alpha * alpha * (UKF_STATE_DIM + kappa) - UKF_STATE_DIM;
+
+    weights_m_.resize(UKF_SIGMA_POINTS);
+    weights_c_.resize(UKF_SIGMA_POINTS);
+
+    weights_m_[0] = lambda_ / (UKF_STATE_DIM + lambda_);
+    weights_c_[0] = weights_m_[0] + (1 - alpha * alpha + beta);
+
+    for (int i = 1; i < UKF_SIGMA_POINTS; ++i) {
+      weights_m_[i] = 0.5 / (UKF_STATE_DIM + lambda_);
+      weights_c_[i] = weights_m_[i];
+    }
+  }
+
+  /**
+   * @brief 生成 Sigma 点
+   */
+  void generateSigmaPoints(
+      Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> &sigma_points) {
+    // 计算矩阵平方根
+    Eigen::Matrix2d sqrt_P = ((UKF_STATE_DIM + lambda_) * P_).llt().matrixL();
+
+    sigma_points.col(0) = x_;
+
+    for (int i = 0; i < UKF_STATE_DIM; ++i) {
+      sigma_points.col(i + 1) = x_ + sqrt_P.col(i);
+      sigma_points.col(i + 1 + UKF_STATE_DIM) = x_ - sqrt_P.col(i);
+    }
+  }
+
+  // 状态向量 [位置, 速度]
+  Eigen::Vector2d x_;
+  Eigen::Vector2d x_pred_;
+
+  // 协方差矩阵
+  Eigen::Matrix2d P_;
+  Eigen::Matrix2d P_pred_;
+
+  // 过程噪声和测量噪声
+  Eigen::Matrix2d Q_;
+  Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> R_;
+
+  // 预测测量
+  Eigen::Matrix<double, UKF_MEAS_DIM, 1> z_pred_;
+
+  // UKF 参数
+  double lambda_;
+  std::vector<double> weights_m_;
+  std::vector<double> weights_c_;
+
+  // 采样时间
+  double dt_;
+
+  // 初始化标志
+  bool initialized_;
+};
+
+// IMU 数据结构 - 使用 MSG_BODY_ACCELERATION 数据包
+struct ImuDataFrame {
+  bool valid = false;
+
+  // 滤波修正后的机体系加速度 (m/s²) - 不包括重力
+  float acc_x = 0, acc_y = 0, acc_z = 0;
+
+  // 当地重力加速度 (m/s²)
+  float g_force = 0;
+
+  // 时间戳 (从接收到数据的时间)
+  std::chrono::steady_clock::time_point receive_time;
 };
 
 // 线程安全的环形缓冲区
@@ -191,23 +403,48 @@ public:
     this->declare_parameter("verbose", true); // 是否输出详细日志
     this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭）
 
+    // UKF 参数
+    this->declare_parameter("ukf_process_noise", 0.001);    // 过程噪声
+    this->declare_parameter("ukf_measurement_noise", 0.03); // 测量噪声
+    this->declare_parameter("ukf_dt", 0.01);                // 采样时间
+
     // 获取参数
     serial_port_ = this->get_parameter("serial_port").as_string();
     baudrate_ = this->get_parameter("baudrate").as_int();
     enable_crc_ = this->get_parameter("enable_crc").as_bool();
 
+    // 获取 UKF 参数并设置
+    double process_noise = this->get_parameter("ukf_process_noise").as_double();
+    double measurement_noise =
+        this->get_parameter("ukf_measurement_noise").as_double();
+    double dt = this->get_parameter("ukf_dt").as_double();
+
+    // 设置 UKF 参数
+    ukf_x_.setProcessNoise(process_noise);
+    ukf_x_.setMeasurementNoise(measurement_noise);
+    ukf_x_.setDt(dt);
+    ukf_y_.setProcessNoise(process_noise);
+    ukf_y_.setMeasurementNoise(measurement_noise);
+    ukf_y_.setDt(dt);
+    ukf_z_.setProcessNoise(process_noise);
+    ukf_z_.setMeasurementNoise(measurement_noise);
+    ukf_z_.setDt(dt);
+
     RCLCPP_INFO(this->get_logger(), "---------------------------------");
-    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点");
+    RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF 版本)");
     RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
     RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
     RCLCPP_INFO(this->get_logger(), "CRC校验: %s",
                 enable_crc_ ? "启用" : "禁用");
+    RCLCPP_INFO(this->get_logger(),
+                "UKF 参数 - 过程噪声: %.4f, 测量噪声: %.4f, dt: %.4f",
+                process_noise, measurement_noise, dt);
     RCLCPP_INFO(this->get_logger(), "---------------------------------");
 
     // 创建发布者
-    ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
+    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
-    body_vel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
+    accel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
-        "imu/body_velocity", 10);
+        "imu/body_acceleration", 10);
     connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
         "imu/connection_status", 10);
 
@@ -351,9 +588,6 @@ private:
     std::vector<uint8_t> current_frame;
     current_frame.reserve(MAX_FRAME_SIZE);
 
-    // 当前正在积累的 IMU 数据帧
-    ImuDataFrame current_data;
-
     while (running_ && rclcpp::ok()) {
       ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf));
 
@@ -396,8 +630,8 @@ private:
                                          rx_buffer.begin() + i + 5 + remaining);
 
                     if (current_frame.back() == FRAME_END) {
-                      // 解析帧到 current_data
+                      // 解析帧
-                      parseFrameToData(current_frame, current_data);
+                      parseFrame(current_frame);
                     }
                     in_frame = false;
                     i += total_frame_len;
@@ -414,7 +648,7 @@ private:
           } else {
             current_frame.push_back(byte);
             if (byte == FRAME_END) {
-              parseFrameToData(current_frame, current_data);
+              parseFrame(current_frame);
               in_frame = false;
               i++;
             } else if (current_frame.size() >= MAX_FRAME_SIZE) {
@@ -437,8 +671,8 @@ private:
     }
   }
 
-  // 解析帧到数据结构 - 独立处理，不等待配对
+  // 解析帧 - 只处理 MSG_BODY_ACCELERATION
-  void parseFrameToData(const std::vector<uint8_t> &frame, ImuDataFrame &data) {
+  void parseFrame(const std::vector<uint8_t> &frame) {
     if (frame.size() < 8)
       return;
     if (frame.back() != FRAME_END)
@@ -458,43 +692,29 @@ private:
     }
 
     const uint8_t *payload = frame.data() + 7;
-    bool should_push = false;
 
-    switch (msg_id) {
+    // 只处理 MSG_BODY_ACCELERATION 数据包 (0x62)
-    case MSG_AHRS:
+    if (msg_id == MSG_BODY_ACCELERATION) {
-      if (data_len >= 48) {
+      if (data_len >= 16) {
-        data.ahrs_valid = true;
+        ImuDataFrame data;
-        data.roll_speed = *reinterpret_cast<const float *>(payload);
+        data.valid = true;
-        data.pitch_speed = *reinterpret_cast<const float *>(payload + 4);
-        data.heading_speed = *reinterpret_cast<const float *>(payload + 8);
-        data.roll = *reinterpret_cast<const float *>(payload + 12);
-        data.pitch = *reinterpret_cast<const float *>(payload + 16);
-        data.heading = *reinterpret_cast<const float *>(payload + 20);
-        data.q1 = *reinterpret_cast<const float *>(payload + 24);
-        data.q2 = *reinterpret_cast<const float *>(payload + 28);
-        data.q3 = *reinterpret_cast<const float *>(payload + 32);
-        data.q4 = *reinterpret_cast<const float *>(payload + 36);
-        data.timestamp_us = *reinterpret_cast<const int64_t *>(payload + 40);
-        should_push = true;
-      }
-      break;
 
-    case MSG_BODY_VEL:
+        // 滤波修正后的机体系加速度 (m/s²) - 不包括重力
-      if (data_len >= 12) {
+        data.acc_x = *reinterpret_cast<const float *>(payload);
-        data.vel_valid = true;
+        data.acc_y = *reinterpret_cast<const float *>(payload + 4);
-        data.vel_x = *reinterpret_cast<const float *>(payload);
+        data.acc_z = *reinterpret_cast<const float *>(payload + 8);
-        data.vel_y = *reinterpret_cast<const float *>(payload + 4);
-        data.vel_z = *reinterpret_cast<const float *>(payload + 8);
-        should_push = true;
-      }
-      break;
-    }
 
-    // 只要有有效数据就推入缓冲区（不等待配对）
+        // 当地重力加速度 (m/s²)
-    if (should_push) {
+        data.g_force = *reinterpret_cast<const float *>(payload + 12);
+
+        data.receive_time = std::chrono::steady_clock::now();
+
+        // 推入环形缓冲区
         ring_buffer_.push(data);
       }
     }
+    // 其他数据包类型被忽略
+  }
 
   // 处理循环 - 固定频率从环形缓冲区读取并发布
   void processLoop() {
@@ -523,49 +743,51 @@ private:
     }
   }
 
-  // 发布数据
+  // 发布数据 - 使用 UKF 对加速度进行滤波
   void publishData(const ImuDataFrame &data) {
-    // 时间戳转换
     rclcpp::Time current_time = this->now();
-    rclcpp::Time imu_time;
 
-    if (!imu_time_initialized_) {
+    // 使用 UKF 对加速度数据进行滤波
-      imu_base_time_ =
+    float filtered_acc_x = ukf_x_.update(data.acc_x);
-          current_time - rclcpp::Duration(0, data.timestamp_us * 1000);
+    float filtered_acc_y = ukf_y_.update(data.acc_y);
-      imu_time_initialized_ = true;
+    float filtered_acc_z = ukf_z_.update(data.acc_z);
-    }
+
-    imu_time = imu_base_time_ + rclcpp::Duration(0, data.timestamp_us * 1000);
+    // 发布加速度消息 (滤波后的数据)
-    latest_imu_timestamp_ = imu_time;
+    geometry_msgs::msg::Vector3 accel_msg;
+    accel_msg.x = filtered_acc_x;
+    accel_msg.y = filtered_acc_y;
+    accel_msg.z = filtered_acc_z;
+    accel_pub_->publish(accel_msg);
 
     // 发布 IMU 消息
     sensor_msgs::msg::Imu imu_msg;
-    imu_msg.header.stamp = imu_time;
+    imu_msg.header.stamp = current_time;
     imu_msg.header.frame_id = "imu_link";
-    imu_msg.angular_velocity.x = data.roll_speed;
-    imu_msg.angular_velocity.y = data.pitch_speed;
-    imu_msg.angular_velocity.z = data.heading_speed;
-    imu_msg.orientation.x = data.q2;
-    imu_msg.orientation.y = data.q3;
-    imu_msg.orientation.z = data.q4;
-    imu_msg.orientation.w = data.q1;
-    imu_msg.linear_acceleration.x = 0;
-    imu_msg.linear_acceleration.y = 0;
-    imu_msg.linear_acceleration.z = 0;
-    ahrs_pub_->publish(imu_msg);
 
-    // 发布速度
+    // 线性加速度 - 使用 UKF 滤波后的值
-    geometry_msgs::msg::Vector3 vel_msg;
+    imu_msg.linear_acceleration.x = filtered_acc_x;
-    vel_msg.x = data.vel_x;
+    imu_msg.linear_acceleration.y = filtered_acc_y;
-    vel_msg.y = data.vel_y;
+    imu_msg.linear_acceleration.z = filtered_acc_z;
-    vel_msg.z = data.vel_z;
+
-    body_vel_pub_->publish(vel_msg);
+    // 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据）
+    imu_msg.angular_velocity.x = 0;
+    imu_msg.angular_velocity.y = 0;
+    imu_msg.angular_velocity.z = 0;
+
+    imu_msg.orientation.x = 0;
+    imu_msg.orientation.y = 0;
+    imu_msg.orientation.z = 0;
+    imu_msg.orientation.w = 1; // 单位四元数
+
+    imu_pub_->publish(imu_msg);
 
     // 日志
-    RCLCPP_INFO(this->get_logger(),
+    RCLCPP_INFO(
-                "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f | VEL - X: "
+        this->get_logger(),
-                "%.3f, Y: %.3f, Z: %.3f",
+        "Body Accel(raw): [%.3f, %.3f, %.3f] | Accel(UKF): [%.3f, %.3f, "
-                data.roll, data.pitch, data.heading, data.vel_x, data.vel_y,
+        "%.3f] m/s² | G-force: %.3f m/s²",
-                data.vel_z);
+        data.acc_x, data.acc_y, data.acc_z, filtered_acc_x, filtered_acc_y,
+        filtered_acc_z, data.g_force);
   }
 
   void publishStatus() {
@@ -592,13 +814,13 @@ private:
   std::thread process_thread_;
   RingBuffer<ImuDataFrame> ring_buffer_;
 
-  // IMU 时间戳同步
+  // UKF 滤波器（用于加速度滤波）
-  bool imu_time_initialized_ = false;
+  UKF ukf_x_;
-  rclcpp::Time imu_base_time_;
+  UKF ukf_y_;
-  rclcpp::Time latest_imu_timestamp_;
+  UKF ukf_z_;
 
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_vel_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr accel_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
   rclcpp::TimerBase::SharedPtr status_timer_;
 };