从零构建机器人位姿估计器：深入解析扩展卡尔曼滤波的Python实践

在机器人导航与自动驾驶领域，准确估计自身在环境中的位置和姿态（合称“位姿”）是执行一切高级任务的基础。无论是仓库中的AGV小车，还是室内的服务机器人，它们都需要回答一个根本问题：“我现在在哪里？我的朝向如何？”现实世界充满不确定性——轮子会打滑，传感器有噪声，单一的数据源往往不可靠。因此，多传感器融合成为了解决这一问题的核心钥匙，而扩展卡尔曼滤波（EKF）则是其中最经典、最实用的算法框架之一。

ROS生态中广为人知的 robot_pose_ekf 包，就像一个封装精密的黑盒，为开发者提供了开箱即用的融合方案。但知其然，更要知其所以然。对于有志于深入机器人感知与状态估计领域的学习者、工程师，或是正在寻找毕业设计课题的学生而言，仅仅调用现成的ROS包是远远不够的。理解EKF内部的数学原理、代码实现细节，乃至亲手“造一次轮子”，是突破技术瓶颈、获得真正掌控感的必经之路。本文将带你抛开现成的工具包，从最基础的数学模型出发，一步步推导，并用Python实现一个简化但功能完整的机器人位姿EKF估计器。我们会融合模拟的轮式里程计与IMU数据，并通过可视化对比，让你直观感受融合带来的精度提升。

1. 理论基础：扩展卡尔曼滤波为何适用于位姿估计

在开始写代码之前，我们必须先理解手中的“武器”。卡尔曼滤波（KF）本质上是针对线性高斯系统的最优状态估计器。但机器人的运动模型和观测模型往往是非线性的。例如，机器人的转向角（偏航角）与运动速度之间的关系就是三角函数，是非线性的。扩展卡尔曼滤波（EKF）通过一阶泰勒展开，在状态估计值附近对非线性模型进行线性化近似，从而将卡尔曼滤波的框架扩展到非线性系统。

对于移动机器人的平面位姿，我们通常关心三个状态量：平面坐标 (x, y) 和偏航角 θ（即yaw角）。我们的目标是利用两类常见传感器：

1. 轮式里程计 (Odometry)：通过编码器测量轮子转动的圈数，积分得到机器人的相对位移和转角。它提供的是增量式的位姿变化信息，但容易因打滑、地面不平而产生累积误差。
2. 惯性测量单元 (IMU)：特别是其中的陀螺仪，可以直接测量机器人的角速度，积分得到偏航角的变化。其提供的姿态信息，尤其是俯仰和横滚角，具有绝对参考（重力方向），但积分会引入漂移。

EKF的巧妙之处在于，它用预测-更新的循环，动态地、定量地结合了这两种信息。它维护两个核心变量：

• 状态估计均值 (x): 我们认为机器人最可能处于的位姿。
• 状态估计协方差矩阵 (P): 表示我们对当前估计值的不确定度（信心程度）。

提示：协方差矩阵 P 是EKF的灵魂。它的大小决定了滤波器是更相信预测模型，还是更相信新的传感器观测。一个大的协方差意味着“我不太确定”，滤波器会更倾向于采纳新的观测来修正自己。

EKF的流程可以概括为以下周期：

1. 预测步 (Predict)：根据上一时刻的状态和控制输入（或运动模型），预测当前时刻的状态和协方差。这一步主要依赖里程计数据。
2. 更新步 (Update)：当收到新的传感器观测（如IMU的偏航角）时，计算预测观测与实际观测之间的差异（创新），然后根据预测的不确定度和传感器观测的不确定度（由传感器协方差矩阵 R 表示），以最优的比例（卡尔曼增益 K）来修正预测的状态和协方差。

通过这个持续的过程，EKF能够有效抑制里程计的累积漂移，同时用里程计来校正IMU的积分漂移，实现“1+1>2”的效果。

2. 构建我们的简化版EKF：模型定义与初始化

我们将实现一个针对二维平面移动机器人的EKF。状态向量定义为 x = [x, y, theta, v, omega]^T，即包含位置、朝向以及线速度和角速度。速度和角速度作为状态的一部分，可以让模型更通用，尽管我们可能不直接观测它们。

首先，我们需要定义几个核心的数学模型：

