Factor Graph Based Sensor Fusion

Motivation

In general, sensor fusion or multi-sensor fusion refers to the concept of estimating one or more quantities based on various sensor information. Especially in mobile robotics, we need an estimate of the robot’s own state – usually with regard to some reference system. Based on it, higher algorithms make decisions e.g. for planning and navigation. Errors within the estimation process may lead to catastrophic outcomes for the robot. Such errors include faulty mean but also wrong probability estimates. Possible reasons include non-linearities of the system, outliers within the sensors’ information, insufficient models of the sensors and its corresponding error distribution, or simply time delays.

Bayesian filtering algorithms, like the Extended Kalman Filter (EKF) or variants of it, like the Unscented Kalman Filter (UKF), are state of the art for fusing sensor information in mobile robotics. However, the above stated error sources are only difficult or insufficiently managable by these approaches and need complex extensions.

Our Approach

In our work, we concentrate on the concept of factor-graph based sensor fusion, where we can address the above mentioned sources of error more intuitively. Simply speaking, factor graphs are a graphical representation of an underlying optimization problem, with all its pros and cons. Especially regarding the computation time, such solutions may be critical. However, by using a sliding-window approach (fixed-lag smoothers) or a newer adaptive-lag approach (e.g. implemented by the popular iSAM2 algorithm), real-time capabilities are feasible.

In our opinion, the pros outweigh possibly increasing computational costs in comparison to usual filter techniques – note, there are also examples in the SLAM domain, where the computational cost of an EKF based approach is higher in comparison. Especially, in incremental sensor fusion applications of a non-linear system without global sensor information – like GNSS – Kalman filter based approaches may accumulate errors based on imprecise chosen linearisation points. Because the filter based approach is marginalizing out the previous state and sensor information, such an error can not be recovered and may lead not only to wrong estimates of the system’s state (mean) but also to inconsistent covariance estimates. In particular, growing uncertainty in the believe state typically leads to failure. By using the factor-graph based approach, we can address such problems. Likewise, a good chance of recovering from faulty linearisation points is present.

In our previous work, we compared the factor-graph based approach to the conventional EKF approach on the use-case of a UAS. In current work, we will extend this efforts in terms of various sensor configurations on our ground based mobile robots. Additionally, our work on robust and multi-modal factor-graph based optimization will be used in our new sensor fusion framework.

Publications

• ?. ()
• ?. ()
• ?. ()
• ?. ()