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

asked 2017-10-10 00:51:57 -0600

fj138696 gravatar image

updated 2017-10-10 01:22:45 -0600

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 test_ekf_localization_node_bag2.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, frame_id = odom child_frame_id = base_link

IMU Source: LPMS USBAL2 using lpms_imu from larics publishing /imu_data topic at 50Hz, frame_id = base_link

      <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 ...
(more)
edit retag flag offensive close merge delete

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?

stevemacenski gravatar image stevemacenski  ( 2017-10-12 13:16:30 -0600 )edit

Hi, my bagfile is upload here.

fj138696 gravatar image fj138696  ( 2017-10-12 21:00:36 -0600 )edit

Please post sample IMU and odometry messages with the question.

Tom Moore gravatar image Tom Moore  ( 2018-01-11 04:37:21 -0600 )edit

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.

Tom Moore gravatar image Tom Moore  ( 2018-01-11 05:17:26 -0600 )edit