• 运动模型 (Process Model): 描述状态如何随时间演化。我们采用基于速度的运动模型：

  x_{k} = x_{k-1} + v * dt * cos(theta_{k-1})
  y_{k} = y_{k-1} + v * dt * sin(theta_{k-1})
  theta_{k} = theta_{k-1} + omega * dt
  v_{k} = v_{k-1}  // 假设速度短时间内恒定
  omega_{k} = omega_{k-1} // 假设角速度短时间内恒定
  

  这个模型是非线性的，因为包含了 cos(theta) 和 sin(theta)。

• 观测模型 (Observation Model): 描述传感器观测与状态之间的关系。

  • 对于里程计，我们通常直接得到相对位移和转角 (delta_x, delta_y, delta_theta)，这可以视为对速度的间接观测。为了简化，我们可以假设有一个“理想”的里程计直接观测位置和朝向 [x, y, theta]。
  • 对于IMU，我们假设它直接观测偏航角 theta。

接下来，我们用Python代码来搭建这个框架的骨架。我们将使用 numpy 进行矩阵运算，matplotlib 进行可视化。

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import block_diag

class RobotPoseEKF:
    def __init__(self, initial_pose, initial_covariance):
        """
        初始化EKF。
        :param initial_pose: 初始状态向量 [x, y, theta, v, omega]
        :param initial_covariance: 初始状态协方差矩阵 (5x5)
        """
        self.state = np.array(initial_pose, dtype=np.float64).reshape(-1, 1)  # 列向量
        self.P = np.array(initial_covariance, dtype=np.float64)  # 状态协方差

        # 过程噪声协方差矩阵 Q，表示运动模型的不确定度
        # 数值需要根据实际机器人特性调整
        self.Q = np.diag([0.1, 0.1, 0.05, 0.2, 0.1]) ** 2  # 假设的噪声方差

        # 观测噪声协方差矩阵 R，对于不同的观测源（如odom, imu）会不同
        # 将在更新步中动态指定
        self.R_odom = None
        self.R_imu = None

        # 时间步长，假设固定，实际中应根据传感器数据时间戳计算
        self.dt = 0.1  # 100ms

在上面的初始化中，Q 矩阵的设定至关重要。它反映了我们对运动模型的信任程度。例如，Q[2,2]（对应 theta 的噪声）设得较大，意味着我们认为运动模型在预测转角时不确定性较高。这些值通常需要通过实验或系统辨识来调整。

3. 核心算法实现：预测步与更新步

这是整个EKF的心脏部分。我们需要计算雅可比矩阵（Jacobian），即非线性模型在当前状态处的偏导数矩阵，用于线性化。

3.1 预测步的实现

预测步需要计算状态转移函数 f 及其雅可比矩阵 F（关于状态 x 的偏导），以及过程噪声的雅可比矩阵 V（关于过程噪声 w 的偏导，通常简化假设为单位阵或与模型相关）。这里我们做一个简化，假设过程噪声是加性的，且 V 为单位阵。

    def predict(self, control_input=None):
        """
        执行EKF预测步。
        :param control_input: 控制输入，本例中我们使用状态内部的速度，因此可为None。
        """
        # 从状态中提取变量
        x, y, theta, v, omega = self.state.flatten()

        # 1. 状态预测 (基于非线性运动模型)
        new_x = x + v * self.dt * np.cos(theta)
        new_y = y + v * self.dt * np.sin(theta)
        new_theta = theta + omega * self.dt
        new_v = v  # 恒定速度模型
        new_omega = omega  # 恒定角速度模型

        self.state_pred = np.array([new_x, new_y, new_theta, new_v, new_omega]).reshape(-1, 1)

        # 2. 计算状态转移雅可比矩阵 F
        # F = df/dx，在当前状态 (x, y, theta, v, omega) 处求导
        F = np.eye(5)
        F[0, 2] = -v * self.dt * np.sin(theta)  # dx/dtheta
        F[0, 3] = self.dt * np.cos(theta)       # dx/dv
        F[1, 2] = v * self.dt * np.cos(theta)   # dy/dtheta
        F[1, 3] = self.dt * np.sin(theta)       # dy/dv
        F[2, 4] = self.dt                       # dtheta/domega
        # 注意：这里没有体现速度变化对位置预测的依赖，因为我们用的是恒定速度模型。
        # 如果使用加速度模型，F矩阵会更复杂。

        # 3. 协方差预测
        # P_{k|k-1} = F * P_{k-1|k-1} * F^T + Q
        self.P_pred = F @ self.P @ F.T + self.Q

        # 将预测值设为当前估计，为可能的更新步做准备
        self.state = self.state_pred.copy()
        self.P = self.P_pred.copy()

