ekf angle estimation

edit

Hello everyone! My setup is ROS Jade, Ubuntu Trusty I am currently using a 2 wheeled robot that is driving a full circle or a straight line, for calibrating purposes. The available sensors are: wheel encoders and gyro.

I now fuse linear velocity and yaw velocity in the ekf_localization_node via a TwistWithCovarianceStamped message. First I ignored yaw velocity to determine the gain of the wheel encoders. It works just fine since when I rostopic echo /odometry/filtered I actually see that the robot followed a 5.5 m straight line.

Now, when i also fuse yaw velocity I have some issues.Although the robot is supposed to drive 2pi counter-clockwise, what happens is that after being filtered, the estimated orientation (following z axis) is between 1 and -1 when I determined the gain.

To determine the gain I just assumed that the total number of impulses output to the gyro between the beginning and the end equals 2pi (6.28). Did I make a mistake?

Here is my launchfile:

<!-- Launch file for ekf_localization_node -->

<launch>

<!-- This node will take in measurements from odometry, IMU, stamped pose, and stamped twist messages. It tracks
     the state of the robot, with the state vector being defined as X position, Y position, Z position,
     roll, pitch, yaw, their respective velocites, and linear acceleration. Units for all measurements are assumed
     to conform to the SI standard as specified in REP-103. -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

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

  <!-- The frequency, in Hz, at which the filter will output a position estimate. Note that
       the filter will not begin computation until it receives at least one message from
       one of the inputs. It will then run continuously at the frequency specified here,
       regardless of whether it receives more measurements. Defaults to 30 if unspecified. -->
  <param name="frequency" value="20"/>

  <!-- The period, in seconds, after which we consider a sensor to have timed out. In this event, we
       carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
       as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
       if not specified. -->
  <param name="sensor_timeout" value="0.06"/>

  <!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
       are operating in a planar environment and want to ignore the effect of small variations in the
       ground plane that might otherwise be detected by, for example, an IMU. Defaults to false if
       unspecified. -->
  <param name="two_d_mode" value="true"/>

  <!-- REP-105 (http://www.ros.org/reps/rep-0105.html) specifies three principal coordinate frames: map,
       odom, and base_link. base_link is the coordinate frame that is affixed to the robot. The robot's
       position in the odom frame will drift over time, but is accurate in the short term and should be
       continuous. The odom frame is therefore the best frame for executing local motion plans. The map
       frame, like the odom frame, is a world-fixed coordinate frame ...