# OBU for Accurate Navigation through Sensor Fusion in the Framework of the EMERGE Project
## 3 Sensor Fusion: Loosely Coupled Algorithm Implementation
### The Initialisation Loop
We further define the attitude axis update matrix as the coordinate transformation matrix from the body reference frame at the end of the update (\(b+\)) to the body reference system at the beginning (\(b-\))

\[\mathbf{C}_{b+}^{b-}=\exp[\mathbf{\alpha}_{ib}^{b}]_{\times}=\sum_{r=0}^{\infty} \frac{[\mathbf{\alpha}_{ib}^{b}]_{\times}^{r}}{r!}, \tag{11}\]

where \(\mathbf{\alpha}_{ib}^{b}=\mathbf{\omega}_{ib}^{b}\tau\) is the attitude increment. Truncating the formula to the fourth order gives the Rodrigues formula, used to calculate:

\[\mathbf{C}_{b}^{n}(t+\tau)\approx\bigg{[}\mathbf{I}-\bigg{(}\mathbf{\Omega}_{ie}^ {n}+\frac{1}{2}\mathbf{\Omega}_{en}^{n}(t)+\frac{1}{2}\mathbf{\Omega}_{en}^{n}(t+\tau )\bigg{)}\tau\bigg{]}\mathbf{C}_{b}^{n}(t)\mathbf{C}_{b+}^{b-} \tag{12}\]

where \(\mathbf{\Omega}_{en}^{n}(t+\tau)\) is calculated from \(L_{b}(t+\tau),\lambda(t+\tau),h_{b}(t+\tau)\). Velocity update:

\[\mathbf{v}_{eb}^{n}(t+\tau)=\mathbf{v}_{eb}^{n}(t)+[\mathbf{\epsilon}_{ib}^{n}+ \mathbf{g}_{b}^{n}(L_{b},h_{b})-(\mathbf{\Omega}_{en}^{n}+2\mathbf{\Omega}_{ie}^{n}) \mathbf{v}_{eb}^{n}(t)]\tau \tag{13}\]where the acceleration due to gravity, \(\mathbf{g}_{b}^{n}\), is modeled as a function of latitude and height, and \(\mathbf{f}_{ib}^{n}\) denotes the measured specific force resolved in the local navigation frame using the estimated \(\mathbf{C}_{b}^{n}\). Position update:

\[\mathbf{h}_{b}(t+\tau)\approx\mathbf{h}_{b}(t)-\frac{1}{2}[\mathbf{v}_{eb,D}^{ n}(t)+\mathbf{v}_{eb,D}^{n}(t+\tau)] \tag{14}\]

\[\mathbf{L}_{b}(t+\tau)\approx\mathbf{L}_{b}(t)+\frac{1}{2}\Bigg{[}\frac{ \mathbf{v}_{eb,N}^{n}(t)}{R_{N}(\mathbf{L}_{b}(t))+\mathbf{h}_{b}(t)}+\frac{ \mathbf{v}_{eb,N}^{n}(t+\tau)}{R_{N}(\mathbf{L}_{b}(t))+\mathbf{h}_{b}(t+\tau )}\Bigg{]}\tau \tag{15}\]

\[\boldsymbol{\lambda}_{b}(t+\tau)\approx\boldsymbol{\lambda}_{b}(t)+\frac{1}{2 }\Bigg{[}\frac{\mathbf{v}_{eb,E}^{n}(t)}{(R_{N}(\mathbf{L}_{b}(t))+\mathbf{h}_ {b}(t))\cos\mathbf{L}_{b}(t)}+\frac{\mathbf{v}_{eb,E}^{n}(t+\tau)}{(R_{N}( \mathbf{L}_{b}(t+\tau))+\mathbf{h}_{b}(t+\tau))\cos\mathbf{L}_{b}(t+\tau)} \Bigg{]}\tau \tag{16}\]

In order to have the velocity and position accuracy update, we considered the use of the mean transformation matrix \(\mathbf{\tilde{C}}_{b}^{n}\) in the transformation of the specific force in the NED coordinate system:

\[\mathbf{\tilde{C}}_{b}^{n}=\mathbf{C}_{b}^{n-}\mathbf{C}_{b}^{b-} \tag{17}\]

where

\[\mathbf{C}_{b}^{b-}=\mathbf{I}+\frac{1-\cos\left|\mathbf{a}_{ib}^{b}\right|} {\left|\mathbf{a}_{ib}^{b}\right|^{2}}\Big{[}\boldsymbol{\alpha}_{ib}^{b} \Big{]}_{X}+\frac{1}{\left|\mathbf{a}_{ib}^{b}\right|^{2}}\frac{1-\sin\left| \mathbf{a}_{ib}^{b}\right|}{\left|\mathbf{a}_{ib}^{b}\right|}\big{[} \boldsymbol{\alpha}_{ib}^{b}\big{]}_{X}^{2} \tag{18}\]

obtaining

\[\mathbf{f}_{ib}^{n}=\mathbf{\tilde{C}}_{b}^{n}\mathbf{\tilde{C}}_{b}^{b}, \qquad\mathbf{\tilde{C}}_{b}^{n}=\mathbf{C}_{b}^{n-}\mathbf{\tilde{C}}_{b}^{b -}-\frac{1}{2}(\boldsymbol{\Omega}_{ie}^{n}+\boldsymbol{\Omega}_{en}^{n}) \mathbf{C}_{b}^{n-}\tau \tag{19}\]

### ZUPT Detection Algorithm

To make the Algorithm 1 more efficient, the ZUPT algorithm is used to correct errors accumulated in the inertial navigation data when the system is stationary [34]. Thus, when the average velocity is below a fixed threshold of 0.5 m/s for a fixed time period of 4 s, the algorithm assumes the system is stationary and updates the current attitude and position based on the averages of the previous values.