3.2 更新步的实现（以IMU观测为例）

更新步需要计算观测函数 h 及其雅可比矩阵 H。不同传感器对应不同的 h 和 H。我们先实现一个通用的更新函数，然后分别处理里程计和IMU的更新。

    def update_generic(self, z, H, R, hx):
        """
        通用的EKF更新步。
        :param z: 实际观测值 (列向量)
        :param H: 观测模型的雅可比矩阵 (观测维度 x 状态维度)
        :param R: 当前观测的噪声协方差矩阵
        :param hx: 观测函数 h(state_pred) 的预测值
        """
        # 1. 计算卡尔曼增益 K
        # K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
        S = H @ self.P_pred @ H.T + R
        K = self.P_pred @ H.T @ np.linalg.inv(S)

        # 2. 计算观测残差 (Innovation)
        y = z - hx  # 实际观测 - 预测观测

        # 3. 状态更新
        self.state = self.state_pred + K @ y

        # 4. 协方差更新 (Joseph form 更数值稳定)
        I = np.eye(self.P_pred.shape[0])
        self.P = (I - K @ H) @ self.P_pred @ (I - K @ H).T + K @ R @ K.T
        # 也可以使用简化形式: self.P = (I - K @ H) * self.P_pred，但上述形式更稳健。

    def update_with_imu(self, z_theta, R_imu):
        """
        使用IMU的偏航角观测进行更新。
        :param z_theta: 观测到的偏航角 (标量)
        :param R_imu: IMU观测噪声协方差 (1x1矩阵，即方差)
        """
        # 观测函数 h(x) = theta (状态向量的第三个元素)
        H = np.zeros((1, 5))
        H[0, 2] = 1  # 观测只与状态中的theta相关

        # 预测的观测值
        hx = self.state_pred[2]  # 预测的theta

        # 调用通用更新
        self.update_generic(np.array([[z_theta]]), H, R_imu, hx)

    def update_with_odom(self, z_pose, R_odom):
        """
        使用里程计的位置和朝向观测进行更新。
        :param z_pose: 观测到的位姿 [x, y, theta] (3x1 列向量)
        :param R_odom: 里程计观测噪声协方差 (3x3矩阵)
        """
        # 观测函数 h(x) = [x, y, theta]^T (状态向量的前三个元素)
        H = np.zeros((3, 5))
        H[0, 0] = 1
        H[1, 1] = 1
        H[2, 2] = 1

        # 预测的观测值
        hx = self.state_pred[:3]

        # 调用通用更新
        self.update_generic(z_pose, H, R_odom, hx)

在 update_with_imu 中，H 矩阵非常简单，因为IMU只观测偏航角。R_imu 是一个很小的数（例如 0.001），表示我们非常信任IMU提供的角度信息（假设IMU已校准且质量较好）。相反，在 update_with_odom 中，R_odom 矩阵的对角线元素（x, y, theta 的方差）可能会设置得大一些，尤其是 theta，因为轮式里程计测角度的精度通常低于IMU。

4. 仿真实验：数据生成、融合与可视化对比

理论需要实践来验证。我们将模拟一个机器人的运动轨迹，并生成带有噪声的里程计和IMU数据，然后用我们实现的EKF进行融合，最后与真实轨迹进行对比。

4.1 生成模拟数据

我们模拟一个机器人做匀速圆周运动，这样它既有位置变化也有持续的朝向变化，能很好地测试滤波器。

def generate_simulation_data(total_time=30, dt=0.1):
    """
    生成真实轨迹、带噪声的里程计观测和带噪声的IMU观测。
    """
    steps = int(total_time / dt)
    time = np.arange(0, total_time, dt)

    # 真实状态 [x, y, theta, v, omega]
    v_true = 1.0  # 线速度 1 m/s
    omega_true = 0.3  # 角速度 0.3 rad/s
    true_states = []
    true_poses = []

    # 初始状态
    state = np.array([0, 0, 0, v_true, omega_true])
    for t in time:
        # 真实运动模型积分
        x, y, theta, v, omega = state
        state[0] = x + v * dt * np.cos(theta)  # x
        state[1] = y + v * dt * np.sin(theta)  # y
        state[2] = theta + omega * dt          # theta
        # v 和 omega 保持不变（匀速圆周运动）
        true_states.append(state.copy())
        true_poses.append(state[:3].copy())

    true_states = np.array(true_states)
    true_poses = np.array(true_poses)

    # 生成带噪声的里程计观测
    # 里程计给出的是相对位移，我们这里简化为直接观测带噪声的绝对位姿
    odom_noise_std = [0.15, 0.15, 0.1]  # x, y, theta 的标准差
    odom_poses = true_poses.copy()
    for i in range(3):
        odom_poses[:, i] += np.random.randn(steps) * odom_noise_std[i]

    # 生成带噪声的IMU观测 (只观测theta)
    imu_noise_std = 0.05  # theta 的标准差，通常比里程计更准
    imu_thetas = true_poses[:, 2].copy() + np.random.randn(steps) * imu_noise_std

    return time, true_states, true_poses, odom_poses, imu_thetas, odom_noise_std, imu_noise_std

