robot_localization with 2 GPS

edit

Hi everyone, I am trying to fuse 2 gps, 1 IMU, robot's odometry using robot_localization package. My navsat_transform launch file is as follows:

<?xml version="1.0" encoding="ISO-8859-15"?>
<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_novatel" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_novatel.yaml" />
      <remap from="odometry/gps" to="novatel/odometry"/>
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/navsat_fix"/>
      <remap from="gps/filtered" to="navsatfix/filtered"/> 
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/> 
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_mtig" clear_params="true">
    <rosparam command="load" file="$(find rasberry_navigation)/config/navsat_transform_mtig.yaml" />
      <remap from="odometry/filtered" to="rl/odometry"/> 
      <remap from="gps/fix" to="/fix"/>
      <remap from="odometry/gps" to="mtig/odometry"/>
      <remap from="gps/filtered" to="fix/filtered"/>
      <param name="magnetic_declination_radians" value="0.01338085759"/>
      <param name="yaw_offset" value="1.570796327"/>
  </node>

  <node name="bl_novatel_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 base_link gps 100" output="log"/>
  <node name="novatel_fix_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0 0 0 0 0 1 gps base_imu 100" output="log"/>
  <node name="front_left_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 0.41 0 0 0 0 1 front_frame frame_left_link 100" output="log"/>
  <node name="front_right_tf" pkg="tf" type="static_transform_publisher" respawn="true" args="0 -0.41 0 0 0 0 1 front_frame frame_right_link 100" output="log"/>
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rasberry_navigation)/workshop_rviz.rviz"/>
</launch>

My dual_navsat_ekf launch file is as follows:

   <?xml version="1.0" encoding="ISO-8859-15"?>
    <launch>
      <rosparam command="load" file="$(find rasberry_navigation)/config/dual_ekf_navsat_2gps.yaml" />
      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true">
        <remap from="odometry/filtered" to="rl/odometry"/>  
      </node>

      <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
        <remap from="odometry/filtered" to="odometry/filtered_map"/>
      </node> 
      <include file="$(find rasberry_navigation)/navsat_transform_2_gps.launch"/>
    </launch>

My dual_navsat_ekf config file is as follows:

ekf_se_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  odom0: odometry
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false  #turn off for displaying in rviz

  imu0: imu/data
  imu0_config: [false, false, false,
                false, false, true,
                false, false, false,
                false, false, false,  #for rosbag angular velocity for yaw is zero
                true,  false, false]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: false

  use_control: false

  process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0 ...