0
$\begingroup$

Rosanswers logo

Hello dear ROS community. I'm trying to use robot_localization with the ekf node inorder to produce an accurate orientation of my robot using 3 IMUs. I wrote the program for the IMU (Sparkfun SEN-14001) myself and it works pretty good. The IMUs give euler angles with respect to a fixed world frame which is exactly what i need. I wrote a node (python) to read the data through serial from the IMUs and publish it to three topics named: imu_0, imu_1, imu_2 (sensor_msgs/imu).

when i launch the ekf node of robot localization and open up Rviz i can see my static transforms are ok but then orientation output from the ekf node is totally wrong and behaves randomly.

so, for now i switched off all the inputs except the pitch angle so that i can debug more easily.

anyone see what im doing wrong?

here are my files.

i dont have enough points to upload a picture of my tf tree unfortunately as im pretty new to ROS, but it goes:

odom -> base_link -> imu_0 -> [ imu_1 , imu_2 ]

my launch file:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/skyline_template.yaml" />

   <!--  Placeholder for output topic remapping
    <remap from="odometry/filtered" to=""/>
    <remap from="accel/filtered" to=""/>
    -->

</node>

   <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_imu0" args="0 0 0 0 0 0 base_link imu_0" />
   <node pkg="tf2_ros" type="static_transform_publisher" name="imu0_to_imu1" args="0 -0.25 0 0 0 0 imu_0 imu_1" />
   <node pkg="tf2_ros" type="static_transform_publisher" name="imu0_to_imu2" args="0 0.28 0 0 0 0 imu_0 imu_2" />

</launch>

my ekf.yaml:

frequency: 50
sensor_timeout: 1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

