robot_localization problems
I'm now using robotlocalization package on Ubuntu 16.04. I want to fuse the data of pose (given by orbslam2), imu and gps signal collected by DJI Matrice 100. But the result looks strange. I'm not sure whether my configuration is right or not. The gps odometry (the green arrows in the picture) is not showing the ground truth path (a rectangular path on a fixed height), and the odometry/filtered_map (the red arrows) fluctuates badly along all axis when gps is fused and its path doesn't make sense. I will put my configuration file here and please help me check if there are errors. Thanks!
I'm sorry that I can't upload files or images because I don't have enough points. Please visit my google drive. google drive The bag file is also in this folder. The pose information given by orb_slam is also recorded and can be directly put into the ekf node.
The tf between base link and imu or gps is set to [0 0 0 0 0 0] because for now I'm not sure how imu and gps are mounted on the drone. This might be a problem.
/orbslam/posewithcov is the pose information given by orbslam2.
The imu reads 0 when the drone is facing northeast, so I set yawoffset to 0.7853. I'm not sure if this is right because the heading of drone may not be the heading of imu.
<param name="/use_sim_time" value="true" />
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_link" args="0 0 0 0 0 0 base_link body_FLU" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0 0 0 0 base_link gps" />
<node name="test_ekf_localization_node_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<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="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="pose0" value="/orb_slam/posewithcov"/>
<param name="imu0" value="/dji_sdk/imu"/>
<rosparam param="pose0_config">[true , true , true ,
false, false, false,
false, 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="pose0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="pose0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="pose0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<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,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
<node name="test_ekf_localization_node_gps" pkg="robot_localization" type="ekf_localization_node" clear_params="true">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<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="map"/>
<param name="transform_time_offset" value="0.0"/>
<param name="transform_timeout" value="0.0"/>
<param name="pose0" value="/orb_slam/posewithcov"/>
<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/dji_sdk/imu"/>
<rosparam param="pose0_config">[true , true , true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="odom0_config">[true , true , true ,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, false,
true, true, true]</rosparam>
<param name="pose0_differential" value="false"/>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="pose0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="pose0_queue_size" value="10"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<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,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
<remap from="/odometry/filtered" to="/odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" clear_params="true" output="screen">
<param name="frequency" value="30"/>
<param name="delay" value="0"/>
<param name="magnetic_declination_radians" value="0.16318828506"/>
<param name="yaw_offset" value="0.78539816"/>
<param name="zero_altitude" value="true"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="publish_filtered_gps" value="false"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<remap from="/odometry/filtered" to="/odometry/filtered_map"/>
<remap from="/gps/fix" to="/dji_sdk/gps_position"/>
<remap from="/imu/data" to="/dji_sdk/imu"/>
</node>
Asked by JakeSheng on 2018-09-28 21:02:13 UTC
Answers
There's quite a bit going on here!
If you can update your question and add some sample messages from each sensor source, I'd appreciate it.
I'd be very careful about fusing multiple sources of absolute pose information. I don't know enough about the SLAM package in question to comment on how it behaves, but if the package has any divergence or has a poor loop closure (is it running live SLAM, or just providing a pose estimate in your map?), then the SLAM position estimate and the GPS position estimate are going to differ wildly, and it'll just jump back and forth between those state estimates.
The same goes for your IMU data. You are fusing absolute orientation data from your IMU, but there's no guarantee that it will match your SLAM positions. For example, if you start your robot in a pose where the IMU reads pi/4 radians, and then drive straight forward one meter, your SLAM package, if not using your IMU absolute orientation, is going to probably read an XY position of (1, 0), but that disagrees with the orientation of the IMU, which would suggest that we should be at (0.707, 0.707) after driving forward one meter.
In any case, if you are fusing multiple absolute pose sources, you need to make sure they are in the same coordinate frame, or that they have a transform defined that will make them so.
My suggestion to you is to start with just the "tier 1" (odom frame) EKF. Get that looking the way you want, then move on to the second tier (map frame) EKF. If I were you, I'd probably fuse my full 3D pose from the SLAM package in the odom-frame EKF, and maybe just fuse the angular velocities from the IMU. If you lack a velocity measurement source, I'd stop fusing the linear acceleration data from the IMU, too.
Asked by Tom Moore on 2018-11-09 07:14:56 UTC
Comments
Hi Tom, thank you very much for the answer and suggestion! Recently, I decided to write a simple sensor fusion node for our drone, and now I have some questions.
- My IMU topic type is sensor_msgs/Imu. If I want to get the x,y,z velocity, do I just need to integrate the x,y,z acceleration?
Asked by JakeSheng on 2018-11-14 01:56:59 UTC
- The EKF updates predictions using one input and one observation right? I decide to take imu as the input. What about the SLAM position and GPS position? Do I need to make them into one observation?
Asked by JakeSheng on 2018-11-14 02:01:20 UTC
In robot_localization,the EKF predict function use the system model to calculate the predicted state_X,and all the sensors' message is being take to the measurements.It is the same as robot_pose_ekf in some kind.
Asked by nlwmode on 2019-12-12 21:02:00 UTC
Comments
@JakeSheng, did you manage to solve this? I'm doing something similar but using only the IMU and GPS on the Matrice 100. I find that my odometry filtered estimates just jumps around
Asked by marinePhD on 2018-11-20 11:36:20 UTC