ros2 foxy robot-localization 里程计 imu多传感器融合踩坑指南

前言

Robot_localization是一个基于卡尔曼滤波 ROS的包，可以对里程计 imu gps多种传感器进行数据融合，进而完成机器人的定位的算法。对于不懂卡尔曼滤波的小伙伴，通过修改配置文件就能顺利的使用大佬们造好的轮子，极大的降低了使用多传感器定位的难度。

使用环境：

ubuntu 20.04  ros2  foxy

在线安装robot-localization

sudo apt install ros-foxy-robot-localization

robot_localization中有两个用于融合传感器数据的两种不同的算法节点ekf_localization_node和ukf_localization_node，它们的主要区别在于使用的滤波器算法不同。

运行测试

运行扩展卡尔曼滤波器（EKF）：ros2 launch robot_localization ekf.launch.py

报错 退出
[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-07-14-12-26-37-916195-ubun-10497
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ekf_node-1]: process started with pid [10499]
[ERROR] [ekf_node-1]: process has died [pid 10499, exit code -11, cmd '/opt/ros/foxy/lib/robot_localization/ekf_node --ros-args -r __node:=ekf_filter_node --params-file /opt/ros/foxy/share/robot_localization/params/ekf.yaml'].
运行无迹卡尔曼滤波器（UKF）：ros2 launch robot_localization ukf.launch.py  

报错 退出
[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-07-14-13-06-00-945589-ubun-11298
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ukf_node-1]: process started with pid [11300]
[ERROR] [ukf_node-1]: process has died [pid 11300, exit code -11, cmd '/opt/ros/foxy/lib/robot_localization/ukf_node --ros-args -r __node:=ukf_filter_node --params-file /opt/ros/foxy/share/robot_localization/params/ukf.yaml'].

不知道什么原因，我的在线安装的robot-localization工作不正常，一运行就报错退出，有知道原因的小伙伴评论区留言，没办法，只能卸载了用源码安装了。

卸载

sudo apt remove ros-foxy-robot-localization
 

源码安装robot-localization

源文件下载地址：

文件 · foxy-devel · mirrors / cra-ros-pkg / robot_localization · GitCode

根据使用的ros版本选择适合的程序下载（一定要注意下载的版本和本机的ros版本是否配套，不配套编译会过不了的），解压后放在ros工作空间src目录下。

编译

colcon build --packages-select robot_localization 

文件结构

测试运行

m@ubun:~/ros2_ws$ source install/setup.bash
ros2 launch robot_localization ekf.launch.py

能看到这个说明你安装成功了，下面该做的就是修改配置文件，把别人造好的轮子转起来。 

文件目录

先看一下我们关心的2个文件夹 launch params

ekf.launch.py    //扩展卡尔曼滤波器（EKF） 启动程序

ekf.yaml            //扩展卡尔曼滤波器配置文件        

为了不破坏源文件，我们复制一份ekf.launch.py原文件改名

odom_ekf.launch.py  并修改引用的配置文件的名字

# Copyright 2018 Open Source Robotics Foundation, Inc.
# Copyright 2019 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os
import yaml
from launch.substitutions import EnvironmentVariable
import pathlib
import launch.actions
from launch.actions import DeclareLaunchArgument

def generate_launch_description():
    return LaunchDescription([
        launch_ros.actions.Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[os.path.join(get_package_share_directory("robot_localization"), 'params', 'odom_ekf.yaml')], 
#只是这里把ekf.yaml修改成了odom_ekf.yaml 
           ),
])

复制一份ekf.yaml原文件改名

odom_ekf.yaml  翻译备注并修改文件内容

### ekf config file ###
###ekf配置文件###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.

# 滤波器输出位置估计的频率，单位为Hz。请注意，过滤器在从其中一个输入接收到至少一条消息之前不会开始计算。
# 然后，无论是否接收到更多的测量值，它都将以此处指定的频率连续运行。如果未指定，则默认为30。        
        frequency: 30.0

# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.

# 我们认为传感器超时的时间段（以秒为单位）。在这种情况下，我们在EKF上执行一个预测周期，而不进行校正。
# 这个参数可以被认为是滤波器产生新输出的最小频率。如果未指定，则默认为1/频率。
        sensor_timeout: 0.1

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.