4.2 运行EKF并进行对比

现在，我们将模拟数据灌入我们实现的EKF中，并记录每一步的估计结果。

def run_ekf_simulation():
    # 生成数据
    dt = 0.1
    time, true_states, true_poses, odom_poses, imu_thetas, odom_std, imu_std = generate_simulation_data(dt=dt)

    # 初始化EKF
    # 初始猜测可以偏离真实值，以测试滤波器的收敛能力
    init_pose_guess = [0.5, -0.3, 0.2, 0.8, 0.25]  # 故意给一些误差
    init_cov = np.diag([1.0, 1.0, 0.5, 0.4, 0.2])  # 初始不确定性较大
    ekf = RobotPoseEKF(init_pose_guess, init_cov)
    ekf.dt = dt

    # 设置观测噪声协方差
    ekf.R_odom = np.diag(np.array(odom_std) ** 2)  # 3x3矩阵
    ekf.R_imu = np.array([[imu_std ** 2]])         # 1x1矩阵

    # 存储估计结果
    estimated_poses = []
    estimated_cov = []

    # 主循环
    for i in range(len(time)):
        # --- 预测步 ---
        ekf.predict()

        # --- 更新步 (模拟传感器以不同频率到达) ---
        # 假设里程计更新频率是10Hz (每步都更新)，IMU更新频率是20Hz (每两步更新一次)
        if i % 1 == 0:  # 每步都进行里程计更新
            z_odom = odom_poses[i].reshape(-1, 1)
            ekf.update_with_odom(z_odom, ekf.R_odom)

        if i % 2 == 0:  # 每两步进行一次IMU更新
            z_imu = imu_thetas[i]
            ekf.update_with_imu(z_imu, ekf.R_imu)

        # 记录当前估计
        estimated_poses.append(ekf.state[:3].flatten().copy())
        # 记录位置的不确定度（协方差矩阵对角线元素的开方，即标准差）
        pos_std = np.sqrt(np.diag(ekf.P[:3, :3]))
        estimated_cov.append(pos_std.copy())

    estimated_poses = np.array(estimated_poses)
    estimated_cov = np.array(estimated_cov)

    return time, true_poses, odom_poses, imu_thetas, estimated_poses, estimated_cov

4.3 结果可视化与分析

可视化是理解滤波器性能的关键。我们将绘制真实轨迹、纯里程计轨迹、IMU观测的朝向以及EKF融合后的轨迹。

