I am using ros2 humble and the extended Kalman Filter from the robot_localization package to get both the odom -> base_link and map -> odom transform.
The first work quite well, but I am struggling a bit with the second one. I use the navsat transform node to get an odometry from the fix data, and it seems to work well, the position shown in rviz is pretty coherent.
I use a system called RTK, which gives me really accurate GPS position (we are talking centimeter level precision) and it seems also well reported in the odometry message.
However, when I input this in my Kalman Filter, I notice a strange behavior : the robot's position is delayed compared with the gps odometry, in fact it seems that it won't move at the start of the movement, while it do when I only input my IMU and odometry to my EKF (for the odom->base_link transform).
I would have thought that adding a more precise measurement could only make it more precise, but in this case with navigation, it get worse from a time perspective and it is uncontrollable. I would have thought that the estimation would take the value of the GPS odometry as it's current position, and use the velocity in between two values of GPS odometry to smooth the measurement.
I tried to increase the process noise covariance, without success.
Here is my configuration for the ekf node :
ekf_global:
ros__parameters:
frequency: 30.0 # 30.0
sensor_timeout: 0.05 #2.0
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.2
print_diagnostics: false
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom_combined # Defaults to "odom" if unspecified
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
world_frame: map # Defaults to the value of odom_frame if unspecified
odom0: odom_combined
odom0_config: [false, false, false,
false, false, true,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 1
odom0_nodelay: false
odom0_differential: true
odom0_relative: false
odom1: /odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 1
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
imu0: mobile_base/sensors/imu_data
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: true
imu0_relative: false
imu0_queue_size: 4 #4
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [5.50, 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.05
0.0, 3.50, 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.05
0.0, 0.0, 0.06, 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.03, 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.03, 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.15, 0.0, 0.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, 1.35, 0.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, 1.20, 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, 0.04, 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.01, 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.01, 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.2, 0.0, 0.0, 0.0, #0.02
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, #0.01
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, 1.1, 0.0, #0.01
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.015]
initial_estimate_covariance: [1e-5, 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, #1e-9
0.0, 1e-5, 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, #1e-9
0.0, 0.0, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9, 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, 1e-9]
If anybody know what I could try, I would be glad to hear it, and thanks for reading !