# ekf_localization_node and ukf_localization_node都使用3D全向运动模型。
# 如果此参数设置为true，则不会在状态估计中使用任何三维信息。
# 如果您在平面环境中操作，并且希望忽略地平面中可能检测到的微小变化的影响，请使用此选项
# 例如通过IMU。如果未指定，则默认为false。 
        two_d_mode: true

# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.

# 使用此参数可以为ekf_location_node生成的变换提供偏移。这可以用于未来的日期转换，这是与其他一些包交互所必需的。
# 如果未指定，则默认为0.0。
        transform_time_offset: 0.0

# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
# Defaults to 0.0 if unspecified.

#使用此参数可以指定tf侦听器应等待转换可用的时间。
#如果未指定，则默认为0.0。
        transform_timeout: 0.0

# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.

#如果遇到问题，请尝试将其设置为true，然后echo the /diagnostics_agg主题，查看节点是否对任何设置或数据不满意
        print_diagnostics: true

# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.

# 调试设置。不适合胆小的人。向debug_out_file指定的文件输出数量惊人的信息。我希望你喜欢矩阵！
# 请注意，将此设置为true将对节点的性能产生严重的有害影响。如果未指定，则默认为false。
        debug: false

# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.

#如果未指定，则默认为“robot_location_debug.txt”。请指定完整路径。
        debug_out_file: /path/to/debug/file.txt

# Whether we'll allow old measurements to cause a re-publication of the updated state
# 我们是否允许旧的测量结果导致更新状态的重新发布
        permit_corrected_publication: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
 #是否发布加速状态。如果未指定，则默认为false。
        publish_acceleration: false

# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
#是否在/tf主题上广播转换。如果未指定，则默认为true。
        publish_tf: false

# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:

# REP-105(http://www.ros.org/reps/rep-0105.html)指定四个主要坐标系：base_link、odom、map和earth。
# base_link是固定在机器人上的坐标系。odom和map都是world-fixed frames。
# 机器人在odom框架中的位置会随着时间的推移而漂移，但在短期内是准确的，应该是连续的。因此，odom帧是用于执行局部运动计划的最佳帧。
# 与odom框架一样，map框架是一个世界固定的坐标框架，虽然它包含了机器人最全局准确的位置估计，但它会受到离散跳跃的影响，
# 例如，由于GPS数据的融合或基于地图的定位节点的校正。地球坐标系用于通过给多个地图坐标系提供一个公共参考坐标系来关联它们。
# ekf_location_node和ukf_localization_node与map帧无关。

#以下是如何使用以下设置：
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
#         odom_frame.
# 1.将map_frame、odom_frame和base_link帧设置为系统的适当帧名称。
#     1a. 如果您的系统没有map_frame，只需将其删除，并确保“world_frame”设置为odom_frame的值。
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 2. 如果要融合连续位置数据，如车轮编码器里程计、视觉里程计或IMU数据，请将“world_frame”设置为odom_frame值。这是robot_location的状态估计节点的默认行为。
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
# 3. 如果您正在融合受离散跳跃影响的全球绝对位置数据（例如，GPS或地标的位置更新观察），然后：    
#     3a. 将“world_frame”设置为map_frame值
#     3b. 确保其他东西正在生成odom->base_link变换。请注意，这甚至可以是robot_location的另一个状态估计节点！但是，该实例不应该融合全局数据。

        map_frame: map              # 如果未指定，则默认为“map”
        odom_frame: odom            # 如果未指定，则默认为"odom"
        base_link_frame: base_link  # 如果未指定，则默认为"base_link"
        world_frame: odom           # 如果未指定，则默认为"odom"

# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.

# 过滤器接受来自每个输入消息类型的任意数量的输入（nav_msgs/Odometry、geometry_msgs/PoseWithCovarianceStamped、
# geometry_msgs/TwistWithCovariance Stamped,sensor_msgs/Imu）。
#要添加输入，只需将序列中的下一个数字附加到其“base”名称，例如odom0、odom1、twist0、twist1、imu0、imu1、imu2等。
#该值应为topic名称。这些参数显然没有默认值，必须指定。
        odom0: /odom_diff

# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.

# 每个传感器读数都会更新过滤器的部分或全部状态。这些选项使您能够更好地控制将每个测量值输入到过滤器的值。
# 例如，如果您有里程计消息作为输入，但只想使用其Z位置值，则将整个矢量设置为false，第三个条目除外。
# 值的顺序为x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。
# 请注意，并非某些消息类型不提供过滤器估计的某些状态变量。例如，TwistWithCovarianceStamped消息没有姿势信息，
# 因此在这种情况下，前六个值将毫无意义。如果未指定，则每个向量默认为全假，从而有效地使每个传感器都需要此参数。
        odom0_config: [false, false, false, #x-y-z坐标系的坐标（机器人位置）
                       false, false, false, #绕x/y/z轴的角度（机器人方向）、
                       true, false, false,  #沿x/y/z轴的线速度、
                       false, false, true,  #绕x/y/z轴的角速度、
                       false, false, false] #沿x/y/z轴的加速度。

# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.

#如果您有高频数据或使用低频参数值运行，则可能需要增加订阅队列的大小，以便融合更多的测量值。
        odom0_queue_size: 20

# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.

# [高级]ROS中的大消息在高频到达时会表现出奇怪的行为。这是Nagle算法的结果。
# 此选项告诉ROS订户使用tcpNoDelay选项，该选项禁用Nagle的算法。
        odom0_nodelay: false

# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.

# [高级]当用两个传感器测量一个姿态变量时，可能会出现两个传感器都报告不足的情况。这可能导致滤波器在每次测量到达时快速来回跳跃。
# 在这些情况下，（a）校正测量协变量，或者（b）如果其中一个传感器也测量速度，则让一个传感器测量姿态，而另一个传感器则测量速度，
# 这通常是有意义的。然而，做（a）或（b）并不总是可行的，因此我们公开了微分参数。启用差分模式时，通过对绝对姿态测量值进行微分，
# 将所有绝对姿态数据转换为速度数据。然后像往常一样对这些速度进行积分。注：这仅适用于提供姿态测量的传感器；
# 将twist测量的微分设置为true没有效果。
        odom0_differential: false

# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.

# [ADVANCED]当节点启动时，如果此参数为真，则第一次测量将被视为所有未来测量的“零点”。虽然你可以用微分参数计获得同样的效果，
# 但关键的区别在于，相对参数不会导致测量在积分之前转换为速度。如果你只是想让给定传感器的测量从0开始，请将其设置为true。
        odom0_relative: false

# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.

# [ADVANCED]如果您的数据存在异常值，请使用这些阈值设置（表示为Mahalanobis距离）来控制允许传感器测量距离当前车辆状态的距离。
# 如果未指定，则每个阈值都默认为numeric_limits＜double＞：：max（）。如果不需要，强烈建议删除这些参数。
# 数据是在姿势和扭曲变量级别指定的，而不是单独为每个变量指定的。对于同时具有姿势和扭曲数据的消息，
# 该参数指定我们将阈值应用于消息的哪一部分。
        odom0_pose_rejection_threshold: 5.0
        odom0_twist_rejection_threshold: 1.0

