Setting robot_localization properlly - 2nd odom not working
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 ...
can you edit your question and display the covariance matricies for the input sources?
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.
what about the other inputs?
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!
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
diagnosticstopic? Have you tried only fusing/imu/altitudeas odom0? Do that, turn on debug mode, run for a few seconds, and then look through the output.Feel free to post a bag file as well. I can usually troubleshoot faster that way.
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!
Does your IMU have a magnetometer?