# Problem while integrating GPS data into robot_localization

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"/>

<!-- 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 ... edit retag close merge delete ## 2 Answers Sort by » oldest newest most voted Thanks for the nicely formatted question! I'm not positive this will do anything, but before I start looking at bag files, I do see that your navsat_transform_node instance is listening to the wrong EKF output. navsat_transform_node will work with a two-level EKF setup, but doesn't require it, so this line in your navsat_transform_node configuration: <remap from="odometry/filtered" to="$(arg local_odom)"/>


should be

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


The circular dependency between navsat_transform_node and the EKF is intentional. Again, I'm not sure that's your issue, but we should fix it before continuing.

more

Ok, I set the input of navsat to the output of the second EKF, the situation is a little better, i.e. setting map_link as fixed in RVIZ, I can see odom-->base TF remaining consistent as before, since it comes from the first EKF, but map_link-->odom_link stays a bit closer to the real trajectory, even if after a bit it still diverges very much, even if it seems the raw GPS data are not so noisy.

So, the overview graph is now like that?

• With your correction, the first warning about odom-->map transform disappeared, but the second one about map-->base is still there. I have GPS data produced at about 1Hz, do I have to change navsat frequency to a lower value?
• I also tried to set use_odometry_yaw="true" in navsat parameters, but from the node graph it still seems not using the IMU orientation for creating /odometry/navsat. Is this an intended behavior?
• If I have understood well, looping /odometry/global into navsat and keeping /odometry/local separate from it have the effect of the first one producing map-->odom and the second odom-->base independently, right? This concept is still a bit obscure to me after reading this and your dual_ekf ROS example... :(
• The topic /odometry/global from the second EKF has frame_id = map and child_frame_id = base_link. Should not the second be equal to odom?
• A small note, in this wiki page, you mention the datum parameter has 5 arguments, but it seems the latest version accepts only the first 3, frames are not necessary.

Thanks,

Guido

more

This is an excellent graph with good questions; can you please edit the original question with this information? They try to avoid turning these questions into threads.

( 2017-02-01 07:09:53 -0500 )edit