ros_localization - drift in z when fusing pitch from imu sensor

edit

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