# Further input parameter examples
# 进一步的输入参数示例
        # odom1: example/odom2
        # odom1_config: [false, false, true,
        #                false, false, false,
        #                false, false, false,
        #                false, false, true,
        #                false, false, false]
        # odom1_differential: false
        # odom1_relative: true
        # odom1_queue_size: 2
        # odom1_pose_rejection_threshold: 2.0
        # odom1_twist_rejection_threshold: 0.2
        # odom1_nodelay: false
        # pose0: example/pose
        # pose0_config: [true,  true,  false,
        #                false, false, false,
        #                false, false, false,
        #                false, false, false,
        #                false, false, false]
        # pose0_differential: true
        # pose0_relative: false
        # pose0_queue_size: 5
        # pose0_rejection_threshold: 2.0  # 注意参数名称的差异
        # pose0_nodelay: false

        # twist0: example/twist
        # twist0_config: [false, false, false,
        #                 false, false, false,
        #                 true,  true,  true,
        #                 false, false, false,
        #                 false, false, false]
        # twist0_queue_size: 3
        # twist0_rejection_threshold: 2.0
        # twist0_nodelay: false

        imu0: /imu_data

        imu0_config: [false, false, false,   #x-y-z坐标系的坐标（机器人位置）
                      false, false, false,  #绕x/y/z轴的角度（机器人方向）
                      false, false, false,   #沿x/y/z轴的线速度
                      false,  false,  true,     #绕x/y/z轴的角速度
                      true, true, false]    #沿x/y/z轴的加速度              
        imu0_nodelay: false
        imu0_differential: false
        imu0_relative: false
        imu0_queue_size: 20
        imu0_pose_rejection_threshold: 0.8                 # 注意参数名称的差异
        imu0_twist_rejection_threshold: 0.8                #
        imu0_linear_acceleration_rejection_threshold: 0.8  #

# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.

# [高级]一些IMU会自动消除重力造成的加速度，而另一些则不会。如果您的数据不符合，请将其设置为true，
# 并*确保*您的数据符合REP-103，特别是数据在ENU框架中。
        imu0_remove_gravitational_acceleration: false

# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.

# [高级]EKF和UKF模型遵循标准的预测/校正周期。在预测期间，如果没有加速度参考，则简单地将时间t+1处的速度预测为与时间t处的速度相同。
# 在校正期间，将该预测值与测量值融合以产生新的速度估计。这可能是有问题的，因为最终速度实际上是旧速度和新速度的加权平均值。
# 当这个速度被整合到一个新的姿势中时，结果可能是缓慢的隐蔽。这种效果在旋转过程中的激光雷达数据中尤为明显。为了解决这个问题，
# 用户可以尝试为有问题的速度变量增加process_noise_covariance，或者在测量本身中减少有问题的变量的方差。
# 此外，用户还可以利用在我们进行预测时向机器人发出的控制命令。如果使用控制，它将被转换为加速项，该加速项将在预测过程中使用。
# 请注意，如果从其中一个输入中可以获得有关变量的加速度测量值，则控制项将被忽略。
# 无论我们是否在预测期间使用控制输入。默认为false。
        use_control: false
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.

#输入（假定为cmd_vel）是geometry_msgs/Twist还是geometry_msgs/TwistStamped消息。默认为false。
        stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.

#最后发布的控制命令将用于该时段的预测。默认值为0.2。
        control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.

#正在控制哪些速度。顺序是vx、vy、vz、vroll、vpitch、vyaw。
        control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.

# 限制加速度项的大小。应与机器人的运动学相匹配。
        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.

#机器人的加速和减速限制并不总是相同的。
        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains

#如果你的机器人不能立即达到其加速度极限，那么可以通过这些增益来控制允许的变化
        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains

# 如果你的机器人不能立即达到减速极限，那么允许的变化可以用这些增益来控制
        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [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.

# [ADVANCED]过程噪声协方差矩阵可能很难调整，并且可能因每个应用而变化，因此它被公开为配置参数。
# 这个矩阵表示我们在每个预测步骤之后添加到总误差中的噪声。全向运动模型与系统匹配得越好，这些值就越小。
# 然而，如果用户发现给定的变量收敛较慢，一种方法是增加该变量的process_noise_covariance对角值，这将导致滤波器的预测误差更大，
# 这将使得滤波器在校正期间更加信任传入的测量。值的顺序为x、y、z、滚转、俯仰、偏航、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。
# 如果未指定，则默认为以下矩阵。
        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.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,    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,    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,    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,    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,    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,
                                   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,    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,    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,    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,    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,    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,    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,     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.

# [ADVANCED]这表示状态估计误差协方差矩阵的初始值。将对角线值（方差）设置为大值将导致所讨论变量的初始测量的快速收敛。
# 用户应注意不要对不会直接测量的变量使用大值。
# 用户应注意不要对不会直接测量的变量使用大值。
# 值的顺序为x、y、z、滚转、俯仰、偏航、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。
# 如果未指定，则默认为以下矩阵。
        initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.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.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.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.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.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.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.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.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.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.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.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.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.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.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]

