Hi all,
I'm trying to use robot_localization
in Groovy to fuse information from several sensors (IMU, turn rate sensor and GPS). I'm using version x.1.6 from the hydro-devel branch (Check previous question regarding problems running in Groovy).
I've followed the tutorials from robot_localization
(http://wiki.ros.org/robot_localization) and GPS integration (http://wiki.ros.org/robot_localization). Two instances of ekf_localization_node
are running. One fuses IMU and turn rate sensor data, and the second one fuses IMU, turn rate and GPS. world_frame of the 1st instance is set to odom, and world_frame of 2nd one is set to map.
IMU sensor provides roll, pitch, yaw and X,Y,Z accelerations. Turn rate sensor provides Z axis angular speed (yaw rate). GPS data is feed through the navsat_transform_node
.
All testing has been made with the platform static (it is an USV that is for the moment dry-docked).
Everything seem to be working, as the respective TFs and topics are published but I have two issues:
1.- I expected that "map" frame would be the UTM grid and the map->odom TF would give me basically a filtered transformation between the UTM grid and odom (the output of the now deprecated utm_transform_node
, gone through the EKF with the IMU and turn rate sensor). However, what it seems to give me is the drift of the odom frame with respect of the initial point which is (0,0) and not the original UTM coordinates that should be something around (582409, 4793246).
Is this correct? Is there a way to get that the second instance of the ekf_localization_node
gives me that transformation (UTM map --> odom)?
If I use the utm_transform node
, whould be the map->utm transform equivalent to what I'm looking for? It seems that navsat_transform_node
doesn't publish the odom-->utm transform.
2.- Is it enough an IMU with accelerations to have good estimations with ekf_localization_node
? The IMU I'm using (RPY and accelerations, plus yaw rate from other sensor) in static is giving some accelerations, probably because it is not perfectly planar, and the EKF seems to be integrating them without any filtering, so in a few minutes it is giving me speeds of Km/s. I undertand that this should be normal, as no other source of positioning is present, but it doesn't seem to be compensated at all by the GPS input in the second instance.
Is there any roll-pitch gravity compensation introduced in the EKF or is it necessary to introduce it before feeding the EKF with the accelerations?
I understand that it is still a feature under development, so I don't know if this question should be put here or in another forum.
Thank you.
UPDATE:
I've fixed the errors pointed out in the comments (negative Z acceleration when static and using /imu/pseaa in navsat_transform_node). Results are now much better than before, but still the accelerations add up and in a couple of minutes I get a drift of more than 100m in X,Y.
I've also added a fake TwistWithCovarianceStamped to the EKF filters publishing 0 velocities with high covariance (though a lower covariance didn't seem to change te behaviour):
rostopic pub -r 10 cmd_vel geometry_msgs/TwistWithCovarianceStamped '{twist: {twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}, covariance: [10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10]}}'
So, at this moment as sources we have:
- A Twist message with 0 velocities.
- An IMU giving RPY and accelerations in X,Y at arount 0.02 m/s² (I guess due not being perfectly planar) and in Z at around 10.4 m/s².
- A GPS giving positioning that varies only in the 6th decimal degree (+- 2m, after transforming it to UTM).
- A turn rate sensor giving an error of around 0.01 rad/sec.
All sources are reporting that it is static, with what looks like reasonable error. However, the EKF seems unable to compensate the error, and after a couple minutes, the position has drifted around 100m and the speeds are around 3m/s (and 3Km and 60m/s in Z axis).
Should be this the expected result? Should't be the EKF able to filter the sensor errors and keep the position static?
I have uploaded a bag file that can be found here.
The bagfile is 135 seconds long. In the 30 first seconds, there is only the output from the sensor and after that robot_localization is launched and /odometry/imu and /odometry/filtered are published. When the bagfile ends, the position in both topics has drifted as reported before.
UPDATE 2:
Just noticed that IMU covariances maybe are too small for the error it is actually giving. I will try to put higher ones and report back.
UPDATE 3:
Increasing the IMU covariance largely reduced the drift to a half, more or less. However, it is still huge (aprox 60m in a couple minutes).
UPDATE 4:
Added /odometry/gps
message.
UPDATE 5:
Fixed the errors pointed in the comments:
navsat_transform_node
now listens to/odometry/filtered
instead of/odometry/imu
.- "gpsodom0" changed to "odom0" in "map" ekf_localization_node (
ekf_localization_gps
). - Created an small node that publishes TwisthWithCovarianceStamped with time stamp and frame_id = /base_footprint (sample message at the end).
Now it seems to be working as expected: Twist messages compensate the error on the IMU accelerations in the "odom" ekf instance and the GPS readings avoid the position to drift too much in the "map" ekf instance. This compensation works also when not using the TwistWithCovarianceStamped, in which case the odom frame drifts a lot, but the map frame remains close to the origin.
However, I have found that the compensation of the GPS is not exactly as expected. What I've appreciated is that, first, the the map frame starts drifting and, after some time, it stabilises and remains more or less static in some apparently random position, moving from it only whitin GPS error. This drifting is not always the same, sometimes it is very close to the origin, some other is more than 10 meters away. All tests are made under the same conditions, but the results from one to another are quite different.
A couple examples: In this bagfile, first, the map frame drifts slowly, until it stabilises at arount (14, -16). In this other, the map drifts much less and stays whithn a few meters of the origin. In theses bagfiles /cmd_vel topic is not used, but it is included in the bagfile.
Is this behaviour normal?
In my application, an error of few meters in the global positioning is not a problem as long as odom frame is good enough, but an error in the order or tens of meters is too much.
Also, now I'm getting the following warning in the map ekf node:
[ WARN] [1416228236.448192519]: MessageFilter [target=/base_footprint ]: Discarding message from [unknown] due to empty frame_id. This message will only print once.
I don't know what can be the soruce of this warning, as all topics seem to have frame_id. I also don't know what effect can have this on the ekf node.
(UPDATE 5) Updated launch file:
<launch>
<!-- IMU localization-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_imu" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="odom"/>
<param name="imu0" value="/imu/imu"/>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
<param name="imu1" value="/imu/pseaa"/>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, false, false, true, false, false, false]</rosparam>
<param name="imu1_differential" value="false"/>
<param name="twist0" value="/cmd_vel"/>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, false, false, false, true, false, false, false]</rosparam>
<param name="twist0_differential" value="false"/>
<remap from="/odometry/filtered" to="/odometry/imu"/>
</node>
<!--robot_localization: utm_transform_node, ekf_localization_node -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<!-- Compass reports magnetic north-->
<param name="yaw_offset" value="0"/>
<!-- Magnetic declination in Pasajes: -0º 49' W (http://magnetic-declination.com)-->
<param name="magnetic_declination_radians" value="-0.014253522"/>
<!-- On level ground, your IMU should read 0 for roll. If it doesn't,
enter the offset here (desired_value = offset + sensor_raw_value). -->
<param name="roll_offset" value="0.02"/>
<!-- On level ground, your IMU should read 0 for pitch. If it doesn't,
enter the offset here (desired_value = offset + sensor_raw_value). -->
<param name="pitch_offset" value="0.02"/>
<remap from="/imu/data" to="/imu/imu" />
<remap from="/gps/fix" to="/gps/fix" />
</node>
<!-- GPS integrated localization -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_gps" respawn="true" output="screen">
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_footprint"/>
<param name="world_frame" value="map"/>
<param name="imu0" value="/imu/imu"/>
<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
<param name="imu1" value="/imu/pseaa"/>
<rosparam param="imu1_config">[false, false, false, false, false, false, false, false, false, false, false, true, false, false, false]</rosparam>
<param name="imu1_differential" value="false"/>
<param name="twist0" value="/cmd_vel"/>
<rosparam param="twist0_config">[false, false, false, false, false, false, true, true, false, false, false, true, false, false, false]</rosparam>
<param name="twist0_differential" value="false"/>
<param name="odom0" value="/odometry/gps"/>
<rosparam param="odom0_config">[true, true, false, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
</node>
</launch>
Example of /imu/imu output
header:
seq: 54278
stamp:
secs: 1415891538
nsecs: 881067037
frame_id: imu
orientation:
x: -0.0138267071095
y: 0.0068664070791
z: -0.959724247099
w: 0.280519240258
orientation_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: -0.2548
y: -0.2254
z: -10.3488
linear_acceleration_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
(UPDATE 1) Final /odometry/imu message in the bagfile (aprox 100 seconds):
header:
seq: 3056
stamp:
secs: 1415962914
nsecs: 607047460
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: -149.016141137
y: -45.095185213
z: 3573.57328201
orientation:
x: 0.000870648461272
y: 2.93825796934e-06
z: -0.000700732886557
w: 0.999999375468
covariance: [8802.261111477996, -0.005788838569369977, 5.444902206855785, -3.840273544411622e-12, 2.5082517391441255e-09, 1.6506454847914552e-11, -0.005788838569369087, 8802.271754156352, -11.356349203451261, -2.508260913770995e-09, -3.84303696516608e-12, -5.483616850128093e-11, 5.444902206855782, -11.35634920345128, 17638.61274511664, -3.0400515647995436e-11, 1.004057461029083e-10, 0.0, -3.840273544411622e-12, -2.5082609137709947e-09, -3.0400515647995494e-11, 9.993481116512381e-07, 0.0, 0.0, 2.508251739144128e-09, -3.843036965166083e-12, 1.0040574610290828e-10, 0.0, 9.993481116512381e-07, 0.0, 1.6506454847914558e-11, -5.483616850128093e-11, 0.0, 0.0, 0.0, 9.996695826509655e-07]
twist:
twist:
linear:
x: -2.8057898119
y: -0.726292330684
z: 70.1421818858
angular:
x: 3.71348986853e-05
y: -0.000336629444983
z: 0.010646294744
covariance: [2.5463296868001555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.5463296868001555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.092618308225821, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.007793185274937632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.007793185274937632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00024768868126744505]
Example of /gps/fix:
header:
seq: 28786
stamp:
secs: 1415891697
nsecs: 482672929
frame_id: /gps
status:
status: 1
service: 1
latitude: 43.287458
longitude: -1.98402266667
altitude: 193.7
position_covariance: [1.2100000000000002, 0.0, 0.0, 0.0, 1.2100000000000002, 0.0, 0.0, 0.0, 4.840000000000001]
position_covariance_type: 1
(UPDATE 1) Last /odometry/filtered message in the bagfile (after aprox 100 sec):
header:
seq: 2938
stamp:
secs: 1415962914
nsecs: 611566163
frame_id: map
child_frame_id: base_footprint
pose:
pose:
position:
x: -134.980940092
y: -42.2113010159
z: 3301.74562816
orientation:
x: 0.000870741064745
y: 2.78125021491e-06
z: -0.000707626114296
w: 0.999999370534
covariance: [7822.053947085913, -0.005169207414691056, 4.922211730418841, -3.7242073374581155e-12, 2.411348508464105e-09, 1.59223380525217e-11, -0.005169207414698126, 7822.063312624678, -10.070601138861608, -2.4113572292972366e-09, -3.7268762749434515e-12, -5.206107264704302e-11, 4.922211730418825, -10.07060113886157, 15676.933289130826, -2.9328427884104475e-11, 9.534040799821162e-11, 0.0, -3.724207337458112e-12, -2.4113572292972386e-09, -2.9328427884104475e-11, 9.993481116512381e-07, 0.0, 0.0, 2.4113485084641085e-09, -3.726876274943456e-12, 9.534040799821181e-11, 0.0, 9.993481116512381e-07, 0.0, 1.5922338052521704e-11, -5.206107264704302e-11, 0.0, 0.0, 0.0, 9.996695826509655e-07]
twist:
twist:
linear:
x: -2.66379304704
y: -0.700978246536
z: 67.4209537165
angular:
x: 3.71350689302e-05
y: -0.000336629340775
z: 0.010646294744
covariance: [2.4480686118481643, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.4480686118481643, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.896098910796469, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.007793185274937632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.007793185274937632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00024768868126744505]
---
(UPDATE 1) Final /odometry/gps message form the bagfile
header:
seq: 973
stamp:
secs: 0
nsecs: 0
frame_id: odom
child_frame_id: ''
pose:
pose:
position:
x: 2.33176480979
y: 0.0612228550017
z: -0.356700941047
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance: [0.6406396378901923, -0.0005784933813285471, -0.03503371173521515, 0.0, 0.0, 0.0, -0.0005784933813285471, 0.6405231938216485, 0.03168475581723582, 0.0, 0.0, 0.0, -0.03503371173521514, 0.03168475581723582, 2.55883716828816, 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.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0]
---
(UPDATE 5) Example of TwistWithCovarianceStamped published in /cmd_vel:
---
header:
seq: 236202
stamp:
secs: 1416229824
nsecs: 221582889
frame_id: /base_footprint
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]
---
Originally posted by IvanV on ROS Answers with karma: 329 on 2014-11-13
Post score: 4