# Revision history [back]

### ros_localization - drift in z when fusing pitch from imu sensor

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, 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.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] initial_estimate_covariance: [1, 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, 1, 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, 1.0, 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, 1.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, 1.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, 1.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, 1.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, 1.0]   2 retagged clyde 867 ●22 ●38 ●35 ### ros_localization - drift in z when fusing pitch from imu sensor 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
world_frame: odom

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

initial_estimate_covariance: [1, 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,    1, 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,    1.0,  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,    1.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,    1.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,     1.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,     1.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,    1.0]