Exploding covariance in one state, rest is OK

asked 2022-08-04 03:06:33 -0500

Dats gravatar image

updated 2022-09-29 09:45:29 -0500

Hi, Im working on an AUV and trying to use robot_localization EKF for pose estimation. Im using a Water Linked A125 DVL, a pressure sensor and the internal IMU of the Pixhawk flight controller. My estimations are looking somewhat good in terms of movement in Rviz, but the covariance for X in the output from the EKF is exploding compared to all the other states. I cannot understand how X and Y can be so wildly different.

Static transformations between sensors are done in the launch file and are accurate withing +/- 1cm.

Does anyone have any idea what might be the cause of my troubles, or see anything wrong in my parameter file?

General info: ROS Melodic OS: Ubuntu 18.04

Params Yaml:

frequency: 15
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: false
debug: false
publish_tf: true

publish_acceleration: false


# Frames

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


# Imu angular vel
imu0: /mavros/imu/data
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              true, true, true,
              false,false,false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_linear_acceleration_rejection_threshold: 4  #
imu0_remove_gravitational_acceleration: true


#DVL
odom0: /arrov/dvl/velocity
odom0_config: [false,  false, false, # x, y, z
               false, false, false,  # roll, pitch, yaw
               true, true, true,     # vx, vy, vz
               false, false, false,   # vroll, vpitch, vyaw
               false, false, false]  # ax, ay, az
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
#odom0_pose_rejection_threshold: 5
#odom0_twist_rejection_threshold: 1

#Depth sensor - Z value
odom1: /arrov/odom
odom1_config: [false,  false, true, # x, y, z
               false, false, false,  # roll, pitch, yaw
               false, false, false,     # vx, vy, vz
               false, false, false,   # vroll, vpitch, vyaw
               false, false, false]  # ax, ay, az 
odom1_queue_size: 10
odom1_nodelay: false
odom1_differential: false
odom1_relative: false

initial_state: [ 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]

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.05, 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.01, 0,     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.1,   0,    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.1,  0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0 ...
(more)
edit retag flag offensive close merge delete

Comments

Please include a sample message from every sensor input.

Tom Moore gravatar image Tom Moore  ( 2022-09-26 04:17:56 -0500 )edit

Hi Tom, thanks for your reply. I've added examples of all the sensor messages I input to the EKF. I apologize for the bad formatting. As I am new to the forum I am not allowed to add images to my post. If there is anything else you need, please let me know. All help is truly appreciated.

Dats gravatar image Dats  ( 2022-09-29 09:47:24 -0500 )edit