robot_pose_ekf是ROS Navigation stack中的一個包，通過擴展卡爾曼濾波器對imu、裏程計odom、視覺裏程計vo的數據進行融合，來估計平面移動機器人的真實位置姿態，輸出odom_combined消息。robot_pose_ekf只適用於平面上的輪式移動機器人，因此odom信息中的z，pitch和roll分量可以被忽略。IMU可以提供車體坐標系相對於世界坐標系的姿態（RPY角），其中Roll和Pitch是絕對角度，因為有重力方向作為參考，而偏航角Yaw則是一個相對角度（如果IMU中沒有集成電子羅盤測量地球磁場角作為參考）。IMU姿態的協方差矩陣代表了姿態測量的不確定度。

　　robot_pose_ekf默認監聽的topic為：imu_data、odom和vo，因此要註意發布消息時topic的名稱要對應，否則會起不到濾波作用（The robot_pose_ekf node does not require all three sensor sources to be available all the time. A source can appear and disappear over time, and the node will automatically detect and use the available sensors）。不想使用默認名稱的話可以用remap元素進行名稱重映射。robot_pose_ekf.launch文件如下：

 <launch>
      <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom_combined"/>
        <param name="base_footprint_frame" value="base_footprint"/>
        <param name="freq" value="30.0"/>
        <param 
 
name="sensor_timeout" value="1.0"/>
        <param name="odom_used" value="true"/>
        <param name="imu_used" value="true"/>
        <param name="vo_used" value="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>
    </node>
 </launch>

　　參數說明如下：

• freq：濾波器更新和發布頻率。註意頻率高僅僅意味著一段時間可以獲得更多機器人位姿信息，但是並不表示可以提高位姿估計的精度。

• sensor_timeout：當傳感器停止向濾波器發送信息時，濾波器在沒有傳感器的情況下等待多長時間才重新開始工作。

• odom_used, imu_used, vo_used：確認是否輸入。

• output_frame, base_footprint_frame：用於指定輸出tf變換中坐標系的名字，默認為odom_combined和base_footprint。

Published Topics

• robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)： The output of the filter (the estimated 3D robot pose)

　　rostopic list命令可以查看ros中的topic，下圖中白色的/robot_pose_ekf/odom_combined話題就是robot_pose_ekf節點發布的：

Provided tf Transforms

• odom_combined → base_footprint

　　robot_pose_ekf在輸出odom_combined信息同時還會發布相關的坐標變換，輸入下面指令查看tf變換關系：

rosrun rqt_tf_tree rqt_tf_tree

　　可以看出robot_pose_ekf節點會發布base_footprint坐標系相對於odom_combined坐標系的變換：

　　參數output_frame和base_footprint_frame默認為odom_combined和base_footprint，也可以根據需要在launch文件中進行更改，比如分別改為odom和base_link，再次用rqt_tf_tree命令查看，如下圖所示：

　　robot_pose_ekf節點默認會從odom、imu_data、vo這三個topic上訂閱消息，可以使用remap將其映射到新名稱的topic上。重映射是基於替換的思想，每個重映射包含一個原始名稱和一個新名稱。每當節點使用重映射中的原始名稱時，ROS客戶端庫就會將它默默地替換成其對應的新名稱。

 <launch>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom"/>
        <param name="base_footprint_frame" value="base_link"/>
        <param name="freq" value="30.0"/>
        <param name="sensor_timeout" value="1.0"/>
        <param name="odom_used" value="true"/>
        <param name="imu_used" value="true"/>
        <param name="vo_used" value="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>

        <remap from="imu_data" to="imu" />      <!-- 將節點訂閱的imu_data話題改名為imu -->
    </node>
 </launch>

　　下面在VREP中進行仿真測試：將移動機器人目錄中的lumibot模型拖入場景中，並將加速度計和陀螺儀安裝在機器人上，並在lumibot_body上添加一個腳本，發布裏程計和imu信息（可以在發布的信息上添加一些噪聲）。註意使用robot_pose_ekf進行濾波時傳感器的協方差矩陣信息不能空著，否則可能會出現錯誤，因此要設置合理的值。

　　imu數據的協方差矩陣設置可以參考：https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]

　　底盤運動時odom的協方差矩陣如下，參考：https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py

ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                        0, 1e-3, 0, 0, 0, 0,
                        0, 0, 1e6, 0, 0, 0,
                        0, 0, 0, 1e6, 0, 0,
                        0, 0, 0, 0, 1e6, 0,
                        0, 0, 0, 0, 0, 1e3]

ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                         0, 1e-3, 0, 0, 0, 0,
                         0, 0, 1e6, 0, 0, 0,
                         0, 0, 0, 1e6, 0, 0,
                         0, 0, 0, 0, 1e6, 0,
                         0, 0, 0, 0, 0, 1e3]

　　註意imu信息的協方差矩陣中代表機器人航向角的分量方差為1e-6，而裏程計信息的協方差矩陣中機器人姿態分量的協方差為1e3，兩個值相差很大。在進行EKF融合時，會更“相信”imu提供的姿態信息，因為其方差更小。比如機器人在轉動過程中輪子發生了打滑，用編碼器推算出的姿態一直在旋轉，而實際姿態（主要由IMU測量得到）卻沒發生太大變化，這種情況就需要使用信息融合方法來減小誤差。協方差矩陣中的參數設置非常重要，要根據傳感器手冊或者實際使用測量來確定。

　　VREP腳本中可以訂閱odom_combined消息，在回調函數中設置odomCombined相對於odom的位置，然後通過Graph記錄下來，繪制出軌跡曲線。

function subscriber_callback(msg)
    -- This is the subscriber callback function
    simSetObjectPosition(odomCombinedHandle,odomHandle,{msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z})
end


if (sim_call_type==sim_childscriptcall_initialization) then
　　 
　　subscriber = simExtRosInterface_subscribe(‘/robot_pose_ekf/odom_combined‘,‘geometry_msgs/PoseWithCovarianceStamped‘,‘subscriber_callback‘)
   simExtRosInterface_subscriberTreatUInt8ArrayAsString(subscriber)

end

　　運行robot_pose_ekf.launch文件，然後開始VREP仿真。下圖中藍色的曲線是使用robot_pose_ekf融合後的機器人運動軌，紅色為原始的帶噪聲的軌跡曲線（這只是一個例子，實際效果怎麽樣還要調整各種參數）：

　　另外需要註意的是，robot_pose_ekf會發布base_link到odom的tf變換，因此我們自己的程序中就不用發布了，否則會出現沖突（在tf樹中是不能構成回路的，只能有一個父坐標系，但是可以有很多子坐標系 ）。下圖是仿真過程中rivz顯示的原始odom（黃色箭頭）和融合後的odom_combined（紅色箭頭）信息，以及base_link坐標系和odom坐標系間的變換關系。

參考：

robot_pose_ekf

robot_pose_ekf 使用說明

V-rep中的加速度計與陀螺儀

ROS中測試機器人裏程計信息

robot_pose_ekf with an external sensor

ROS與navigation教程-robot_pose_ekf介紹

ROS下robot_pose_ekf擴展卡爾曼融合包的使用

航向姿態參考系統與慣性測量單元的聯系與區別

使用robot_pose_ekf對傳感器信息融合