###################################################
imu0: imu0
imu0_config: [false, false, false,
              false,  true, false,
              false, false, false,
              false, false, false,
              false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 1
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #
imu0_remove_gravitational_acceleration: true

####################################################
imu1: imu1
imu1_config: [false, false, false,
              false, true, false,
              false, false, false,
              false, false, false,
              false, false, false]
imu1_nodelay: false
imu1_differential: false
imu1_relative: true
imu1_queue_size: 1
imu1_pose_rejection_threshold: 10                 # Note the difference in parameter names
imu1_twist_rejection_threshold: 10                #
imu1_linear_acceleration_rejection_threshold: 10  #
imu1_remove_gravitational_acceleration: true

####################################################
imu2: imu2
imu2_config: [false, false, false,
              false, true, false,
              false, false, false,
              false, false, false,
              false, false, false]
imu2_nodelay: false
imu2_differential: false
imu2_relative: true
imu2_queue_size: 1
imu2_pose_rejection_threshold: 10                 # Note the difference in parameter names
imu2_twist_rejection_threshold: 10                #
imu2_linear_acceleration_rejection_threshold: 10  #
imu2_remove_gravitational_acceleration: true
####################################################

use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [false, false, false, false, false, false]
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,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 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,
                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

and this is my python script for reading the data from the IMUs and publishing it using sensor_msgs/imu:

#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

## Simple talker demo that published std_msgs/Strings messages
## to the 'chatter' topic

import serial
import rospy
import tf
import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String
from sensor_msgs.msg import Imu
#import sensor_msgs.msg 


def imu_reader():

    # declare a tf publisher (this is used for the static tf of the IMUs)
    pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)
    
    ## ====================== Open Serial Ports ======================= ##
    ser0 = serial.Serial('/dev/ttyACM0', 115200)
    ser1 = serial.Serial('/dev/ttyACM1', 115200)
    ser2 = serial.Serial('/dev/ttyACM2', 115200) 

    ## ======================== ROS A ======================== ##
    msg0 = Imu()
    msg1 = Imu()
    msg2 = Imu()
    rospy.init_node('imu_reader', anonymous=True)
    pub0 = rospy.Publisher('imu0', Imu)#, queue_size=10)
    pub1 = rospy.Publisher('imu1', Imu)
    pub2 = rospy.Publisher('imu2', Imu)    
    rate = rospy.Rate(100) # 10hz
    ## ======================== ROS B ======================== ##


    while not rospy.is_shutdown():
    ## ======================== ROS A ======================== ##
    # headers for ros messages 
    msg0.header.stamp= rospy.Time.now()
    msg0.header.frame_id = 'imu_0'
    msg1.header.stamp= rospy.Time.now()
    msg1.header.frame_id = 'imu_1'
    msg2.header.stamp= rospy.Time.now()
    msg2.header.frame_id = 'imu_2'
        
    #rospy.loginfo(ser.readline())
    ## ======================== ROS B ======================== ##
    

    ##===========================================================================##
    ##=========================== Get data from IMU-0 ===========================##
    ##===========================================================================##
    flag0 = float(ser0.readline())
    if flag0 == 123456789:
      thetaX0 = float(ser0.readline())
      thetaY0 = float(ser0.readline())
      thetaZ0 = float(ser0.readline())
      gyroX0 = float(ser0.readline())
      gyroY0 = float(ser0.readline())
      gyroZ0 = float(ser0.readline())
      acclX0 = -float(ser0.readline())
      acclY0 = -float(ser0.readline())
      acclZ0 = -float(ser0.readline())
      print "thetaX0 = " + str(thetaX0)
      print "thetaY0 = " + str(thetaY0)
      print "thetaZ0 = " + str(thetaZ0)
      print "acclX0 = " + str(acclX0)
      print "acclY0 = " + str(acclY0)
      print "acclZ0 = " + str(acclZ0)        
      print "gyroX0 = " + str(gyroX0)
      print "gyroY0 = " + str(gyroY0)
      print "gyroZ0 = " + str(gyroZ0)
      
      ## Orientation of IMU-0 (Quaternions)  
      q0 = tf.transformations.quaternion_from_euler(thetaZ0,thetaY0,thetaX0) #was roll pitch yaw originally
      msg0.orientation.x = q0[0]#thetaX0 #q0[0] #magnetometer
          msg0.orientation.y = q0[1]#thetaY0 #q0[1]
          msg0.orientation.z = q0[2]#thetaZ0 #q0[2]
          msg0.orientation.w = q0[3]#0 #q0[3]

      ## Orientation Covariance IMU-0:
      msg0.orientation_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 2]

      ## Angular Velocity of IMU-0:
      msg0.angular_velocity.x = gyroX0
      msg0.angular_velocity.y = gyroY0
      msg0.angular_velocity.z = gyroZ0

      ## Angular Velocity Covariance IMU-0:
          msg0.angular_velocity_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 2]
    
      ## Linear Accelartion of IMU-0:
      msg0.linear_acceleration.x = acclX0
      msg0.linear_acceleration.y = acclY0
      msg0.linear_acceleration.z = acclZ0

      ## Linear Accelartion Covariance IMU-0:
      msg0.linear_acceleration_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 2]

    ##===========================================================================##
    ##=========================== Get data from IMU-1 ===========================## 
    ##===========================================================================##
    flag1 = float(ser1.readline())
    if flag1 == 123456789:  
      thetaX1 = float(ser1.readline())
      thetaY1 = float(ser1.readline())
      thetaZ1 = float(ser1.readline())
      gyroX1 = float(ser1.readline())
      gyroY1 = float(ser1.readline())
      gyroZ1 = float(ser1.readline())
      acclX1 = -float(ser1.readline())
      acclY1 = -float(ser1.readline())
      acclZ1 = -float(ser1.readline())
      #print "thetaX1 = " + str(thetaX1)
      #print "thetaY1 = " + str(thetaY1)
      #print "thetaZ1 = " + str(thetaZ1)
      #print "acclX1 = " + str(acclX1)
      #print "acclY1 = " + str(acclY1)
      #print "acclZ1 = " + str(acclZ1)        
      #print "gyroX1 = " + str(gyroX1)
      #print "gyroY1 = " + str(gyroY1)
      #print "gyroZ1 = " + str(gyroZ1)
      
      ## Orientation of IMU-1 (Quaternions)
      q1 = tf.transformations.quaternion_from_euler(thetaZ1,thetaY1,thetaX1) #was roll pitch yaw originally
      msg1.orientation.x = q1[0]#thetaX1#q1[0] #magnetometer
      msg1.orientation.y = q1[1]#thetaY1#q1[1]
          msg1.orientation.z = q1[2]#thetaZ1#q1[2]
          msg1.orientation.w = q1[3]#0 #q1[3]

      ## Orientation Covariance IMU-1:
      msg1.orientation_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]

      ## Angular Velocity of IMU-1:
      msg1.angular_velocity.x = gyroX1
      msg1.angular_velocity.y = gyroY1
      msg1.angular_velocity.z = gyroZ1

      ## Angular Velocity Covariance IMU-1:
      msg1.angular_velocity_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]

      ## Linear Accelartion of IMU-1:
      msg1.linear_acceleration.x = acclX1
      msg1.linear_acceleration.y = acclY1
      msg1.linear_acceleration.z = acclZ1

      ## Linear Accelartion Covariance IMU-1:
      msg1.linear_acceleration_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]

    ##===========================================================================##
    ##=========================== Get data from IMU-2 ===========================##
    ##===========================================================================##
    flag2 = float(ser2.readline())
    if flag2 == 123456789:  
      thetaX2 = float(ser2.readline())
      thetaY2 = float(ser2.readline())
      thetaZ2 = float(ser2.readline())
      gyroX2 = float(ser2.readline())
      gyroY2 = float(ser2.readline())
      gyroZ2 = float(ser2.readline())
      acclX2 = -float(ser2.readline())
      acclY2 = -float(ser2.readline())
      acclZ2 = -float(ser2.readline())
      #print "thetaX2 = " + str(thetaX2)
      #print "thetaY2 = " + str(thetaY2)
      #print "thetaZ2 = " + str(thetaZ2)
      #print "acclX2 = " + str(acclX2)
      #print "acclY2 = " + str(acclY2)
      #print "acclZ2 = " + str(acclZ2)        
      #print "gyroX2 = " + str(gyroX2)
      #print "gyroY2 = " + str(gyroY2)
      #print "gyroZ2 = " + str(gyroZ2)
        
      ## Orientation of IMU-2 (Quaternions)
      q2 = tf.transformations.quaternion_from_euler(thetaZ2,thetaY2,thetaX2) #was roll pitch yaw originally
          msg2.orientation.x = q2[0]#thetaX2#q2[0] #magnetometer
          msg2.orientation.y = q2[1]#thetaY2#q2[1]
          msg2.orientation.z = q2[2]#thetaZ2#q2[2]
          msg2.orientation.w = q2[3]#0 #q2[3]

      ## Orientation Covariance IMU-2:
      msg2.orientation_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]       

      ## Angular Velocity of IMU-2:
      msg2.angular_velocity.x = gyroX2
      msg2.angular_velocity.y = gyroY2
      msg2.angular_velocity.z = gyroZ2

      ## Angular Velocity Covariance IMU-2:
      msg2.angular_velocity_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]
    
      ## Linear Accelartion of IMU-2:
      msg2.linear_acceleration.x = acclX2
      msg2.linear_acceleration.y = acclY2
      msg2.linear_acceleration.z = acclZ2
    
      ## Linear Accelartion Covariance IMU-2:
      msg2.linear_acceleration_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]

    
    ## ======================== ROS A ======================== ##
    pub0.publish(msg0)
    pub1.publish(msg1)
    pub2.publish(msg2)
        
    #tfm2 = tf2_msgs.msg.TFMessage([t2])
    #pub_tf.publish(tfm2)
    rate.sleep() ##calculates how much time to sleep inorder to cycle at specified Hz from ->       rospy.Rate(#Hz)
    ## ======================== ROS B ======================== ##


