Fusing IMU + GPS with robot_location package

edit

Currently, I am trying to realize the localization by using the robot_localization package based on a GPS and an IMU. THe realization principle is based on the setting in the following link: http://answers.ros.org/question/20007... . I jut copied it here:

ekf_localization_node

Inputs
    IMU (type: sensor_msgs/Imu; topic: /imu; frame_id: base_imu_link)
    Transformed GPS data as an odometry message (navsat_transform_node output: topic: /odometry/gps)

Outputs
    Odometry message (this is what you want to use as your state estimate for your robot; topic: /odometry/filtered)

navsat_transform_node

Inputs
    IMU (type: sensor_msgs/Imu; topic: /imu; frame_id: base_imu_link)
    Raw GPS (type: NavSatFix; topic: /fix; frame_id: gps_reddot)
    Odometry (output of ekf_localization_node: topic: /odometry/filtered)

Outputs
    Transformed GPS data as odometry message (topic: /odometry/gps)

The image of my hardware setting is quite simple: https://drive.google.com/file/d/0BwCt...

My current launch file is:

<launch>
  <include file="$(find gps_ublox)/launch/gps.launch"/>
  <include file="$(find imu_ftdi)/launch/imu.launch"/>
  <!--node name="location_prediction_node" pkg="location_prediction" type="location_prediction_node" output="screen"/-->


  <!-- Parameters setting of the node: ekf_localization_node -->
  <node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link base_imu_link 20"/>
  <node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps_reddot 20"/>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
      <param name="output_frame" value="odom"/>
      <param name="frequency" value="20"/>
      <param name="odom_used" value="true"/>
      <param name="imu_used" value="true"/>
      <param name="vo_used" value="false"/>  
      <param name="sensor_timeout" value="0.1"/>  
      <param name="two_d_mode" value="false"/>

      <param name="map_frame"       value="map"/>
      <param name="odom_frame"      value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame"     value="odom"/>

      <param name="odom0" value="/odometry/gps"/>
      <param name="imu0"  value="/imu"/> 

      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false, 
                                      false , false, false, 
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="imu0_config">[false, false, false, 
                                     true , true , true, 
                                     false, false, false, 
                                     true , true , true ,
                                     true , true , true ]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential"  value="false"/>

      <param name="imu0_remove_gravitational_acceleration" value="true"/>

      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative"  value="false"/>

      <param name="print_diagnostics" value="true"/>

       <!-- ======== ADVANCED PARAMETERS ======== -->

      <param name="odom0_queue_size" value="2"/>
      <param name="imu0_queue_size" value="10"/>

      <rosparam param="process_noise_covariance">[0.05, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                              0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0 ...