What's the correct approach to merging of localization and odometry data?

Question

We've got a mobile platform with a source of odometry and an IMU, which are merged in an EKF filter (robot_localization node), producing continuous odom->base_link transform. The robot is also equipped with a lidar, that we use for SLAM. Now, since the robot's pose estimate coming from SLAM has a known covariance, I used a second EKF, merging the odometry, IMU and SLAM pose and producing the map->odom transform. As you can see, I followed the standard approach to use two EKFs, where the first one is merging continuous data and the second all data.

From what I understood, the pose coming from SLAM is not continuous (similary to GPS signal). On the other hand, I noticed that visual odometry is usually considered continuous, thus only one EKF is used. However, the map->odom transform is not static, so the second EKF looks like a good way to update it dynamically.

If we stick with two separate EKFs as mentioned in the beginning, what are the state of the art techniques to take into account the quality of previous position estimates? For example, if the robot starts at a docking station, the initial position is well known and shouldn't be too much affected by the map->odom transform. Or if the initial position has a large covariance, then the ongoing computation could actually improve it.

Thanks for any suggestion or links to articles regarding this topic.

Answer 1

what are the state of the art techniques to take into account the quality of previous position estimates?

The Kalman filter and extended Kalman filter use the measurement and model covariances to balance the "quality" of estimated (model-based) and measured (feedback-based) position estimates.

From the Wikipedia entry on Kalman filters:

$w_k$ is the process noise which is assumed to be drawn from a zero mean multivariate normal distribution, $\mathcal{N}$, with covariance, $Q_k$

$v_k$ is the observation noise which is assumed to be zero mean Gaussian white noise with covariance $R_k$

and from the Wikipedia entry on extended Kalman filters:

$w_k$ and $v_k$ are the process and observation noises which are both assumed to be zero mean multivariate Gaussian noises with covariance $Q_k$ and $R_k$ respectively.

So the covariances are built into the Kalman filter already. It's all taken care of for you :)