Revision history [back]

My response is for robot_localization. Have a read through the wiki, but pay particular attention to this page. In particular, note that inflating covariances for variables you want to ignore is not going to produce good results for the state estimation nodes in robot_localization. Instead, use the configuration parameters to fuse only the variables you want.

Having said that, you need to make sure that you are accounting for all of your pose (position and orientation) variables. If you don't account for any one of them, you'll see the covariances explode, and your state estimate will act unpredictably. For position, you can either have absolute measurements (X, Y, and Z) or velocity measurements (X velocity, Y velocity, and Z velocity). For orientation, you will want to have at least one source of absolute measurement data.

Some questions for you:

1. Do you have an IMU on your robot?
2. Do your AR code-based measurements yield position only, or do they also produce orientation?
3. Is this robot only operating in 2D? If it is, then you should either set two_d_mode to true, or let your AR code measurements produce estimates for Z, roll, and pitch. Make them 0, and make the covariances very small.

Your setup is pretty much identical to a system with odometry and GPS, and that setup works well with robot_localization. Without knowing more, what I would do is this:

1. Run one instance of ekf_localization_node with the world_frame parameter set to the same value as the odom_frame parameter. In this instance, fuse your odometry and IMU if it's available. If you only have odometry, then don't run this instance. Instead, make sure that your odometry source is also producing an odom->base_link (or whatever your frames are) transform.
2. Run a second instance of ekf_localization_node, and fuse the odometry, IMU (again, if available), and AR measurements. Set the world_frame parameter set to the same value as the map_frame parameter.

How you deal with orientations will be important here, so I'll need more information before I can offer up any more advice. Please post a sample message from each input source, and also please post your ekf_localization_node launch file.

My response is for robot_localization. Have a read through the wiki, but pay particular attention to this page. In particular, note that inflating covariances for variables you want to ignore is not going to produce good results for the state estimation nodes in robot_localization. Instead, use the configuration parameters to fuse only the variables you want.

Having said that, you need to make sure that you are accounting for all of your pose (position and orientation) variables. If you don't account for any one of them, you'll see the covariances explode, and your state estimate will act unpredictably. For position, you can either have absolute measurements (X, Y, and Z) or velocity measurements (X velocity, Y velocity, and Z velocity). For orientation, you will want to have at least one source of absolute measurement data.

Some questions for you:

1. Do you have an IMU on your robot?
2. Do your AR code-based measurements yield position only, or do they also produce orientation?
3. Is this robot only operating in 2D? If it is, then you should either set two_d_mode to true, two_d_mode to true, or let your AR code measurements produce estimates for Z, roll, and pitch. Make them 0, and make the covariances very small.

Your setup is pretty much identical to a system with odometry and GPS, and that setup works well with robot_localization. Without knowing more, what I would do is this:

1. Run one instance of ekf_localization_node with the world_frame parameter set to the same value as the odom_frame parameter. In this instance, fuse your odometry and IMU if it's available. If you only have odometry, then don't run this instance. Instead, make sure that your odometry source is also producing an odom->base_link (or whatever your frames are) transform.
2. Run a second instance of ekf_localization_node, and fuse the odometry, IMU (again, if available), and AR measurements. Set the world_frame parameter set to the same value as the map_frame parameter.

How you deal with orientations will be important here, so I'll need more information before I can offer up any more advice. Please post a sample message from each input source, and also please post your ekf_localization_node launch file.