robot_localization problems

edit

I'm now using robot_localization package on Ubuntu 16.04. I want to fuse the data of pose (given by orb_slam2), imu and gps signal collected by DJI Matrice 100. But the result looks strange. I'm not sure whether my configuration is right or not. The gps odometry (the green arrows in the picture) is not showing the ground truth path (a rectangular path on a fixed height), and the odometry/filtered_map (the red arrows) fluctuates badly along all axis when gps is fused and its path doesn't make sense. I will put my configuration file here and please help me check if there are errors. Thanks!

I'm sorry that I can't upload files or images because I don't have enough points. Please visit my google drive. google drive The bag file is also in this folder. The pose information given by orb_slam is also recorded and can be directly put into the ekf node.

The tf between base link and imu or gps is set to [0 0 0 0 0 0] because for now I'm not sure how imu and gps are mounted on the drone. This might be a problem.

/orb_slam/posewithcov is the pose information given by orb_slam2.

The imu reads 0 when the drone is facing northeast, so I set yaw_offset to 0.7853. I'm not sure if this is right because the heading of drone may not be the heading of imu. <launch> <arg name="output_final_position" default="false"/> <arg name="output_location" default="test.txt"/>

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

<node pkg="tf2_ros" type="static_transform_publisher" name="imu_link" args="0 0 0 0 0 0 base_link body_FLU" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0 0 0 0 base_link gps" />

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

  <param name="frequency" value="30"/>
  <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="transform_time_offset" value="0.0"/>
  <param name="transform_timeout" value="0.0"/>

  <param name="pose0" value="/orb_slam/posewithcov"/>
  <param name="imu0" value="/dji_sdk/imu"/>

  <rosparam param="pose0_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="pose0_differential" value="false"/>
  <param name="imu0_differential" value="false"/>

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

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

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

  <param name="pose0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

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