ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

robot_localization ekf_node fuse 2hz depth measurement

asked 2021-06-04 16:45:22 -0600

crazymumu gravatar image

Hi, I am using robot_localization ekf to fuse 100 hz imu, 4hz twist(x,y,z velocity) and 2hz pose(only z position). The pose is only has z, which is the position measured from pressure sensor then convert to odom frame. What I am really want to do is using this z as the final result in the filtered odometry, like trust this measurement the most.

The param file is here: check. The depth is just keeping draft, the pose seems not helped in the ekf. Any suggestions? Thanks for the help.

edit retag flag offensive close merge delete

2 Answers

Sort by ┬╗ oldest newest most voted
0

answered 2021-06-07 09:35:48 -0600

crazymumu gravatar image

I adjust the configure, not it looks good.

frequency: 50
#sensor_timeout: 1.0
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
# debug_out_file: /path/to/debug/file.txt
publish_tf: true

# publish_acceleration: false
smooth_lagged_data: true
history_length: 1.0

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

# [X,      Y,       Z,
#  roll,   pitch,   yaw,
#  V_x,    V_y,     V_z,
#  V_roll, V_pitch, V_yaw,
#  ax,     ay,      az]

imu0: /rov/processed/ahrs/imu_madgwick
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_remove_gravitational_acceleration: true      # the raw data is not removed
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 100
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #

twist0: /rov/processed/dvl/twist_filtered
twist0_config: [false, false, false,
                false, false, false,
                true, true, true,
                false, false, false,
                false, false, false]
twist0_nodelay: true
twist0_differential: false
twist0_relative: false
twist0_queue_size: 20

pose0: /rov/processed/dvl/depth
pose0_config: [false, false, true,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 10
pose0_rejection_threshold: 2  # Note the difference in parameter name
pose0_nodelay: true

# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.

### set larger value for trusted sensor measurements
process_noise_covariance: [1e-8, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    1e-8, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    1.0,  0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.5, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0 ...
(more)
edit flag offensive delete link more
0

answered 2021-06-08 05:05:05 -0600

Is your process noise of z suppose to be this high? (1)

What you can do is decrease the covariance of the pose (with z) and increase the covariance in the other topics like imu. This way the system will know to trust the z input more than the other.

edit flag offensive delete link more

Comments

actually, process noise of z to 1 is good. check the gps example. They set gps measurement related process noise to 1. Also, large process noise will cause the filter to trust the incoming measurement more during correction.

And my result is good so far.

crazymumu gravatar image crazymumu  ( 2021-06-08 09:38:19 -0600 )edit

Question Tools

3 followers

Stats

Asked: 2021-06-04 16:45:22 -0600

Seen: 73 times

Last updated: Jun 08 '21