if __name__ == '__main__':
    try:
    imu_reader()
    rospy.spin()

    except rospy.ROSInterruptException:
        pass

in the odometry filtered message that robot_localization publishes i can see that the covariances explode over time:

header: 
  seq: 983
  stamp: 
    secs: 1508405356
    nsecs: 387490988
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.625654852985
      z: 0.0
      w: 0.780099996755
  covariance: [1624.1014521872319, 0.0, 315.3714366222018, 0.0, 0.0, 0.0, 0.0, 1953.6062147951848, 0.0, 0.0, 0.0, 0.0, 315.37143662219944, 0.0, 1672.7446854188258, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4351.414706538122, 0.0, 3113.1139353455387, 0.0, 0.0, 0.0, 0.0, 0.03544975751264216, 0.0, 0.0, 0.0, 0.0, 3113.1139353455737, 0.0, 2547.4820123390623]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.337388947807
      z: 0.0
  covariance: [29.884067975599514, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 29.884067975599514, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 44.87776058183954, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.20663533072335905, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0024667230231830454, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4132706604467181]

I'd greatly appreciate any help

thank you for reading, Jair


Originally posted by Jair on ROS Answers with karma: 11 on 2017-10-18

Post score: 0


Original comments

Comment by gvdhoorn on 2017-10-18:\

