1
$\begingroup$

Firstly, I have had a look at other questions targeted around exploding covariances, yet I still cannot figure out what is wrong with my setup.

I have multiple static cameras that detect a moving AR marker, so they get absolute pose x, y, z, roll, pitch, yaw. I want to fuse the pose estimations from each camera using the ekf_node from robot_localization.

The issue is that the covariances explode very quickly. I have just limited my input to the node to use one camera for now. I have calculated the covariance for this camera by collecting pose data over roughly a minute, and calculating the variance of that data.

My TF tree looks like this. The pose data is outputted to (for just one of the cameras) /zed_left/marker_pose. It is the pose data of marker_39_link, which is in the frame of zed_left_camera_link.

TF tree

I figured that all I should have is a global ekf node in this case, which will publish the map->odom, so I've set up a static transform odom->marker_39_link (my base_link)

Have I set up all the frames correctly?

Here is my ekf.yaml:

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
        # computation until it receives at least one message from one of the inputs. It will then run continuously at the
        # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 15.0

        # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
        # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
        # filter will generate new output. Defaults to 1 / frequency if not specified.
        sensor_timeout: 0.1

        # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
        # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
        # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
        # by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

        # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
        # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
        # unspecified.
        transform_time_offset: 0.0

        # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
        # Defaults to 0.0 if unspecified.
        transform_timeout: 0.0

        # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
        # unhappy with any settings or data.
        print_diagnostics: true

        # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
        # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
        # effects on the performance of the node. Defaults to false if unspecified.
        debug: true

        # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
        debug_out_file: /home/mcav-fyp/fyp_ws/src/adr/infrastructure/marker_localization/ekf_debug.txt

        # Whether we'll allow old measurements to cause a re-publication of the updated state
        permit_corrected_publication: false

        # Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: false

        # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
        publish_tf: true

        # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
        # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
        # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
        # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
        # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
        # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
        # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
        # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
        # Here is how to use the following settings:
        # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
        #     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
        #         odom_frame.
        # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
        #   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
        # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
        # from landmark observations) then:
        #     3a. Set your "world_frame" to your map_frame value
        #     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
        #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: marker_39_link # Defaults to "base_link" if unspecified
        world_frame: map # Defaults to the value of odom_frame if unspecified

        # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
        # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
        # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
        # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
        # default values, and must be specified.
        pose0: zed_left/marker_pose

        # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
        # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
        # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
        # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
        # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
        # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
        # if unspecified, effectively making this parameter required for each sensor.
        pose0_config: [true,  true,  true,
                       true, true, true,
                       false, false, false,
                       false, false, false,
                       false, false, false]
        pose0_queue_size: 5
        pose0_differential: false
        pose0_relative: false
        pose0_pose_rejection_threshold: 2.0

        #pose1: zed_right/marker_pose
        #pose1_config: [true,  true,  true,
        #               false, false, false,
        #               false, false, false,
        #               false, false, false,
        #               false, false, false]
        #pose1_differential: true
        #pose1_relative: false
        #pose1_queue_size: 5
        #pose1_rejection_threshold: 2.0  # Note the difference in parameter name

        use_control: false

        stamped_control: false

        # The last issued control command will be used in prediction for this period. Defaults to 0.2.
        control_timeout: 0.2

        # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
        control_config: [false, false, false, false, false, false]

        # Places limits on how large the acceleration term will be. Should match your robot's kinematics.
        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

        # Acceleration and deceleration limits are not always the same for robots.
        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

        # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
        # gains
        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

        # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
        # gains
        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

        # [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.
        # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support
        # both full specification or specification of only the diagonal values.
        #process_noise_covariance: [0.05, 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.05, 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.05, 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.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.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,     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.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,     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.0,    0.0,    0.00, 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.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,    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.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,    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,    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,    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,    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.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.025, 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.025, 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.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.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,    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.015]

        # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
        # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
        # question. Users should take care not to use large values for variables that will not be measured directly. The values
        # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below
        # if unspecified. In this example, we specify only the diagonal of the matrix.
        initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]

Here is a screenshot of the debug output:

Screenshot of the debug output

I'm also a bit confused by what the odom frame should be in this case... because it should be fixed to the marker position but I'm unsure because I'm not measuring odometry directly? I'm only measuring global data

$\endgroup$
0

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.