IMU plugin has strange measurements in RViz

edit

My robot has an IMU attached to its arm. The following URDF xacro macro is defining the IMU:

<xacro:macro name="xsens_imu" params="prefix parent xyz rpy simulation:=false">

    <material name="xsens-orange">
        <color rgba="0.8862745098 0.56862745098 0.09019607843 1"/>
    </material>

    <joint name="${parent}_to_xsens_imu" type="fixed">
        <parent link="${parent}"/>
        <child link="xsens_imu"/>
        <origin xyz="${xyz}" rpy="${rpy}"/>
    </joint>

    <link name="xsens_imu">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.057 0.041 0.024"/> <!--a simple box is used with the outer dimensions for simplicity-->
            </geometry>
            <material name="xsens-orange"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.057 0.041 0.024"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="0.055"/>
            <origin xyz="0 0 0"/>
            <inertia ixx="1.60333333333e-05" ixy="0.0" ixz="0.0" iyy="2.80333333333e-05" iyz="0.0"
                     izz="1.60333333333e-05"/>
        </inertial>
    </link>

    <xacro:if value="${simulation}">
        <gazebo>
            <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>400.0</updateRate>
                <bodyName>xsens_imu</bodyName>
                <topicName>imu</topicName>
                <robotNamespace>sensors</robotNamespace>
                <serviceName>imu/is_calibrated</serviceName>
                <gaussianNoise>0.00000001</gaussianNoise>
                <xyzOffsets>0 0 0</xyzOffsets>
                <rpyOffsets>0 0 0</rpyOffsets>
                <frameId>world</frameId>
            </plugin>
        </gazebo>
    </xacro:if>
</xacro:macro>

But when I attach it to my robot, the outputs of the IMU are wrong. The resulting direction of the linear acceleration does not make sense to me. As the robot is stationary in this moment, I expect the acceleration to be in z direction -9.81 and zero otherwise.

header: 
  seq: 150358
  stamp: 
    secs: 376
    nsecs: 345000000
  frame_id: "kinova_link_6"
orientation: 
  x: -0.707163260091
  y: 0.492549225252
  z: 0.00974669647696
  w: -0.507168991746
orientation_covariance: [1.0391111027982223e-18, 0.0, 0.0, 0.0, 1.0391111027982223e-18, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 2.01395400932e-08
  y: -1.39027463744e-08
  z: -1.25824759946e-09
angular_velocity_covariance: [1.0000000000000001e-16, 0.0, 0.0, 0.0, 1.0000000000000001e-16, 0.0, 0.0, 0.0, 1.0000000000000001e-16]
linear_acceleration: 
  x: 4.7659779885
  y: 7.13092593212
  z: -4.76145551598
linear_acceleration_covariance: [1.0000000000000001e-16, 0.0, 0.0, 0.0, 1.0000000000000001e-16, 0.0, 0.0, 0.0, 1.0000000000000001e-16]

One thing that makes me think is the frame of the IMU measurement. I want to have it in world coordinates, not in the kinova_link_6 (which is just the last link of the robot).

Does anyone see a mistake in my URDF that could fix this problem?

Thanks a lot!

edit: I added <frameId>world</frameId> but still do not get the desired result.