The IMUs give euler angles with respect to a fixed world frame [..]

just making sure: the sensor_msgs/Imu::orientation is a Quaternion, not a 3 tuple of Euler angles. Is that correctly implemented in your Imu driver?

Comment by Jair on 2017-10-18:
yes thank you, im using the built in function of tf to do that: q=tf.transformations.quaternion_from_euler(roll,pitch,yaw) then x=q[0] y=q[1] and so on..

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

All three of your IMU inputs are measuring absolute orientation (right now, just pitch). Are you sure they all agree with one another? In other words, if you were to start up your IMU node and plot the pitch angle over time, would they start to diverge from one another? If so, is that divergence captured in the covariance data? If they diverge, then your state estimate is going to jump around between them as it receives each message, unless you get your covariances down.

For r_l to work, you need a pose and velocity reference for every pose variable. In other words, you need to measure X, Y, Z, roll, pitch, and yaw, or their respective velocities (in two_d_mode, this is limited to just X, Y, and yaw). Any pose variable that you don't measure either directly or through its velocity will have its covariance explode rapidly, since the filter adds error at every time step (the process_noise_covariance). That error growth can either be drastically slowed by measuring velocities, or completely bounded by measuring the pose data directly.

Your use case is not typical; r_l assumes you want the entire state vector to be estimated. The correlation between state variables means that huge covariance values for unmeasured variables will affect your measured ones, and vice-versa.

You could always read the three measurements from your IMU sensors, then do a straight average of them and compute the covariance manually. Since they are all the same IMU, their covariance values ought to be the same per IMU, so a simple average is all the EKF would end up doing anyway (if you ignore the state prediction step, that is). An EKF is just doing weighted averaging based on relative covariance values.

Alternatively, you could try publishing all of them to the same topic, then feeding that topic to imu_filter_madgwick.

Finally, if you want to watch the orientation of the sensor in rviz, I think imu_filter_madwick can support that. If not, you can just manually publish a PoseWithCovarianceStamped message using rostopic pub that feeds 0s to the filter for all the unmeasured variables.


Originally posted by Tom Moore with karma: 13689 on 2017-10-20

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by Jair on 2017-10-21:
Firstly thank you very much for answering. The IMUs do not diverge. but you have given me some food for thought. i will update as i solve. thank you :)

Comment by Jair on 2017-10-22:
Ok Tom, your suggestion to use the velocities worked, is it not possible to use the velocities and the orientation? thanks, Jair

Comment by Tom Moore on 2017-10-24:
Yes, absolutely. Using velocities and orientation is definitely supported. The key is that you need at least a velocity reference for each variable.

$\endgroup$

Your Answer

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