Hello there,
I'm having trouble with the robot_localization package. At first I had odometry, IMU and GPS, but the IMU data got disturbed by a magnetic field of our robot. So I have to use only odometry and GPS for this approach since I can not make any new measurements and have to evaluate what I can from this data. So, I want to fuse odometry and GPS. I am using the Dual-EKF-Node example. I will give all information needed here: As there is no usable IMU-Data, I have to use the wait-for-datum-mode and give a datum in the config-file.
Launch-File:
<launch>
<rosparam command="load" file="$(find bike_localization)/config/odom_gps.yaml" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_state_estimate_odom" clear_params="true"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_state_estimate_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
<remap from="gps/fix" to="gps_rep"/>
</node>
</launch>
Config: odom_gps.yaml
ekf_state_estimate_odom:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /odom_can
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
ekf_state_estimate_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: /odom_can
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: true
odom1: odometry/gps
odom1_config: [true, true, false,
false, false, false,
false , false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: false
odom1_differential: false
odom1_relative: false
use_control: false
navsat_transform:
frequency: 30
delay: 2.0
magnetic_declination_radians: 3.55641
#yaw_offset: 1.570796327
zero_altitude: true
broadcast_utm_transform: false
publish_filtered_gps: true
use_odometry_yaw: false # I tried true, too
wait_for_datum: true
datum: [XXXXX, YYYYY, -1.5707, map, base_link] # we start facing south, 0 is facing east, so -pi/2 is facing south, tried other values, get the same output
# For the actual run, real gps data is used
Now, this ist, what one of my odometry-messages look like. The odometry is facing X:
header:
seq: 113
stamp:
secs: 1559130292
nsecs: 62068063
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.281387492407
y: 0.0023472056171
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0118238048699
w: 0.999930096376
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]
twist:
twist:
linear:
x: 0.440455263089
y: 0.00978800793434
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.0706352548189
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]
I do not publish a transform between base_link and odom or between odom and map and this is what my tf-tree looks like:
This is the actual pathway using a very good DGPS as reference (only as reference, not usable in later stages). The Beginning is marked with the yellow array. After the start it drives a big 8:
This is the actual gps of the robot:
The steering of the robot was a bit broken and therefore has a lot of inaccuracies. Nonetheless the odometry:
And now my configuration of robot_localization produces this estimate in the odmoetry/filtered_map, start is at (0,0) facing X:
I know, that my data are not the best, but this should nonetheless not be the estimate from these data. I have exhausted all my nerves for multiple weeks now. I am trying my best to find out, where my error might be. If you can help me, please do so.
EDIT1
As adviced I set the covariances. As I do not know the exact values, I tried some. The overall plot varies a little but the problem still remains. I used rather large and small covariances. Here are just two of the many tries:
Example picture for velocity covariances x->0.3 y->0.9
And covariances for velocities x->0.7, y->0.7
Minor changes are visible but the overall plot stays really bad, especially the many circles around the beginning, which don't match the overall gps.
EDIT2
As asked, the GPS data has covariance, too which is of course changing over time.
header:
seq: 243
stamp:
secs: 1559316888
nsecs: 496287087
frame_id: "gps_rep"
status:
status: 0
service: 1
latitude: XXXXXXXX
longitude: YYYYYYYYY
altitude: 49.455
position_covariance: [3.078, 0.0, 0.0, 0.0, 3.078, 0.0, 0.0, 0.0, 3.605]
position_covariance_type: 2
Originally posted by Bob112358 on ROS Answers with karma: 1 on 2019-05-29
Post score: 0
