Robotics StackExchange | Archived questions

robot_localization nan output when using ODOM x,y in differential mode

Hello Everyone!

As I am trying to solve my previous question i have encountered another problem when trying to run robot_localization.

Brief background:

base on my experiments in which data to fuse from ODOM and IMU on robot_localization i did modify the testekflocalizationnodebag2.test with this configuration:

  <rosparam param="odom0_config">[true, true, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 false, false, true,
                                 false, false, false,
                                 false, false, false,
                                 false, false, false]</rosparam>

  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="imu0_remove_gravitational_acceleration" value="false"/>
  <param name="odom0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>

output i got is something like this: image description

Problem: Making the same pattern as the test2.bag, My configuration is as follows

ODOM Source: iRobotcreate2 using autonomy_create driver publishing odom->base_link at 50Hz, frameid = odom childframeid = `baselink`

IMU Source: LPMS USBAL2 using lpms_imu from larics publishing /imu_data topic at 50Hz, frameid = `baselink`

      <node name="robot_localization" pkg="robot_localization" type="ekf_localization_node" clear_params="true" output="screen" >
  <param name="frequency" value="50"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="odom0" value="/odom"/>
  <param name="imu0" value="/imu_data"/>

  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_link"/>
  <param name="world_frame" value="odom"/>
  <param name="two_d_mod" value="true"/>
  <param name="publish_tf" value="false"/>

  <rosparam param="odom0_config">[true, true, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 false, false, true,
                                 false, false, false,
                                 false, false, false,
                                 false, false, false]</rosparam>

  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="imu0_remove_gravitational_acceleration" value="false"/>
  <param name="odom0_differential" value="true"/>
  <param name="imu0_differential" value="false"/>

  <rosparam param="process_noise_covariance">[0.03, 0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0.03, 0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0.4, 0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0.03, 0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0.03, 0,    0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0.06, 0,     0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0.025, 0,     0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0.025, 0,    0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0.05, 0,     0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0.002, 0,     0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0.002, 0,     0,    0,    0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0.004, 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.01, 0,
                                              0,    0,    0,   0,    0,    0,    0,     0,     0,    0,     0,     0,     0,    0,    0.01]</rosparam>

  <rosparam param="initial_estimate_covariance">[1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,     1e-9,  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,     1e-9, 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,    1e-9]</rosparam>

</node>

upon running robot_localization, my /odometry/filtered output is all "nan"

header: 
  seq: 5824
  stamp: 
    secs: 1507613653
    nsecs: 460683823
  frame_id: odom
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]

what do you think this may cause the error? I uploaded my bagfile here.

Asked by fj138696 on 2017-10-10 00:51:57 UTC

Comments

I'm currrently battling the same NaN issue, with a different configuration. Can you update your ticket to include a sample IMU and odom message? I'm starting to think my covariences on them are bad and causing the instability. What are yours like?

Asked by stevemacenski on 2017-10-12 13:16:30 UTC

Hi, my bagfile is upload here.

Asked by fj138696 on 2017-10-12 21:00:36 UTC

Please post sample IMU and odometry messages with the question.

Asked by Tom Moore on 2018-01-11 05:37:21 UTC

I have no trouble running using your bag file and the launch file you posted. What version are you using? Also, you misspelled two_d_mode in your launch file.

Asked by Tom Moore on 2018-01-11 06:17:26 UTC

Answers