Ask Your Question
0

ros_localization - drift in z when fusing pitch from imu sensor

asked 2017-04-21 11:26:09 -0500

ln267 gravatar image

updated 2017-04-21 17:41:10 -0500

clyde gravatar image

Hello,

I am currently using ros_localization to fuse imu and wheel odometry in the odom frame.

  1. Imu0 is obtained from a pixhawk, using the package mavros, which outputs the topic /mavros/imu/data, which has the message type:image description sensor_msgs/Imu, and provides roll,pitch,yaw, roll vel, pitch vel, yaw vel, x accel, y accel, z accel. I've checked this data in rviz and have confirmed that it is already in the ENU coordinate frame which ROS uses.
  2. odom0 is wheel odometry coming from topic /quadOdom and has the message type: nav_msgs/Odometry. This is a 2D position estimation in odom frame which provides x,y and yaw. The odometry is started at the same heading as the imu so that they are in-line with each other.

When I integrate x,y,yaw from odom0 and yaw from the imu, the EKF filter works fine and the filtered state estimate odometry/filtered looks reasonable (although it is only 2D). However when include pitch and roll from the imu, the z value of the odometry/filtered drifts by a huge amount, although the x,y and yaw values of the filtered path remain fine. How can i fix this so that when I include pitch and roll from the imu sensor the z value of odometry/filtered follows a more sensible path. My launch file is:

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

This is the config file quadPose_IMU_fuse.yaml:

ekf_se_odom:                                                                                                                                                 
frequency: 10                                       
sensor_timeout: 0.5      
two_d_mode: false    
transform_time_offset: 0.0  
transform_timeout: 0.0  
print_diagnostics: true   
debug: true

map_frame: map  
odom_frame: odom  
base_link_frame: base_link    
world_frame: odom

odom0: quadOdom    
odom0_config: [true, true, false,
             false, false, true,
             false, false,  false,
             false, false, false,
             false, false, false]     
odom0_queue_size: 10    
odom0_nodelay: false     
odom0_differential: false      
odom0_relative: false

imu0: mavros/imu/data
imu0_config: [false, false, false,
             true, true, true,
             false, false,  false,
             false, false, false,
             false, false, false]     
imu0_queue_size: 10    
imu0_nodelay: false    
imu0_differential: false    
imu0_relative: false

use_control: false
dynamic_process_noise_covariance: true
smooth_lagged_data: true
process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    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,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                         0,    0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-07-07 07:29:09 -0500

Tom Moore gravatar image

updated 2017-07-07 07:29:39 -0500

First, make sure you turn off debug mode. That will do unpleasant things to your hard drive and CPU usage. :)

Second, you have two_d_mode turned off, but aren't measuring Z with any sensor. If you are running in 3D mode, you need to make sure that every single pose variable is either being measured directly (preferred) or its velocity is being measured. What's happening in your case is that you have nothing measuring Z, so the Z covariance is exploding. When you start to give the filter measurements for correlated variables (in this case, pitch data), it will affect Z.

Since your robot presumably can't fly, I would recommend fusing Z velocity from your odom0 source (though if you do that, please make sure the Z covariance in the odom0 message is not something massive like 99999).

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-04-21 11:26:09 -0500

Seen: 202 times

Last updated: Jul 07 '17