1
$\begingroup$

According to the documentation in : http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html I was able to transform the imu data header fram from "imu_link" into "base_link" through a tf transformation in a node.

However, somehow my imu data is not being fused into the ekf_localization node. I mean I was able to see the yaw changes as I move my robot. But when I subscribe to the odometry/filtered, it still shows my yaw to be at 0 degree.

I have double checked with the raw data from the imu and verified it outputs data as shown below: The data below are my imu message after I move the robot. This which reflects the changes of quaternion in the orientation

---
header: 
  seq: 9520
  stamp: 
    secs: 1617336774
    nsecs:  71053028
  frame_id: "base_link"
orientation: 
  x: -873.0
  y: -496.0
  z: 0.0
  w: 16353.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.01
  z: -0.18
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Below is my configure file for the ekf kalman filter.

<include file="$(find roboteq_driver)/launch/controller.launch"/>
<include file="bno055.launch"/>
<node pkg="robot_localization" type="ekf_localization_node"
    name="robot_localization_ekf_node_continuous"
    clear_params="true">
    <param name="frequency" value="30" />   
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true" />
    <param name="publish_tf" value="true" />
    <param name="odom_frame" value="odom" /> <!-- publish a new data to odom-->
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="odom" /> <!-- change here if we want to add map frame later-->
    <param name="odom0" value="/drive_odometry" />
    <rosparam param="odom0_config">
        [false,false, false, 
        false, false, false,
        true, true,false,
        false, false, false,
        false, false, false]</rosparam>
    <param name="odom0_differential" value="false"/>
   <param name="odom0_relative" value="false"/>
    <param name="odom0_nodelay" value="true"/>

    <param name="imu0" value="transformed_imu"/> 
    <rosparam param="imu0_config">
        [false,  false, false,
        false, false,false,
        false, false, false,
        false , false, true,
        true, false, false]  <!-- from IMU message acceleration-->  
    </rosparam>
    <param name="imu0_differential" value="false"/>
    <param name="imu0_relative" value="false"/>
    <param name="imu0_nodelay" value="false"/>
    <param name="imu0_remove_gravitational_acceleration" value="true"/>
    <param name="imu0_queue_size" value="10"/>

    <param name="use_control" value="false"/>
    <!--final odom message-->
    <remap from="/odometry/filtered" to="/odometry/filtered_odom"/>
    
</node>

Odometry message from : drive_odometry

header: 
  seq: 1392
  stamp: 
    secs: 1617336632
    nsecs: 897039890
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 10.4
      y: 4.43
      z: 0.0
    orientation: 
     x: -873.0
     y: -500.0
      z: 0.0
      w: 1240.0
  covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4]
---

So In this case, according to the kalman filter in the library, since I only use the yaw velocity from IMU in the configure file to determine the current yaw position of the robot, I am expecting the yaw value output from odometry/filtered_odom to be about the same as the yaw value in the IMU message.

However, when I subscribed to the odometry/filtered_odom, I find the orientation to be: x: 0.0 y: 0.0 z: 0.0 w: 1.0

Note: I only allow the angular velocity of IMU to be fused with the filter in determining the yaw direction. I did not use the yaw message from the odom, because the Odom message is generated by my calculation based on the feedback from the wheel's encoder.

$\endgroup$
8
  • $\begingroup$ Welcome to Robotics shouyu du, but I'm afraid that it is not clear what you are asking. We prefer practical, answerable questions based on actual problems that you face, so it's a good idea to include details of what you want to achieve, what you tried, what you saw & what you expected to see. Please take a look at How to Ask & tour for more information on how stack exchange works and work through the Robotics question checklist to edit your question to make it clearer. $\endgroup$
    – Tully
    May 4, 2022 at 18:33
  • $\begingroup$ Please edit this to make it clear what you are expecting and what you're getting. And clearly indicate your inputs and outputs versus expected outputs. Your input of odometry which doesn't report any yaw, seems like a strong reason for the output to not be yawing. And the orientation of your IMU result is suspect as the quaternion is completely non-normalized. And there does not appear to be any covariance estimates on the IMU data either. $\endgroup$
    – Tully
    May 4, 2022 at 18:36
  • $\begingroup$ @Tully I re-edit my post with the expected behavior. Please let me know if my question still makes sense to you. Thank you very much! $\endgroup$
    – sdu568
    May 4, 2022 at 19:37
  • $\begingroup$ @Tully 1. I purposefully did not use any heading information from my odom calculation, because the error gets off very quickly. However, since the library initalize the orientation to be 0,0,0,1. I believe by only support the angular velocity from the IMU should at least some how change the orientation to be something else rather than staying at 0,0,0,1. $\endgroup$
    – sdu568
    May 4, 2022 at 19:40
  • $\begingroup$ @Tully I think you are right. I think it might be because my covariance is not setup up properly. I will go ahead and test and let you know. $\endgroup$
    – sdu568
    May 4, 2022 at 20:25

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Browse other questions tagged or ask your own question.