## 4 FAST-FUSION Approach
### Orientation Correction  
After having a stable camera motion estimation, the need to support it in rotation-only motion types emerged due to high errors in the estimation in these cases. To do so, a gyroscope is used fusing it with the angular velocity resultant from the VO approach using a KF. Besides the angular velocity, the gyroscope bias is also estimated in order improve the motion estimation accuracy. Only the yaw component is estimated because the robot does not present harsh rotations in the other components. Even so, the model is extendable for all the components. So, the state vector is \(x=[\omega_{x}\,\omega_{y}\,\omega_{z}\,b_{x}\,b_{y}\,b_{z}]^{T}\), where \(\omega_{x},\omega_{y},\omega_{z}\) are the angular velocities states and \(b_{x},b_{y},b_{z}\) the gyroscope bias. The control vector comes from the VO estimation as \(u=[\Delta\theta_{x}\,\,\Delta\theta_{y}\,\,\Delta\theta_{z}]^{T}\) and the observations are \(z=[^{G}\omega_{x}\,^{G}\omega_{y}\,^{G}\omega_{z}]^{T}\) from the gyroscope. This being said, the state model is as follows:  
\[\hat{x}_{k+1|k}=A\hat{x}_{k|k}+Bu_{k}\Leftrightarrow \tag{11}\]  
Figure 6: Two examples (**a**,**b**) of the LIDAR measurements projection in the omnidirectional image and 2-D features association with them.  
\[\Leftrightarrow\hat{x}_{k+1|k}=\begin{bmatrix}0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&1&0&0\\ 0&0&0&0&1&0\\ 0&0&0&0&0&1\end{bmatrix}\hat{x}_{k|k}+\begin{bmatrix}\Delta t&0&0\\ 0&\Delta t&0\\ 0&0&\Delta t\\ 0&0&\Delta t\\ 0&0&0&0\\ 0&0&0\end{bmatrix}u_{k} \tag{12}\]  
with results in:  
\[\hat{\omega}_{i_{k}}=\Delta\theta_{i_{k}}\Delta t \tag{13}\]  
\[\hat{b}_{i_{k}}=b_{i_{k-1}} \tag{14}\]  
with \(i\in\{x,y,z\}\). We considered bias as a constant state ignoring flicker noise and temperature oscillations. Even so, this is a reasonable approximation due to the low impact of these two components in time-limited estimations. In addition the observations model is:  
\[\hat{z}_{k+1}=H\delta_{k+1|k}\Leftrightarrow\hat{z}_{k+1}\left[\begin{array}[ ]{cccccc}1&0&0&1&0&0\\ 0&1&0&0&1&0\\ 0&0&1&0&0&1\end{array}\right]\hat{x}_{k+1|k} \tag{15}\]  
that results, for both components, in an equation of the following form:  
\[{}^{G}\omega_{i}=\hat{\omega}_{i}+\hat{b}_{i} \tag{16}\]  
with \(i\in\{x,y,z\}\). This equation can be interpreted as: the angular velocity state is equal to the gyroscope observation minus the bias estimation. So, it is expected that this performs a correction of the angular velocity observation that is used in the computation of the state.  
The VO approach in some cases provides unrealistic estimations in pure rotations. Due to this, the covariance matrix of the state \(Q\) is dynamic. Both \(Q\) and the observations covariance \(R\) are initialized with constant values on their diagonals. The values of the diagonal of \(Q\) correspondent to the bias states are decreased over time, since bias is considered as a constant state. To detected and cancel the unrealistic peaks of angular velocity on the state, a non-linear approach was adopted. In this, a sigmoid function that varies with the angular velocity states was calibrated. So, if a peak of angular velocity is detected, the sigmoid increases the covariance noise of the angular velocity which leads the filter to consider the gyroscope measure instead.  
In this way, it is possible to have in consideration two different sources of information to estimate the robot rotation.