Problems with the robot Heading using robot_localization

Question

For my Karma, you can see that I am new in ROS. Since some time, I am working with the package robot_localization, using the ROS Indigo distro and ubuntu 14.04, in a simulation using morse (https://www.openrobots.org/wiki/morse). I am working in the simulation of a vehicle that executed one lap in loop (like a square). On the vehicle, I have two odometers and one GPS, and I use the EKF_Kalman node for filtering the noise that I added in the signal of each sensor. The GPS not considered the location data in the format UTM, because in morse, the GPS's topic just shares position information from the robot in the scenario in the simulation (I not use the navsat_transform node).

The robot_localization package works, in the other reasons, for the indications made for the package's creators (Tutorials, REP 103 and 105), and the a lot answers in this site (Thanks a lot TOM MOORE). However, after of follow the indications presented by TOM in (yaw angle jumping erratically), it still I have an error with the robot heading, which it is cumulative after each turn in the scene. This is the EKF_launch file content:

<!-- Launch file for ekf_localization_node -->

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

  <!-- ======== STANDARD PARAMETERS ======== -->

  <param name="frequency" value="50"/>

  <param name="sensor_timeout" value="0.5"/>

  <param name="two_d_mode" value="true"/>


  <!-- Defaults to "map" if unspecified -->
  <param name="map_frame" value="map"/>
  <!-- Defaults to "odom" if unspecified -->
  <param name="odom_frame" value="odom"/>
  <!-- Defaults to "base_link" if unspecified -->
  <param name="base_link_frame" value="base_footprint"/>
  <!-- Defaults to the value of "odom_frame" if unspecified -->
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.1"/>

  <param name="odom0" value="car/odometry"/>
  <param name="odom1" value="car/odometro"/>
  <param name="gps0" value="car/gps"/>

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


  <param name="odom0_differential" value="true"/>
  <param name="odom1_differential" value="false"/>     
  <param name="gps0_differential" value="false"/>
  <!--param name="imu0_differential" value="false"/-->

  <param name="odom0_relative" value="false"/>
  <param name="odom1_relative" value="true"/>
  <param name="gps0_relative" value="false"/>
  <!--param name="imu0_relative" value="false"/-->

  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->

  <param name="odom0_pose_rejection_threshold" value="50"/>
  <param name="odom0_twist_rejection_threshold" value="580"/>
  <param name="odom1_pose_rejection_threshold" value="50"/>
  <param name="odom1_twist_rejection_threshold" value="580"/>
  <param name="gps0_pose_rejection_threshold" value="1"/>
  <!--param name="twist0_rejection_threshold" value="1.2"/-->
  <!--param name="imu0_pose_rejection_threshold" value="10"/-->
  <!--param name="imu0_twist_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/-->

  <param name="debug"           value="true"/>
  <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>


  <rosparam param="process_noise_covariance">[0.25, 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.25, 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.01, 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.01,   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.01,   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.01,   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,    1.39e-4, 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.01,    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.01,   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.01,   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.01,   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,    1.92e-4,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.01,   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.01,   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.01]</rosparam>
                                          <!-- x     y     z    roll   pitch    yaw     vx       vy       vz     vroll   vpitch   vyaw     ax      ay      az   -->                  


       <rosparam param="initial_estimate_covariance">[0.75, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                      0,    0.75, 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,    0.1,  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,     0.1,   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>
                                                 <!-- x     y     z   roll  pitch  yaw    vx    vy    vz  vroll  vpitch  vyaw    ax    ay    az   -->


  <!--  Placeholder for output topic remapping
  <remap from="odometry/filtered" to=""/>
  -->
 
 </node>
 

Link Bag file

Thank you for the help.

EDIT 1

Hi Tom,

Thank you for your answer. I want to explain each part of your Questions:

Below in Topics, you can find the topic description for each sensor in the filter.

The topic odom0 and odom1 are differents (Please see Topics).

Ok, i will change of Differential mode for the odom0.

In the last part, you said " If you only fuse yaw velocity, then I would expect the yaw error to grow every time you turn", but in the question made by Huibuh in the case "robot_localization: yaw angle jumping erratically", you commented the last 16/05/2014 this:

"Does your IMU produce yaw velocity? If it does, then in your case, I would integrate yaw velocity from both the IMU and odometry, and then exclude all yaw measurements from all sensors. If your absolute yaw measurement can't be trusted, then your best bet is to integrate velocities."

For this reason, i do not use the yaw measure. The true is that morse has not implemented a good sensor for that.

Again, thanks a lot for your answer and time.

TOPICS

Topic /car/gps

Topic /car/odometro

Topic /car/odometry

---

Originally posted by Fenix on ROS Answers with karma: 5 on 2016-02-19

Post score: 0

Answer 1

Please post a sample input message for each input topic. Also, you seem to have the same topic for odom0 and odom1, but there appears to be a typo in the second one (/car/odometro). Also, you have differential mode turn on for odom0, yet you are only fusing velocity data. Differential mode only applies to pose data. Finally, I'm not quite sure what you're saying the trouble is. If you only fuse yaw velocity, then I would expect the yaw error to grow every time you turn.

EDIT: I'm not sure that my quote from the other question and what I told you are inconsistent. In your question, you remarked

I have an error with the robot heading, which it is cumulative after each turn in the scene

I assumed you meant heading (yaw) variance. Since you are not measuring yaw directly, I was explaining why the variance would grow.

Also, in your /car/odometro topic, you have the frame_id set to odo (note the missing 'm'). The filter is going to ignore that message unless you provide a transform from odo->odom.

---

Originally posted by Tom Moore with karma: 13689 on 2016-03-01

This answer was ACCEPTED on the original site

Post score: 2