Robot_Localization , exploding covariances, why?

asked 2017-10-18 10:25:16 -0500

Jair
11 ●2 ●4 ●7

updated 2017-10-26 04:11:05 -0500

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 ...

(more)

edit retag flag offensive close merge delete

Comments

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?

gvdhoorn ( 2017-10-18 14:18:58 -0500 )edit

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..

Jair ( 2017-10-18 15:28:38 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2017-10-20 07:55:09 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

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.

edit flag offensive delete link

Comments

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 :)

Jair ( 2017-10-21 01:24:50 -0500 )edit

Ok Tom, your suggestion to use the velocities worked, is it not possible to use the velocities and the orientation? thanks, Jair

Jair ( 2017-10-22 04:09:57 -0500 )edit

Yes, absolutely. Using velocities and orientation is definitely supported. The key is that you need at least a velocity reference for each variable.

Tom Moore ( 2017-10-24 04:02:48 -0500 )edit

add a comment

answered 2017-10-26 04:47:39 -0500

Jair
11 ●2 ●4 ●7

[Answering my own question] So here is what i did. I calculated the standard deviation of the data from the IMUs while they were all stationary and put those values (squared) into the covariance matrices of the IMU messages. then the covariances of the kelman filter stopped exploding. however so far this only worked for the gyro readings. when i introduce the accelerometer readings (with the correct covariance matrix) i cant seem to get an accurate position estimation, not even for a short while. same goes for the orientation data itself, feeding in the euler angles produces a very noisy estimation and i think its because my yaw angle is very noisy and aso when it is being converted to a quternion it actually effects the entire orientation estimation.

So to get around that i used visual odometry (package name was aruco_ros).

The ekf is working fine, reading the gyros from the IMUs and reading the position and the euler angles from the visual odometry.

however, i wish to make my state estimation more robust. i want to be able to retain a correct pose even if the camera loses view of the marker and the odometry message stops broadcasting data.

unfortunately the IMUs so far are terrible for achieving this. 'remove gravitational acceleration = true' does not work well at all. question is, is it because: specificly the IMUs that im using are not accurate enough, or is it because an IMU cant ever get good enough results for state estimation even through a kelman filter?

thank you, Jair

edit flag offensive delete link

Comments

The IMUs will have biases in them, and the EKF is going to integrate those biases rapidly and end up producing large velocities. IMU-only state estimation is notoriously poor. You could always turn your commanded motor velocities into a high-covariance velocity estimate and feed that to the filter.

Tom Moore ( 2017-10-27 03:37:51 -0500 )edit

add a comment

Robot_Localization , exploding covariances, why?

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Robot_Localization , exploding covariances, why? edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Robot_Localization , exploding covariances, why?