Setting robot_localization properlly - 2nd odom not working

asked 2015-07-03 14:58:11 -0600

nvoltex
131 ●13 ●19 ●24

updated 2015-07-07 12:52:24 -0600

Since this post I have added a magnetometer to the system in order to get a world-referenced yaw/heading estimate as well as a barometer for better altitude estimations.

First problem [solved] However, I'm still doing some tests on ground, so what I have been doing lately is to feed a fixed altitude (1.2m) as a new odometry msg (nav_msgs/Odometry), instead of using the altitude calculated from the barometer. After correcting a small problem with the msg stamp the altitude started to be used by the filter. Since then I have added a velocity to the same odometry msg and it seems to be working. I'm going to edit the post to reflect this changes.

Right now the launch file used to test the "fake" odometry msg is:

<launch>

<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 /base_link /imu 20"/>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
  <param name="frequency" value="20"/>  
  <param name="sensor_timeout" value="5"/>  
  <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="/imu/altitude"/>
  <param name="imu0"  value="/imu/data"/> 


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

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

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

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

  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative"  value="false"/>

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

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

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


       <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 ...

(more)

Comments

can you edit your question and display the covariance matricies for the input sources?

l0g1x ( 2015-07-05 20:37:02 -0600 )edit

I added a sample of the IMU topic. The topic with the altitude calculated from the barometer topic still doesn't have an associated covariance matrix, since I'm still trying to get it to work.

nvoltex ( 2015-07-06 04:05:54 -0600 )edit

what about the other inputs?

l0g1x ( 2015-07-06 14:51:22 -0600 )edit

I ended up forgeting to add the remaining ones since one of them is created from the nav_sat_transform_node and the other is a rather simple one with covariances yet to be defined. I edited the question in order to add them. Thanks in advance!

nvoltex ( 2015-07-06 15:15:27 -0600 )edit

For problem 1, at first glance I don't see anything wrong with your configuration or input messages. What do you see when you look at the diagnostics topic? Have you tried only fusing /imu/altitude as odom0? Do that, turn on debug mode, run for a few seconds, and then look through the output.

Tom Moore ( 2015-07-06 19:53:17 -0600 )edit

Feel free to post a bag file as well. I can usually troubleshoot faster that way.

Tom Moore ( 2015-07-06 19:54:18 -0600 )edit

I updated the question with some new info and corrected some things. I still have the doubts from before, but since I corrected the odometry msg I'm getting a strange behavior from the filter (explained in the question). Thanks for the help!

nvoltex ( 2015-07-07 12:53:46 -0600 )edit

Does your IMU have a magnetometer?

Tom Moore ( 2015-07-08 18:57:00 -0600 )edit

see more comments

answered 2015-07-08 20:00:01 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2015-07-09 15:18:51 -0600

Ah, I figured our your issue from edit 1. navsat_transform_node needs ukf_localization_node to produce one message before it can compute the odom->utm transform. When you don't include your /imu/altitude topic, the first message that ukf_localization_node receives (and therefore the first filtered odometry message it produces) is an IMU message with orientation data in it. This causes the odom->utm transform to be computed with the heading that's in that first IMU message. However, when you do include the /imu/altitude topic, the first message ukf_localization_node receives is an altitude message, and so its first output has a different heading, because it hasn't received any heading data yet.

Two things:

This shouldn't have any effect on any higher-level behaviors or any node that uses the output of ukf_localization_node. In fact, if you turn on publish_filtered_gps, you should find that the GPS data it outputs (/gps/filtered) is the same, even if the X-Y odom frame positions are different.
This will be solved when I add this feature. If the filter were to wait until it had received at least one IMU message, then it would have given you the exact same output.

EDIT: actually, one more thing: you should also be able to fix this (I say fix, but really nothing is broken) by setting the delay parameter in navsat_transform_node. If the value is high enough, then you will give ukf_localization_node time to receive at least one IMU message with orientation data in it.

EDIT 2 (to actually respond to your original questions): First, yes, if you don't fuse linear acceleration, the filter will be using only the GPS data to determine its pose. Interestingly, the filter will "create" velocities when you fuse pose only data (due to math within the EKF itself, not from anything I'm doing), though the covariances on those velocities in the filter output will be relatively high. You can try fusing the acceleration data and see what happens. It may just smooth the signal, or the integration of acceleration may start to generate velocity that causes the state estimate to run away.

Second, if your IMU measures roll and pitch and two_d_mode is false, then yes, fuse roll and pitch. To clarify, ekf_localization_node is not doing the same thing as imu_filter_madgwick. imu_filter_madgwick takes raw accelerometer, gyro, and (optionally) magnetometer data and produces an estimate of the orientation. ekf_localization_node does not use raw magnetometer data, and the sensor_msgs/Imu message doesn't contain raw mag data anyway. We use the IMU message's linear acceleration data to compute the vehicle's linear acceleration, linear velocity (integration over time), and position (double integration over time), but we do not use it to compute roll and pitch. Likewise, we use the IMU message's angular velocity data to compute the vehicle's angular velocity and orientation (integration over time).

edit flag offensive delete link

Comments

Oh ok, thanks! Could you give me some insight on the doubts I posted in the question? Another thing, the GPS I'm using is a garmin 18x, which besides the longitude, latitude and altitude also outputs a "speed" parameter. Is it a bad idea to use this as the velocity on the new odometry msg?

nvoltex ( 2015-07-09 04:01:11 -0600 )edit

One of the problems I see is that I don't have the heading associated with that velocity and I'm feeding it as the linear.x. However if the wind conditions make the vehicle move sideways, that won't be compensated by the filter.

nvoltex ( 2015-07-09 05:26:36 -0600 )edit

ekf_localization_node does not assume that your robot is moving in the direction of its yaw/heading. If you're moving sideways, then you'll just have Y velocity. If the GPS starts reporting sideways motion, then the filter should too. You can only do so much with a GPS and IMU.

Tom Moore ( 2015-07-09 15:22:37 -0600 )edit

What i meant when I asked if I should fuse the roll and pitch is that, due to the vehicle being sustained in the air thanks to a ballon, changes on roll and pitch will be small and due to noise from the rotors (or wind). That being said, those changes won't have much effect on the overhall movement

nvoltex ( 2015-07-10 10:37:50 -0600 )edit

add a comment

Setting robot_localization properlly - 2nd odom not working

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Setting robot_localization properlly - 2nd odom not working edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Setting robot_localization properlly - 2nd odom not working