def plot_results(time, true_poses, odom_poses, imu_thetas, estimated_poses, estimated_cov):
    fig, axes = plt.subplots(2, 2, figsize=(14, 10))

    # 1. XY平面轨迹对比
    ax = axes[0, 0]
    ax.plot(true_poses[:, 0], true_poses[:, 1], 'g-', label='真实轨迹', linewidth=2)
    ax.plot(odom_poses[:, 0], odom_poses[:, 1], 'r--', label='里程计观测', alpha=0.7, linewidth=1)
    ax.plot(estimated_poses[:, 0], estimated_poses[:, 1], 'b-', label='EKF估计轨迹', linewidth=1.5)
    ax.set_xlabel('X 位置 (m)')
    ax.set_ylabel('Y 位置 (m)')
    ax.set_title('XY平面轨迹对比')
    ax.legend()
    ax.grid(True)
    ax.axis('equal')

    # 2. 偏航角 (Theta) 随时间变化对比
    ax = axes[0, 1]
    ax.plot(time, true_poses[:, 2], 'g-', label='真实朝向', linewidth=2)
    ax.plot(time, odom_poses[:, 2], 'r--', label='里程计朝向', alpha=0.7)
    ax.plot(time[::2], imu_thetas[::2], 'm.', label='IMU观测', markersize=3, alpha=0.5) # IMU频率减半显示
    ax.plot(time, estimated_poses[:, 2], 'b-', label='EKF估计朝向', linewidth=1.5)
    ax.set_xlabel('时间 (s)')
    ax.set_ylabel('偏航角 (rad)')
    ax.set_title('朝向估计对比')
    ax.legend()
    ax.grid(True)

    # 3. X和Y方向的位置误差与3σ边界
    ax = axes[1, 0]
    x_error = estimated_poses[:, 0] - true_poses[:, 0]
    y_error = estimated_poses[:, 1] - true_poses[:, 1]
    ax.plot(time, x_error, 'b-', label='X方向误差', alpha=0.8)
    ax.plot(time, y_error, 'r-', label='Y方向误差', alpha=0.8)
    # 绘制3倍标准差边界
    ax.fill_between(time, -3*estimated_cov[:, 0], 3*estimated_cov[:, 0], color='blue', alpha=0.2, label='X 3σ边界')
    ax.fill_between(time, -3*estimated_cov[:, 1], 3*estimated_cov[:, 1], color='red', alpha=0.2, label='Y 3σ边界')
    ax.set_xlabel('时间 (s)')
    ax.set_ylabel('位置误差 (m)')
    ax.set_title('估计误差与不确定性边界')
    ax.legend()
    ax.grid(True)

    # 4. 协方差（不确定性）收敛情况
    ax = axes[1, 1]
    ax.plot(time, estimated_cov[:, 0], 'b-', label='X标准差 (σ_x)')
    ax.plot(time, estimated_cov[:, 1], 'r-', label='Y标准差 (σ_y)')
    ax.plot(time, estimated_cov[:, 2], 'g-', label='Θ标准差 (σ_θ)')
    ax.set_xlabel('时间 (s)')
    ax.set_ylabel('状态标准差')
    ax.set_title('状态估计不确定性收敛过程')
    ax.legend()
    ax.grid(True)
    ax.set_yscale('log')  # 使用对数坐标更清晰地观察收敛

    plt.tight_layout()
    plt.show()

# 运行并绘图
time, true_poses, odom_poses, imu_thetas, estimated_poses, estimated_cov = run_ekf_simulation()
plot_results(time, true_poses, odom_poses, imu_thetas, estimated_poses, estimated_cov)

运行上述代码，你会得到一组对比图表。理想情况下，你应该观察到：

• 轨迹图：EKF估计的轨迹（蓝色实线）应该比纯里程计轨迹（红色虚线）更贴近真实轨迹（绿色实线），尤其是在转弯处，里程计的转角误差会被IMU纠正。
• 朝向图：EKF估计的朝向（蓝色实线）应该紧密跟随真实朝向（绿色实线），并且比里程计提供的朝向（红色虚线）更平滑、更准确。IMU的观测点（紫色点）虽然可能有噪声，但提供了绝对的角度参考。
• 误差图：EKF的位置误差应该被控制在较小的范围内，并且大部分时间落在灰色的3σ不确定性边界内。这验证了滤波器协方差估计的有效性。
• 协方差图：状态的不确定性（标准差）应该随着时间收敛到一个较小的稳定值，这表明滤波器对状态的估计越来越有信心。使用对数坐标可以更清楚地看到初始阶段的快速收敛过程。

5. 关键参数调优与实践中的挑战

实现一个能工作的EKF只是第一步，让它在实际系统中稳定、精确地运行，才是真正的挑战。这很大程度上依赖于对几个关键参数的精心调优。

5.1 噪声协方差矩阵：Q与R

这是调优的核心。它们不是物理常数，而是反映了你对模型和传感器的“信任程度”。

• 过程噪声协方差 Q：代表你对运动模型预测的信心。如果机器人运动剧烈、模型粗糙，Q应该设得大一些。在我们的例子中，Q是一个5x5对角矩阵。通常，速度和角速度对应的噪声 (Q[3,3], Q[4,4]) 可以设得相对大一些，因为恒定速度/角速度的假设比较强。
• 观测噪声协方差 R：代表你对传感器数据的信心。传感器精度越高，R的对角线值（方差）应该越小。
  • R_odom：对于轮式里程计，x, y 的方差通常比 theta 的方差小，因为编码器测量位移相对准确，而转角容易受打滑影响。你可以通过让机器人走一个精确的矩形或圆形，对比里程计输出和真实路径来粗略标定这些方差。
  • R_imu：对于IMU的偏航角，其方差通常非常小（例如 0.001 或更小），前提是IMU经过良好的校准和零偏补偿。但要注意，低成本的IMU陀螺仪零偏会随时间漂移，这会导致积分误差，在EKF中表现为观测误差逐渐增大。因此，有时需要引入更复杂的模型或额外的算法（如零偏估计）来处理。

