03-数据融合-ROS轮式机器人数据融合-GPS

1. 可行的主题

Robot Pose EKF节点订阅下面的主题：

/odom 2D消息
/imu_data 3D消息
/vo 3D消息

要在自己的传感器中使用EKF，应该将传感器数据发布在这三个主题中，传感器应该发布机器人本体坐标系相对世界坐标系的关系。每个传感器可以基于不同的世界坐标系。注意：每个主题只能接受一个传感器输入，将来的版本可能会增加多个传感器输入，当前最普遍的输入数据是/vo主题的odometry消息。它包含3D姿态、速度及其协方差。所以最适合将自主的传感器增加在其中。

2. Building a GPS sensor message构建GPS传感器数据

GPS传感数据是3D无朝向信息的数据，GPS传感器发布的数据类似下面：

    msg.header.stamp = gps_time // time of gps measurement
    msg.header.frame_id = base_footprint // the tracked robot frame
    msg.pose.pose.position.x = gps_x // x measurement GPS. 
    msg.pose.pose.position.y = gps_y // y measurement GPS. 
    msg.pose.pose.position.z = gps_z // z measurement GPS. 
    msg.pose.pose.orientation.x = 1 // identity quaternion 
    msg.pose.pose.orientation.y = 0 // identity quaternion 
    msg.pose.pose.orientation.z = 0 // identity quaternion 
    msg.pose.pose.orientation.w = 0 // identity quaternion 
    msg.pose.covariance = {
                           cox_x, 0, 0, 0, 0, 0, // covariance on gps_x
                           0, cov_y, 0, 0, 0, 0, // covariance on gps_y 
                           0, 0, cov_z, 0, 0, 0, // covariance on gps_z 
                           0, 0, 0, 99999, 0, 0, // large covariance on rot x 
                           0, 0, 0, 0, 99999, 0, // large covariance on rot y 
                           0, 0, 0, 0, 0, 99999 } // large covariance on rot z

2.1 使用GPS驱动器发布NavSatFix

gps_common软件包中的utm_odometry_node用于将GPS测量值从sensor_msgs/NavSatFix 转换为sensor_msgs/Odometry.

3 映射主题名称

GPS传感器发布gps_meas主题，EKF节点希望处理的是 odom主题的odometry消息，为了连接GPS传感器与EKF节点，我们将vo映射到gps_meas上，因此在launch文件中增加下面的数据:

滤波器的输出是在 /robot_pose_ekf/odom_combined主题，节点同样'odom_combined' 与'base_link' 的变换发布到 /tf_message。