瞭解ROS的robot_pose_ekf軟件包中擴展卡爾曼濾波器的用法：
robot_pose_ekf軟件包用於基於來自不同來源的（部分）位姿測量值來估計機器人的3D姿勢。它使用帶有6D模型（3D位置和3D方向）的擴展卡爾曼濾波器，將來自車輪里程計，IMU傳感器和視覺里程計的測量結果結合起來。基本思想是提供與不同傳感器的鬆散耦合集成，其中傳感器信號以ROS消息的形式接收。
主頁：http://ros.org/wiki/robot_pose_ekf
Github：https://github.com/ros-planning/robot_pose_ekf
robot_pose_ekf軟件包可用於融合里程計，慣性測量單元和視覺里程計的傳感器輸出，從而減少測量中的總體誤差。
1 如何使用
1.1 配置
可以在robot_pose_ekf軟件包目錄中找到EKF節點的默認啓動文件（launch）。啓動文件包含許多可配置的參數：
freq：濾波器更新和發佈頻率（注意：較高的頻率僅僅意味着一段時間可以獲得更多機器人位姿信息，但是並不可以提高每次位姿估計的精度）sensor_timeout：當傳感器停止向濾波器發送信息時，濾波器在沒有傳感器的情況下等待多長時間才重新開始工作odom_used,imu_used,vo_used：確認是否輸入
可以在啓動文件中修改配置，如下所示：
<launch>
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="output_frame" value="odom"/>
<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="true"/>
<param name="debug" value="false"/>
<param name="self_diagnose" value="false"/>
</node>
</launch>
1.2 運行
（1）編譯
$ rosdep install robot_pose_ekf
$ roscd robot_pose_ekf
$ rosmake
（2）運行
$ roslaunch robot_pose_ekf.launch
2 節點
2.1 訂閱的話題
-
odom(nav_msgs/Odometry)
2D pose (used by wheel odometry) ：該2D pose包含了機器人在地面的位置（position）和方位（orientation）信息以及該位姿的協方差（covariance）。用來發送該2D位姿的消息實際上表示了一個3D位姿，只不過把z，pitch和roll分量簡單忽略了。 -
imu_data(sensor_msgs/Imu)
3D orientation (used by the IMU)：3D方位提供機器人底座相對於世界座標系的Roll，Pitch和Yaw信息。 Roll和Pitch角是絕對角度（因爲IMU具有重力參考），而Yaw角是相對角度。 協方差矩陣用來指定方位測量的不確定度。當僅僅收到這個話題消息時，robot_pose_ekf不會啓動，因爲它還需要來自話題vo或者odom的消息。 -
vo(nav_msgs/Odometry)
3D pose (used by Visual Odometry)：3D位姿可以完整表示機器人的位置和方位，以及該位姿的協方差。當傳感器只測量3D位姿的一部分(e.g. the wheel odometry only measures a 2D pose)時, 可以給3D位姿沒有實際測量的部分指定一個較大的協方差。
robot_pose_ekf節點不需要所有三個傳感器源始終都可用。每個源給出一個位姿估計和一個協方差。這些源以不同的速率和不同的延遲運行。源會隨時間出現或消失，節點將自動檢測並使用可用的傳感器。要添加自己的傳感器輸入，請查看教程：添加GPS傳感器。
總結：
- Odometry message contains the 2D pose, ie x, y and yaw angle
- IMU message contains the 3D orientation, ie. roll, pitch and yaw angles.
- VO message contains the full 3D pose, ie. x,y,z and roll, pitch, yaw angles.
2.2 發佈的話題
robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
濾波器的輸出（估計的機器人3D位姿）。
注意：/odom和/robot_pose_ekf/odom_combined消息類型不同,前者是nav_msgs/Odometry，後者是geometry_msgs/PoseWithCovarianceStamped 。兩者的區別：後者的內容是前者的一部分。
將geometry_msgs/PoseWithCovarianceStamped轉換爲nav_msgs/Odometry：
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
class OdomEKF():
def __init__(self):
# Give the node a name
rospy.init_node('odom_ekf', anonymous=False)
# Publisher of type nav_msgs/Odometry
self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
# Wait for the /odom_combined topic to become available
rospy.wait_for_message('input', PoseWithCovarianceStamped)
# Subscribe to the /odom_combined topic
rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
rospy.loginfo("Publishing combined odometry on /odom_ekf")
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.header.frame_id = '/odom'
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
if __name__ == '__main__':
try:
OdomEKF()
rospy.spin()
except:
pass
2.3 提供的tf轉換
odom_combined → base_footprint
3 如何工作
3.1 基本流程
- 從每個傳感器獲取數據
- 檢查它們是否有效
- 如果它們有效，則將它們相對於參考基準座標系進行轉換
- 當獲得傳感器信息時，它將被存儲，直到所有傳感器的信息可用爲止。收到的每個數據都有自己的時間戳
- 一旦所有數據都可用，則在所有傳感器數據均可用時，針對每個可用傳感器數據更新擴展卡爾曼濾波器（在Orocos-BFL庫中定義）。即：如果來自里程計的數據在時間t_0（> 0）可用，則來自imu的數據在時間t_1（> t_0）處獲得，而來自視覺里程計的數據在時間t_2（> t_1）處獲得，則在時間t_1對所有三組數據進行濾波。
- 該融合的傳感器數據被轉換爲里程計消息，並在話題
/odom_combined上發佈
3.2 位姿解釋
給濾波器節點提供信息的所有傳感器源都有自己的參考座標系，並且隨着時間推移都可能出現漂移現象。因此，每個傳感器發出來的絕對位姿不能直接對比。 因此該節點使用每個傳感器的相對位姿差異來更新擴展卡爾曼濾波器。
3.3 協方差解釋
隨着機器人的移動，其位姿的不確定性越來越大。隨着時間的流逝，協方差將無限增長。因此，在位姿本身上發佈協方差是沒有用的，而是傳感器源發佈協方差如何隨時間變化，即速度的協方差。請注意，使用對世界的觀測（例如，測量到已知牆壁的距離）將減少機器人位姿的不確定性；但是，這是定位而不是里程計。
3.4 Timing
假定機器人上次更新位姿濾波器在t_0時刻， 該節點只有在收到每個傳感器測量值（時間戳>t_0）之後纔會進行下一次的濾波器更新。 例如，在odom話題收到一條消息時間戳（t_1 > t_0）, 且在imu_data話題上也收到一條消息（ 其時間戳t_2 > t_1 > t_0）, 濾波器將被更新到所有傳感器信息可用的最新時刻，這個時刻是t_1。 在t_1時刻odom位姿直接給出了，但是imu位姿需要通過在t_0和t_2兩時刻之間進行線性插值求得。在t_0到 t_1時間段，機器人位姿濾波器使用odom和IMU相對位姿進行更新。
上圖顯示了PR2機器人從給定的初始位置（綠點）啓動，四處行駛並返回初始位置時的實驗結果。完美的里程計x-y圖應顯示出精確的迴路閉合。藍線顯示來自車輪里程計的輸入，藍點表示估計的終點位置。紅線顯示robot_pose_ekf的輸出，該輸出結合了車輪里程計和imu的信息，紅點表示估計的最終位置。
4 源碼解讀
- https://blog.csdn.net/zhxue_11/article/details/83828877
- https://blog.csdn.net/weixin_42048023/article/details/84550630
- https://blog.csdn.net/shoufei403/article/details/102655696
5 總結
首先表明，通過測試，這個包可以用，而且效果還不錯。但是，有些注意事項和踩坑環節，過不去就容易放棄，甚至說它無用。而且，這個高度依賴ROS和TF，雖然方便，但是如果要自己整合或者剝離ROS，也需要下一番功夫。
立個flag：接下來學一學ros_localization~