Problem while integrating GPS data into robot_localization

edit

Hi to Tom and everybody,

I'm currently making some experiments with robot_localization, and I'm having some problems with the integration of GPS data for global localization of a non-holonomic mobile robot constrained to a 2D plane (no Z coordinate and no roll/pitch).

Here is the overview of my setup for odometry which is very similar to the one described in this section of the wiki where the use of a dual EKF node is discussed.

• X and Y are bidimensional coordinates
• Vx and Vy are linear velocities
• H^, X^^ and Y^^ are angular velocity and linear accelerations, respectively
• The topic name is inside parentheses
• The provided TF transformation is inside square brackets

This is my launch file (only the part relevant to replay the bag file):

<launch>

<!-- Reference frames -->
<arg name="world_frame"  default="earth"/>
<arg name="map_frame"    default="map_link"/>
<arg name="odom_frame"   default="odom_link"/>
<arg name="robot_frame"  default="base_link"/>
<arg name="camera_frame" default="camera_link"/>

<!-- Odometry topics -->
<arg name="wheels_odom"  default="/odometry/wheels"/>
<arg name="imu_odom"     default="/odometry/imu"/>
<arg name="gps_odom"     default="/odometry/gps"/>
<arg name="navsat_odom"  default="/odometry/navsat"/>
<arg name="visual_odom"  default="/odometry/visual"/>
<arg name="local_odom"   default="/odometry/local"/>
<arg name="global_odom"  default="/odometry/global"/>

<!-- Sensor fusion 1 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf1_local" clear_params="true" output="screen">
    <param name="frequency"         value="50"/>
    <param name="sensor_timeout"    value="0.2"/>
    <param name="two_d_mode"        value="true"/>
    <param name="map_frame"         value="$(arg map_frame)"/>
    <param name="odom_frame"        value="$(arg odom_frame)"/>
    <param name="base_link_frame"   value="$(arg robot_frame)"/>
    <param name="world_frame"       value="$(arg odom_frame)"/>

    <param name="use_control"       value="false"/>
    <param name="print_diagnostics" value="true"/>
    <param name="debug"             value="false"/>
    <param name="debug_out_file"    value="~/Log/ekf1.txt"/>

    <remap from="odometry/filtered" to="$(arg local_odom)"/>

    <param name="odom0" value="$(arg wheels_odom)"/>
    <rosparam param="odom0_config">
        [ false, false, false,
          false, false, false,
          true,  true,  false,
          false, false, false,
          false, false, false ]
    </rosparam>
    <param name="odom0_differential" value="false"/>
    <param name="odom0_relative"     value="false"/>
    <param name="odom0_queue_size"   value="10"/>
    <param name="odom0_nodelay"      value="true"/>

    <param name="imu0" value="$(arg imu_odom)"/>
    <rosparam param="imu0_config">
        [ false, false, false,
          false, false, false,
          false, false, false,
          false, false, true,
          true,  true,  false ]
    </rosparam>
    <param name="imu0_differential" value="false"/>
    <param name="imu0_relative"     value="false"/>
    <param name="imu0_queue_size"   value="10"/>
    <param name="imu0_nodelay"      value="false"/>
    <param name="imu0_remove_gravitational_acceleration" value="false"/>
</node>

<!-- GPS coordinates transformation -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
    <param name="frequency"            value="20"/>
    <param name="yaw_offset"           value="1.570796327"/>
    <param name="zero_altitude"        value="true"/>
    <param name="publish_filtered_gps" value="false"/>
    <param name="use_odometry_yaw"     value="false"/>
    <param name="wait_for_datum"       value="true"/>
    <param name="broadcast_utm_transform" value="false"/>
    <param name="delay"                value="3"/>
    <param name="magnetic_declination_radians" value="0.00383972"/>
    <rosparam param="datum">[43.542883, 11.571365, 0]</rosparam>

    <remap from="imu/data"          to="$(arg imu_odom)"/>
    <remap from="odometry/filtered" to="$(arg local_odom)"/>
    <remap from="gps/fix"           to="$(arg gps_odom)"/>
    <remap from="odometry/gps"      to="$(arg navsat_odom)"/>
</node>

<!-- Sensor fusion 2 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf2_global" clear_params="true" output="screen">
    <param name="frequency"         value="10"/>
    <param name ...