robot_localization - using odometry and gps - get weird data
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 ...