robot_pose_ekf基本流程:
从每个传感器获取数据
检查它们是否有效,如果它们有效,则将它们相对于参考基准坐标系进行转换
当获得传感器信息时,它将被存储,直到所有传感器的信息可用为止。收到的每个数据都有自己的时间戳
一旦所有数据都可用,则在所有传感器数据均可用时,针对每个可用传感器数据更新扩展卡尔曼滤波器(在Orocos-BFL库中定义)。即:如果来自里程计的数据在时间t_0(> 0)可用,则来自imu的数据在时间t_1(> t_0)处获得,而来自视觉里程计的数据在时间t_2(> t_1)处获得,则在时间t_1对所有三组数据进行滤波。
该融合的传感器数据被转换为里程计消息,并在话题/odom_combined上发布