Error with orientation fusing IMU & GPS with robot_localization

edit

Hi,

I have a GPS and IMU system for a land based robot that I want to fuse using robot_localization. I've configured navsat_transform as mentioned here.

I had a couple of questions:

1. Direction of motion does not match with the display (Rviz). How do I configure the system to get the correct orientation for odometry/gps and odometry/filtered?
2. What could be the cause for the difference in angle between gps and filtered output ?
3. In case of gps jumps, I see that filtered output jumps very far away (from 100,100 to 6000,7000) , odometry/gps still seems to be somewhat on track. Which output should I be using for motion control?

Rviz Output : http://imgur.com/a/0UMBf

Yellow - odometry/gps Red - odometry/filtered Data is captured while moving in a counter clockwise circle.

Bag file: https://drive.google.com/file/d/0BxVo...

IMU Setup:

I've tried to convert our IMU into the specs of REP-103 as follows:

1. Acceleration and angular velocity are reported in body frame - X is forward, Y is left and Z is upwards. Counter clockwise movement is positive angular velocity on yaw.
2. Orientation 0 towards magnetic north, so added 90 offset in navsat_transform and ekf_node.
3. I assume not correcting for magnetic declination will give a fixed offset in the readings.

Launch File:

<launch>

    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_gnss" args="0 0 0 0 0 0 1 base_link gnss" />

    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
      <param name="frequency" value="50"/>
      <param name="delay" value="1.0"/>
      <param name="magnetic_declination_radians" value="0"/>

      <param name="zero_altitude" value="true"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="true"/>
      <!-- NED to END conversion for IMU -->
      <param name="yaw_offset" value="1.570796327"/>

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

      <remap from="imu/data" to="/imu"/>
      <remap from="gps/fix" to="/fix"/>

    </node>

    <arg name="output_final_position" default="true" />
    <arg name="output_location" default="localizatio_log.txt" />

    <param name="/use_sim_time" value="true" />
    <node pkg="rosbag" type="play" name="rosbagplay" args="--clock updated_imu.bag -d 5" required="true"/>

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

      <param name="frequency" value="50"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>


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

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

      <!-- # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types -->
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>
      <param name="yaw_offset" value="1.570796327"/>


      <param name="transform_time_offset" value="0.0"/>
      <param name="transform_timeout" value="0.0"/>

      <param name="imu0" value="/imu"/>

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

      <param name="odom0_differential" value="false ...