# An Enhanced Fusion Strategy for Reliable Attitude Measurement Utilizing Vision and Inertial Sensors
## 2 Problem Formulation and System Modeling  
### Multi-Frequency Problem in Vision and Inertial Heterogeneous Sensor Integrated System  
For a heterogeneous sensors integrated system, there is always a sampling frequency discrepancy problem, namely the multi-frequency problem. The sampling frequencies of different sensors are probably inconsistent. The vision and inertial integrated attitude measurement system, is constructed of two heterogeneous sensors, namely the MEMS-based gyroscope and camera. The sampling frequency of the camera is often several or dozens of Hertz while that of the gyroscope is hundreds of Hertz. There is considerable sampling frequency discrepancy between them, as shown in Figure 1.  
Furthermore, the multi-frequency problem can lead to divergence in attitude results in the vision and inertial integrated measurement system. For both available visual and inertial data, CKF deals with the nonlinear filter problem by fusing them together. Inertial measurements are incorporated in the filter loop as inputs, and visual attitudes are calculated out of the filter loop as the observation vectors. Thus, the nonlinear filter model consists of the nonlinear state model and linear observation model [27].  
\[\left\{\begin{array}{l}x_{k}=f(x_{k-1},u_{k-1})+w_{k-1}\\ z_{k}=H_{k}x_{k}+v_{k}\end{array}\right. \tag{1}\]  
where \(x_{k}\) is the state vector and \(u_{k-1}\) is the control input from the high frequency angular velocity of gyroscope. \(z_{k}\) is the observation vector from low frequency visual measurements. \(H_{k}\) is the measurement matrix, \(w_{k-1}\) and \(v_{k}\) are independent zero-mean Gaussian white process noise with covariances \(Q_{k-1}\) and \(R_{k}\).  
When there are both vision and inertial data, the normal filtering can be used to fuse data together and output the optimal attitude estimation. However, during the intersampling of slow vision data, there is still a significant amount of inertial gyroscope data. To ensure the high frequency of attitude output, the attitude differential equation is used to update attitude angles. The specific formula is given as follows:  
\[\mathbf{q}(k+1)=(\cos(\frac{|\omega|\Delta t}{2})\mathbf{I}+2\frac{\sin(\frac {|\omega|\Delta t}{2})}{|\omega|}\mathbf{\Omega}(\omega))\mathbf{q}(k) \tag{2}\]  
where  
\[\mathbf{\Omega}(\omega)=\frac{1}{2}\left[\begin{array}{cccc}0&-\omega_{x}&- \omega_{y}&-\omega_{z}\\ \omega_{x}&0&\omega_{z}&-\omega_{y}\\ \omega_{y}&-\omega_{z}&0&\omega_{x}\\ \omega_{z}&\omega_{y}&-\omega_{x}&0\end{array}\right] \tag{3}\]  
\(\omega=\left[\omega_{x}\;\omega_{y}\;\omega_{z}\right]^{T}\) is the angular velocity, \(\mathbf{q}\) is the rotation quaternion parameter, and \(\mathbf{I}\) is a unit matrix with proper dimensions.  
Figure 1: Sampling frequency discrepancy between visual and inertial data.  
To obtain an updated attitude by solving the above formula in an integral operation, the measurement errors can accumulate quickly with time due to bias error drift by the MEMS gyroscope. There are two reasons for this. The existing manufacturing level of the general MEMS sensor has too much bias and noise contained in its output signal. Besides, the integral operation means that the bias and noise will inevitably accumulate with time. The two factors makes it difficult to guarantee the convergence during the intersampling of visual data, especially when the interval lasts for a period of time.