Drifting odom to base_frame transform

edit

I have been working with the robot_localization package and have gotten the ekf and navsat nodes working reasonably, but I notice that over time the resultant position drifts, even though my system is sitting stationary on my desk.

I have traced the problem down to the odom to base_frame transform drifting. My INU and GPS are putting out relatively stable values, and the result can easily be seen in the altitude measurement.

Here are my configs for robot_localization:

<!-- sets up the EKF to read in the IMU data and the cross couple of the GPS odometry -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
   <param name="frequency" value="30"/>  
   <param name="sensor_timeout" value="1"/>  
   <param name="two_d_mode" value="false"/>
   <param name="map_frame" value="map"/>
   <param name="odom_frame" value="odom"/>
   <param name="base_link_frame" value="base_link"/>
   <param name="world_frame" value="odom"/>
   <param name="odom0" value="/odometry/gps" />
   <param name="imu0" value="/imu/data"/> 
   <param name="odom1" value="/altitude" />
   <rosparam param="odom0_config">[true,  true,  true, 
                                   false, false, false, 
                                   false, false, false, 
                                   false, false, false,
                                   false, false, false]</rosparam>
   <rosparam param="imu0_config">[false, false, false, 
                                  true , true , true, 
                                  false, false, false, 
                                  false, false, false,
                                  true , true , true ]</rosparam>
   <rosparam param="odom1_config">[false,  false,  true, 
                                   false, false, false, 
                                   false, false, false, 
                                   false, false, false,
                                   false, false, false]</rosparam>
   <param name="odom1_differential" value="true"/>
   <param name="imu0_differential" value="false"/>
   <param name="imu0_remove_gravitational_acceleration" value="true"/>
   <param name="print_diagnostics" value="true"/>
<!-- Advanced parameters are the same as the defaults -->

</node>
<!-- sets up the navsat transformations that reads in IMU, output of EKF, and GPS data to filter the GPS.  Also provides a odom->UTF transformation -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
  <param name="magnetic_declination_radians" value="-0.047124"/>
  <param name="yaw_offset" value="0"/>
  <param name="zero_altitude" value="false"/>
  <param name="broadcast_utm_transform" value="true"/>
  <param name="publish_filtered_gps" value="true"/>
</node>

The odom1 input is a barometric pressure sensor that I was looking to use to stabilize the GPS altitude since GPS can be +/- 15m.

Here is a Bag File showing this problem.

I appreciate any suggestions on how I can fix this problem.

Update: So I've done a lot of tweaking of my system and I've decided that the problem occurs somewhere with my pressure altimeter signal. If this is enabled, then the output altitude will be stable, and then become wildly inaccurate, even though the altimeter signals is reasonably stable. I've even added low-pass filtering on the input to the EKF just to smooth things out.

I also noticed that if I had a Z-only signal (see odom1 config), then the EKF complains about a 0 covariance for element 7 (Y axis), even though it's disabled. If I added in a large covariance (e.g. 1E6), then EKF complains about large covariance when Y is enabled (which it isn't).

Has anyone else tried doing a Z-only sensor? I'd like to use the pressure altimeter to help stabilize my GPS altitude since since GPS altitude wanders +/-5m.