Covariance values too large for robot_localization

[Nick-Sullivan]

When fusing the odometry from the kobuki (mobile_base/odom) with GPS into a robot_localization EKF, the output becomes filled with NaN's.

If I republish the odom with smaller covariances for the unused variables, everything works fine.

Default odom:

header:
  seq: 888
  stamp: 
    secs: 1518127185
    nsecs: 997307393
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.1, 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, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

This causes the output of the EKF to look like this:

header: 
  seq: 152
  stamp: 
    secs: 1455208206
    nsecs: 544255733
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: nan
      y: nan
      z: nan
    orientation: 
      x: nan
      y: nan
      z: nan
      w: nan
  covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]
twist: 
  twist: 
    linear: 
      x: nan
      y: nan
      z: nan
    angular: 
      x: nan
      y: nan
      z: nan
  covariance: [nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan]

If I republish the odom with smaller covariances for the unused variables (1e308 to 1e6):

header: 
  seq: 114
  stamp: 
    secs: 1518127292
    nsecs: 450093413
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.1, 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, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Then the fusion works fine.

This is on Kinetic, and my EKF fusion looks like this:

<!-- Create EKF localisation. -->
<node pkg="robot_localization" type="ekf_localization_node" name="global_ekf_localization" output="screen">
    <!-- Set frame links -->
    <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="map"/>
    <!-- Set modes -->
    <param name="frequency"  value="10"/>
    <param name="two_d_mode" value="true"/>
    <!-- Set input types -->
    <param name="odom0" value="odometry/map_to_baselink"/>
    <param name="odom1" value="odometry/odom_to_baselink"/> <!-- kobuki odom-->
    <!-- Configure inputs -->
    <rosparam param="odom0_config">[ true,  true, false,
                                    false, false,  true,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>                                                                
    <param name="odom0_differential" value="false"/>
    <rosparam param="odom1_config">[ true,  true, false,
                                    false, false, true,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>                                                                
    <param name="odom1_differential" value="false"/>
    <!-- Remap output. -->
    <remap from="odometry/filtered" to="odometry/global_filtered"/>
</node>

[KrisPiters]

It seems the high values stem from lines 152 -154 in odometry.ccp:

  odom->pose.covariance[14] = DBL_MAX; // set a non-zero covariance on unused
  odom->pose.covariance[21] = DBL_MAX; // dimensions (z, pitch and roll); this
  odom->pose.covariance[28] = DBL_MAX; // is a requirement of robot_pose_ekf

DBL_MAX = 1.79769e+308

I've a suspicion these high values also cause rviz to crash when trying to visualize odom data.

[stonier]

Maybe some double - float casting going wrong in places. @Nick-Sullivan send us a PR for something that works and that you think is reasonable.