一个实用的调优方法是“蒙特卡洛”仿真：在仿真中设置不同的Q和R，运行大量次，选择使得平均误差最小的那组参数。然后，在真实机器人上进行微调。

5.2 处理异步与不同频率的传感器数据

在实际的ROS系统中，里程计和IMU的数据到达时间是不同步且频率不同的。我们的仿真代码简单模拟了这一点（IMU频率是里程计的两倍）。在真实实现中，你需要一个更健壮的机制：

• 维护一个状态缓冲区：每次执行预测步时，使用自上次更新以来的时间差 dt。
• 异步更新：为每个传感器设置独立的回调函数。当收到里程计消息时，执行一次预测（基于时间差），然后用里程计观测更新。当收到IMU消息时，同样执行预测，然后用IMU观测更新。关键在于，每次预测和更新后，都要更新当前的状态和时间戳，作为下一次预测的起点。

# 伪代码示例
class AsyncEKF(RobotPoseEKF):
    def __init__(self):
        super().__init__(...)
        self.last_update_time = rospy.Time.now() # 记录上次更新时间

    def odom_callback(self, odom_msg):
        current_time = odom_msg.header.stamp
        dt = (current_time - self.last_update_time).to_sec()
        if dt > 0:
            self.dt = dt
            self.predict()
            # 从odom_msg提取观测值z_odom
            self.update_with_odom(z_odom, self.R_odom)
            self.last_update_time = current_time

    def imu_callback(self, imu_msg):
        current_time = imu_msg.header.stamp
        dt = (current_time - self.last_update_time).to_sec()
        if dt > 0:
            self.dt = dt
            self.predict()
            # 从imu_msg提取观测值z_theta
            self.update_with_imu(z_theta, self.R_imu)
            self.last_update_time = current_time

5.3 从仿真到ROS：话题发布与TF树

当你对自己的EKF实现有信心后，就可以将其移植到ROS中，替代或补充 robot_pose_ekf。你需要做以下几件事：

1. 创建ROS节点：将上面的EKF类封装到一个ROS节点里。
2. 订阅传感器话题：订阅 /odom (类型 nav_msgs/Odometry) 和 /imu/data (类型 sensor_msgs/Imu)。
3. 发布融合结果：发布一个自定义的位姿话题，例如 /fused_pose (类型 geometry_msgs/PoseWithCovarianceStamped)，包含融合后的位姿和协方差。
4. 发布TF变换：这是与导航栈兼容的关键。你需要发布从 odom 帧到 base_footprint 帧（或你的机器人基座帧）的变换。变换的内容就是你EKF估计出的 (x, y, theta)。

   # 在ROS节点更新步之后
   br = tf2_ros.TransformBroadcaster()
   t = geometry_msgs.msg.TransformStamped()
   t.header.stamp = rospy.Time.now()
   t.header.frame_id = "odom"
   t.child_frame_id = "base_footprint"
   t.transform.translation.x = self.state[0]
   t.transform.translation.y = self.state[1]
   t.transform.translation.z = 0.0
   q = tf_conversions.transformations.quaternion_from_euler(0, 0, self.state[2])
   t.transform.rotation.x = q[0]
   t.transform.rotation.y = q[1]
   t.transform.rotation.z = q[2]
   t.transform.rotation.w = q[3]
   br.sendTransform(t)
   

5. 参数服务器：将 Q, R_odom, R_imu 等参数放到ROS参数服务器中，这样可以在launch文件中方便地调整，而无需修改代码。

通过这个过程，你不仅理解了EKF的原理，还拥有了一个可以根据自己机器人特性进行深度定制和优化的位姿估计模块。当你在RViz中看到自己编写的EKF节点发布的TF变换，驱动着机器人模型平滑移动，并且比单纯使用里程计或IMU时更加准确和稳定时，那种成就感是直接使用现成工具包无法比拟的。这正是在机器人技术领域深入探索的乐趣所在。