最近在用robot_pose_ekf想做一个UWB融合定位,所以思路就是把GPS输入替换成UWB输入就行了,于是开始在网上搜索如何在robot_pose_ekf包中加入GPS。
一开始看官网教程,还有一些CSDN的朋友写的教程,发现基本都是通过robot_pose_ekf三个基本输入中odom,imu,vo中的三维视觉里程计VO来实现融合的,就是把GPS数据经过UTM转换再输入VO中,这导致我一直以为robot_pose_ekf包只能接收三路数据输入。
但是这种方法是不正确的,后来有一位ROS元老告诉我其实包里是写了GPS输入的。然后我就去看github的源码,发现源码,还有中有一个附带的launch文件叫做example_with_gps.launch,它们都告诉我其实robot_pose_ekf可以接收4路输入分别是odom/imu/vo/gps。
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<param name="gps_used" value="true"/>
这些是所有的参数,其实想要加入GPS数据只需要将GPS先UTM转换成nav_msgs/Odometry
类型然后将topic
设定成与robot_pose_ekf需要的/gps
就行了。
使用错误的方法完成融合会导致以下问题(用VO作为GPS输入)
1、假如你有odom
的输入,那么两者都存在POSE坐标,robot_poes_ekf先接收谁的数据就会以谁的数据为基准,即使TF不同比如GPS告诉它坐标是100,100,然而odom告诉它坐标是0,0,那么加入先收到odom,那么输出的数据就不会是我们期望的100,100(假设这是准确的)。
2、odom和GPS融合方法和odom和vo的融合方法是不一样的,odom和vo融合近似于一个基于协方差的加权;但是GPS和odom的融合是GPS数据为基础,odom数据辅助修正。(两个方法是我理解推测的,具体源码的实现不在EKF的包中,而在BFL包中,过于复杂我还没有看完,累。)
不一样的融合方法导致用vo替代GPS会导致输出我们不期望看到的结果。
而正确的开启GPS数据融合能够保证最后的融合数据不出现这种BUG。