Fusing GPS in robot_localization
Hi, I am trying to fuse GPS with all other odom and IMUs My local odometry has barely any drift now. The only issue is that the direction of the GPS odometry seems to be rotated with respect to the other odometry..
you can see that the robot is driving towards the left but the GPS is translating upwards. particular attention to imu0
ekf_filter_node_odom:
ros__parameters:
use_sim_time: true
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: false
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /wheel/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: true
odom1: /visual_slam//odometry
odom1_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: true
odom1_relative: false
odom2: /lidar/odom
odom2_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom2_queue_size: 10
odom2_nodelay: true
odom2_differential: false
odom2_relative: false
imu0: /imu/with/absolute/orientation/returns 0 when facing East
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true # IS THIS CORRECT? IF IT IS FALSE, THE DIRECTION OF base_link FACES ANOTHER DIRECTION FROM THE OTHER ODOMETRY SOURCES TRANSLATION DIRECTION
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
imu1: /imu1
imu1_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu1_nodelay: false
imu1_differential: true
imu1_relative: false
imu1_queue_size: 10
imu1_remove_gravitational_acceleration: false
imu2: /imu2
imu2_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu2_nodelay: false
imu2_differential: true
imu2_relative: false
imu2_queue_size: 10
imu2_remove_gravitational_acceleration: false
imu3: /imu3
imu3_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu3_nodelay: false
imu3_differential: true
imu3_relative: false
imu3_queue_size: 10
imu3_remove_gravitational_acceleration: false
use_control: false
process_noise_covariance: ...
initial_estimate_covariance: ...
ekf_filter_node_map:
ros__parameters:
use_sim_time: true
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: false
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: /wheel/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: true
odom1: /visual_slam//odometry
odom1_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: true
odom1_relative: false
odom2: /lidar/odom
odom2_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom2_queue_size: 10
odom2_nodelay: true
odom2_differential: false
odom2_relative: false
odom3: /odometry/gps
odom3_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom3_queue_size: 10
odom3_nodelay: true
odom3_differential: false
odom3_relative: false
imu0: /imu/with/absolute/orientation/returns 0 when facing East
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true # IS THIS CORRECT? IF IT IS FALSE, THE DIRECTION OF base_link FACES ANOTHER DIRECTION FROM THE OTHER ODOMETRY SOURCES TRANSLATION DIRECTION
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: false
imu1: /imu1
imu1_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu1_nodelay: false
imu1_differential: false
imu1_relative: false
imu1_queue_size: 10
imu1_remove_gravitational_acceleration: false
imu2: /imu2
imu2_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu2_nodelay: false
imu2_differential: false
imu2_relative: false
imu2_queue_size: 10
imu2_remove_gravitational_acceleration: false
imu3: /imu3
imu3_config: [false, false, false,
false, false, false,
false, false, false,
true, true, true,
false, false, false]
imu3_nodelay: false
imu3_differential: false
imu3_relative: false
imu3_queue_size: 10
imu3_remove_gravitational_acceleration: false
use_control: false
use_control: false
process_noise_covariance: ...
initial_estimate_covariance: ...
navsat_transform:
ros__parameters:
use_sim_time: true
broadcast_cartesian_transform: true
broadcast_cartesian_transform_as_parent_frame: true
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame_: false
delay: 0.0
frequency: 10.0
magnetic_declination_radians: -0.1188569
publish_filtered_gps: true
transform_timeout: 0.0
use_local_cartesian: false
use_odometry_yaw: false
wait_for_datum: true
datum: [1.2824466988410665, 103.86530962903208,0.0]
yaw_offset: 0.0
zero_altitude: false
gps odometry on the left, local odometry on the right
Asked by MrOCW on 2022-07-05 23:23:48 UTC
Answers
The only issue is that the direction of the GPS odometry seems to be rotated with respect to the other odometry
This says that you have your yaw_offset or magnetic_declination parameters set incorrectly. It could also be a timing thing, especially with the way you have your filter set up. More on that later.
First, you are fusing absolute pose data from four different sources:
/visual_slam//odometry: position and orientation/lidar/odom: position/imu/with/absolute/orientation: orientation/odometry/gps: position
(Your visual slam topic has two slashes in it, by the way).
Anyway, I sincerely doubt these sources are going to agree with one another over time, and so over time, the pose is going to start jumping between them, even without the GPS data. You should choose one as your most trustworthy pose source, and fuse velocity data from the others, or put them in differential mode if they don't provide velocity.
Just so we're clear, navsat_transform_node requires your IMU to read 0 facing east. The EKF doesn't care. navsat_transform_node needs three things:
- An IMU with an earth-referenced heading
- A GPS fix
- Your robot's current pose in its world frame
It doesn't care what (3) is, so long as it's right-handed. It doesn't have to agree at all with the GPS or IMU that you use in n_t_n. You could just use raw wheel odometry alone for your EKF state estimate, and it would work.
But you need to be careful here. If this hapens:
- EKF kicks off, gets a single IMU measurement from, e.g.,
imu3. So its first output has a pose and position of 0, with whatever angular velocity was in that message. navsat_transform_nodeis ready, and gets all its input data, including that first pose from the EKF (i.e., input 3 above). That pose is 0.- EKF fuses pose from some other source. Let's say it's from your SLAM system, that says you are at some non-zero position and with non-zero orientation. Now you've effectively changed your EKF's coordinate frame (i.e., where its origin is). But
navsat_transform_nodehas the wrong EKF pose, so when it transforms the GPS data, it will be wrong.
What you want is:
- EKF fuses data from all pose sources (again, you preferably one have one source for absolute pose)
- navsat_transform_node` is ready, and gets all its input data, including that first pose from the EKF
- Now all posese from
n_t_nshould be consistent with your robot's world frame.
This is why n_t_n has the delay parameter. It gives you time to get a sensor measurement from all your sensors into the EKF before it starts.
Update in response to comments:
should I then set lidar odom to differential=true and gps as absolute x y source?
Up to you, really, but as you have it now, your pose will eventually diverge from the GPS path (lidar odometry will be integrating laser scan matches, I presume, so it will gain error over time).
except imu as i thought n_t_n requires absolute IMU orientation
It does. See the requirements for what n_t_n expects above. I think you may be conflating IMU inputs to your EKF instance and IMU inputs to n_t_n. They are totally unrelated, unless you tell n_t_n to use the state estimate's yaw (use_odometry_yaw), and you don't have that configured (and shouldn't).
is this under the assumption that I have a fix the moment the global map->base_link ekf begins?
Not really, no. When n_t_n starts, it receives input (3) above from the EKF. The first time the EKF publishes, it may not have received an IMU measurement, so the EKF's first output has a yaw of 0. So n_t_n computes the transform from the GPS (really, UTM) frame to the robot's world frame, but the transform is based on that first pose from the EKF, which has a 0 yaw. Then the EKF gets its first IMU measurement, and it has a yaw of, e.g., 2.7 radians. So from then on, the transformed pose coming from n_t_n will be inconsistent with the robot's direction of travel. Instead, if n_t_n had waited until the EKF had received its initial orientation, then it would have based that UTM-to-robot transform on the heading of 2.7 radians, and the transform would be different.
The easiest way around this is for your EKF to only fuse orientation velocity (or at least yaw velocity). Then the robot's heading will start at 0, and will not suddenly jump in the next time step.
Asked by Tom Moore on 2022-07-28 09:34:33 UTC
Comments
"Anyway, I sincerely doubt these sources are going to agree with one another over time, and so over time, the pose is going to start jumping between them, even without the GPS data. You should choose one as your most trustworthy pose source, and fuse velocity data from the others, or put them in differential mode if they don't provide velocity."
in my config, there is only 1 absolute source (most trustworthy lidar odom), i've set the others to differential=true or only fuse the velocities (except imu as i thought n_t_n requires absolute IMU orientation), except in the global odom with lidar and gps odom providing absolute positions. should I then set lidar odom to differential=true and gps as absolute x y source?
requires your IMU to read 0 facing east
Yes it is 0 when facing east, hence yaw_offset = 0
But you need to be careful here. If this hapens
is this under the assumption that I have a fix the moment the global map->base_link ekf begins?
Asked by MrOCW on 2022-07-28 09:47:49 UTC
Comments