使用robot_localization只需要：

配置文件odom_ekf.yaml

launch文件odom_ekf.launch.py

运行我们自己的launch文件

m@ubun:~/ros2_ws$ source install/setup.bash
ros2 launch robot_localization odom_ekf.launch.py

 程序运行成功。

运行robot_localization 需要准备的数据

里程计：

​​​​​​​nav_msgs/Odometry
geometry_msgs/PoseWithCovarianceStamped
geometry_msgs/TwistWithCovarianceStamped

imu：
sensor_msgs/Imu

查看里程计数据属性（为了减少占用篇幅长度去掉了covariance协方差里面的n个0）

ros2 interface proto nav_msgs/msg/Odometry
"header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
child_frame_id: ''
pose:
  pose:
    position:        #x-y-z坐标系的坐标（机器人位置）
      x: 0.0
      y: 0.0
      z: 0.0          
    orientation:   #四元数
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance:      #协方差
  - 0.0
twist:
  twist:
    linear:             #沿x/y/z轴的速度
      x: 0.0
      y: 0.0           
      z: 0.0            
    angular:          #绕x/y/z轴的角速度
      x: 0.0            
      y: 0.0            
      z: 0.0
  covariance:       #协方差
  - 0.0
"

我们要根据上面的Odometry数据给robot_localization填充数据

所有状态估计节点跟踪车辆的15维状态： (X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨)

(X,Y,Z,  x-y-z        坐标系的坐标（机器人位置）、
roll,pitch,yaw,       绕x/y/z轴的角度（机器人方向）、
yaw,X˙,Y˙,Z˙,       沿x/y/z轴的速度、
roll˙,pitch˙,yaw˙,  绕x/y/z轴的角速度、
X¨,Y¨,Z¨               沿x/y/z轴的加速度。

#需要把小车发布的/odom话题改成/odom_diff
#如果我们不用robot_localization融合数据，小车里程计发布的/odom话题被建图或者导航程序直接接收
#我们用robot_localization融合数据，小车里程计发布的/odom_diff话题通过robot_localization
#接收转化成/odometry/filtered发布出来，通过remappings=[('odom', #'/odometry/filtered'),], 做映射被建图或者导航程序接收。

odom0: /odom_diff  

        odom0_config: [false, false, false, #x-y-z坐标系的坐标（机器人位置）
                       false, false, false, #绕x/y/z轴的角度（机器人方向）、
                       true, false, false,  #沿x/y/z轴的线速度、
                       false, false, true,  #绕x/y/z轴的角速度、
                       false, false, false] #沿x/y/z轴的加速度。

 查看imu数据接口属性：

 m@ubun:~$ ros2 interface proto sensor_msgs/msg/Imu
"header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: ''
orientation:                  //四元数方向
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
orientation_covariance:  //协方差
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:       //绕x/y/z轴的角速度
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:    //沿x/y/z轴的加速度
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
"

imu0: /imu_data

        imu0_config: [false, false, false,   #x-y-z坐标系的坐标（机器人位置）
                      false, false, false,  #绕x/y/z轴的角度（机器人方向）
                      false, false, false,   #沿x/y/z轴的线速度
                      false,  false,  true,     #绕x/y/z轴的角速度
                      true, true, false]    #沿x/y/z轴的加速度  

 这个config可以按行置换成true测试，我试了有的换成true能工作，有的不能工作，因为我们的话题提供的数据不能完全包含这15个维度，只有我们话题里面包含的数据填充成true才有效，没有的数据用false。

 只用imu节点图

只用里程计节点图 

 

同时用imu 和 里程计节点图 

 cartographer.launch.py建图需要接收/odom话题，这个话题可以由里程计提供，也可以由robot_localization的/odometry/filtered话题映射成/odom在被建图或者导航程序接收。

总结

robot_localization的作用就是把接收的 里程计  imu gps 的话题通过融合发布/odometry/filtered话题，最后把这个话题映射成/odom供建图或者导航使用。