Problems with the robot Heading using robot_localization

edit

For my Karma, you can see that I am new in ROS. Since some time, I am working with the package robot_localization, using the ROS Indigo distro and ubuntu 14.04, in a simulation using morse ( https://www.openrobots.org/wiki/morse ). I am working in the simulation of a vehicle that executed one lap in loop (like a square). On the vehicle, I have two odometers and one GPS, and I use the EKF_Kalman node for filtering the noise that I added in the signal of each sensor. The GPS not considered the location data in the format UTM, because in morse, the GPS's topic just shares position information from the robot in the scenario in the simulation (I not use the navsat_transform node).

The robot_localization package works, in the other reasons, for the indications made for the package's creators (Tutorials, REP 103 and 105), and the a lot answers in this site (Thanks a lot TOM MOORE). However, after of follow the indications presented by TOM in (yaw angle jumping erratically), it still I have an error with the robot heading, which it is cumulative after each turn in the scene. This is the EKF_launch file content:

<!-- Launch file for ekf_localization_node -->

<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <!-- ======== STANDARD PARAMETERS ======== -->

  <param name="frequency" value="50"/>

  <param name="sensor_timeout" value="0.5"/>

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


  <!-- Defaults to "map" if unspecified -->
  <param name="map_frame" value="map"/>
  <!-- Defaults to "odom" if unspecified -->
  <param name="odom_frame" value="odom"/>
  <!-- Defaults to "base_link" if unspecified -->
  <param name="base_link_frame" value="base_footprint"/>
  <!-- Defaults to the value of "odom_frame" if unspecified -->
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.1"/>

  <param name="odom0" value="car/odometry"/>
  <param name="odom1" value="car/odometro"/>
  <param name="gps0" value="car/gps"/>

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

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

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


  <param name="odom0_differential" value="true"/>
  <param name="odom1_differential" value="false"/>     
  <param name="gps0_differential" value="false"/>
  <!--param name="imu0_differential" value="false"/-->

  <param name="odom0_relative" value="false"/>
  <param name="odom1_relative" value="true"/>
  <param name="gps0_relative" value="false"/>
  <!--param name="imu0_relative" value="false"/-->

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

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

  <param name="odom0_pose_rejection_threshold" value="50"/>
  <param name="odom0_twist_rejection_threshold" value="580"/>
  <param name="odom1_pose_rejection_threshold" value="50"/>
  <param name="odom1_twist_rejection_threshold" value="580"/>
  <param name="gps0_pose_rejection_threshold" value="1"/>
  <!--param name="twist0_rejection_threshold" value="1.2"/-->
  <!--param name="imu0_pose_rejection_threshold" value="10"/-->
  <!--param name="imu0_twist_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/-->

  <param name="debug"           value="true"/>
  <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>


  <rosparam param="process_noise_covariance">[0.25, 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 ...