记录使用robot_localization进行融合
下载地址：git clone https://gitee.com/bingshuibuliang/robot_localization.git
注意：/odometry/filtered是这个节点发送的融合位姿，修改的话需要在ekf_nodelet_template.launch里，在使用robot_localization实现imu和odom融合前需要发布tf关系
<remap from="odometry/filtered" to="odom"/>
使用ekf_nodelet_template.launch，修改ekf_template.yaml参数，在params文件夹
bash
frequency: 50
silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true # 地面机器人必须开
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
permit_corrected_publication: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# ==============================
# 里程计（只融合 x y 位置 + 线速度 vx）
# ==============================
odom0: /ranger_base_node/odom
odom0_config: [true, true, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 5
odom0_differential: false
odom0_relative: false
# ==============================
# IMU（只融合 姿态 + 角速度，完美修正里程计角度）
# ==============================
imu0: /imu/data
imu0_config: [false, false, false,
true, true, true, # 融合 roll pitch yaw（角度由IMU提供！）
false, false, false,
false, false, true, # 只融合yaw角速度
false, false, false] # 不融合加速度（避免震动干扰）
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
# ==============================
# 关闭控制输入（避免干扰融合）
# ==============================
use_control: false
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]*publish_tf**：如果为 true，状态估计节点会将 world_frame 参数指定的帧的变换发布到其子节点。如果world_frame与map_frame相同，它将发布从map_frame到odom_frame的变换，如果world_frame与odom_frame相同，它将发布从odom_frame到base_link_frame的变换。默认为 true。 **Coordinate frame specification**: - "map_frame"、"odom_frame"和"base_link_frame"的默认值分别是"map"、"odom"和"base_link"。"world_frame"参数默认为"odom_frame"的值。 - 如果您的系统没有map_frame，只需删除或注释它，并确保"world_frame"设置为"odom_frame"的值。 - 如果您仅融合连续位置数据，例如车轮编码器里程计、视觉里程计或 IMU 数据，请将"world_frame"设置为"odom_frame"的值。这是 robots_localization 中状态估计节点的默认行为，也是它最常见的用途。这不是我们的情况，因为我们还使用不连续的 GPS 数据。 - 如果您正在融合受离散跳跃影响的全球绝对位置数据（例如，GPS 或来自地标观测的位置更新），则将"world_frame"设置为"map_frame"的值。还要确保其他东西正在生成 odom->base_link 变换。这甚至可以是 robots_localization 状态估计节点的另一个实例。但是，该实例不应融合全局数据。这是我们的案例，因为我们使用 GPS 数据。 sensor：对于每个传感器，您需要根据传感器发布的主题定义此参数。 sensor_config：由布尔 5×3 矩阵定义如下： ``` bash X, Y, Z, Roll, Pitch, Yaw, dX/dt, dY/dt, dZ/dt, dRoll/dt, dPitch/dt, dYaw/dt, d2X/dt2, d2Y/dt2, d2Z/dt2