Geometric Algorithms for Sensor Fusion: From Calibration to State Estimation

Abstract

In this thesis, we present geometric algorithms for fusing measurements provided by various kinds of motion sensors such as cameras, inertial sensors, encoders, etc. Among many issues related to sensor fusion, we particularly consider the problem of two-frame sensor calibration, and estimation of attitudes and gyro bias.

Firstly, in the two-frame sensor calibration problem, the objective is to find rigid-body homogeneous transformation matrices $X,Y$ that best fit a set of equalities of the form $\mathbf{A}_i \mathbf{X} = \mathbf{Y} \mathbf{B}_i$, $i=1, \ldots, N$, where $\{(\mathbf{A}_i, \mathbf{B}_i)\}$ are two sets of homogeneous transformation matrices obtained from two different sensor measurements. A fast and numerically robust local optimization algorithm for the two-frame sensor calibration objective function is proposed. Using coordinate-invariant differential geometric methods that take into account the matrix Lie group structure of the rigid-body transformations, our local descent method makes use of analytic gradients and Hessians, and a strictly descending fast step-size estimate to achieve significant performance improvements. Furthermore, we present a two-phase stochastic geometric optimization algorithm for finding a stochastic global minimizer based on our earlier local optimizer. Numerical simulation and real experiments demonstrate that our algorithm is superior to existing unit quaternion-based methods in terms of robustness and efficiency.

Secondly, we consider the problem of estimating attitudes and gyro bias by using inertial and magnetic sensors. To address this issue, we present an intrinsic unscented Kalman filtering (UKF) algorithm, of which novelty can be traced to the design of measurement function. In our formulation, the measurement has the form of $SO(3)$, which is given by the solution to Wahba's problem. Its merit is that the measurement noise covariance can consider the constraint on two direction vectors and is also well-defined with a full rank. Moreover, we present an offline algorithm for determining the parameters in this covariance from measurements of gravity and geomagnetic field by using actual accelerometers and magnetometers. Synthetic and real experiments show that our algorithm outperforms the existing state-of-the art estimators in terms of both convergence behavior and accuracy.