# OBU for Accurate Navigation through Sensor Fusion in the Framework of the EMERGE Project
## 3 Sensor Fusion: Loosely Coupled Algorithm Implementation
### The Initialisation Loop
```
1:gnss_eps = \(\epsilon\); %duration in seconds
2:for\(i=1\) to init_imu_samplesdo
3:if\(i==1\)then
4:\(tor_{i}\leftarrow\frac{1}{frequency_{IMU}}\);
5:else
6:\(tor_{i}\gets t_{IMU}(i)-t_{IMU}(i-1)\);
7:endif
8:\(f_{\_}{ib}^{b}\gets fb_{IMU}(i)\);
9:\(\omega_{ib}^{b}\gets wb_{IMU}(i)\);
10:is\(gnss\_available\leftarrow\) false;
11:\(gdx\leftarrow\text{find}(t_{GNSS}\geq(t_{IMU}(i)-\)gnss_eps)\) and \(t_{GNSS}<(t_{IMU}(i)+\)gnss_eps));
12:
13:%Check if there is a new GNSS measurement to process at current INS time
14:if(!isempty(\(gdx\)) and \(gdx>1\)) then
15:\(is\_gnss\_available\leftarrow\) true;
16:\(last\_gdx\gets gdx\);
17:gnss position \(GNSS\_r\leftarrow[\)gnss_lat(\(gdx\));gnss_lon(\(gdx\));gnss_h(\(gdx\))];
18:endif
19:\(f\_{\_}{leveling}_{ib}^{b}\leftarrow[f\_{\_}{leveling}_{ib}^{b},f_{ib}^{b}]\);
20:\(\omega\_{\_}{leveling}_{ib}^{b}\leftarrow[\omega\_{\_}{leveling}_{ib}^{b},\omega_{ib}^{b}]\);
21:if\(is\_gnss\_available\)then
22:\(GNSS\_r\_init=[GNSS\_r\_init,GNSS\_r]\);
23:endif
24:endif
25:if\(i>\)init_imu_samples_level and \(\text{isempty}(GNSS\_r\_init)) then
26:\(ave\_{\_}{fib}^{b}\leftarrow\text{mean}(f\_{\_}{leveling}_{ib}^{b},2)\);
27:\(roll\leftarrow\text{atan2}(-ave\_{\_}{fib}^{b}(2),-ave\_{\_}{fib}^{b}(3))\);
28:\(pitch\leftarrow\text{atan}(ave\_{\_}{fib}^{b}(1)/\sqrt{ave\_}{f_{

### Specific Force and Angular Velocity Error Model

The primary sources of error are modelled as follows. Given the true specific force and angular velocity measurements, their noisy counterparts \(\tilde{\mathbf{f}}_{ib}^{b}\) and \(\tilde{\mathbf{\omega}}_{ib}^{b}\) are given by:

\[\tilde{\mathbf{f}}_{ib}^{b}=\mathbf{k}_{ib}^{b}+\mathbf{b}_{a}+\mathbf{n}_{a} \tag{6}\]

\[\tilde{\mathbf{\omega}}_{ib}^{b}=\mathbf{\omega}_{ib}^{b}+\mathbf{b}_{g}+\mathbf{n}_{g} \tag{7}\]

where \(\mathbf{b}_{a}\) and \(\mathbf{b}_{g}\) represent the biases on accelerometers and gyroscopes measurements, respectively, and \(\mathbf{n}_{a}\) and \(\mathbf{n}_{g}\) are the corresponding random noises, assumed to follow a zero-mean normal distribution. ### Navigation Solution Update

Mechanisation equations (in discrete form) [33] are used to update the solution of inertial navigation at the next time instant, using measurements of angular velocity \(\mathbf{\omega}_{ib}^{b}\) and specific force \(\mathbf{f}_{ib}^{b}\) from the IMU sensors. Since mechanisation equations are the result of approximations and may introduce errors, an optimised version was used. Orientation update: if the angular velocity remains constant during the integration interval, i.e., if \(\tau\) is sufficiently small, the updated rotation matrix representing the attitude of the vehicle is given by

\[\mathbf{C}_{b}^{n}(t+\tau)\approx\mathbf{C}_{b}^{n}(t)\Big{(}\mathbf{I}+[\mathbf{ \omega}_{ib}^{b}]_{\times}\tau\Big{)}-(\mathbf{\Omega}_{ie}^{n}+\mathbf{\Omega}_{en}^{ n})\mathbf{C}_{b}^{n}(t)\tau, \tag{8}\]

where

\[\mathbf{\Omega}_{ie}^{n}=[\mathbf{\omega}_{ie}^{n}]_{\times}=\mathbf{\omega}_{ie} \begin{pmatrix} 0&\sin(L_{b})&0\\ -\sin(L_{b})&0&-\cos(L_{b})\\ 0&\cos(L_{b})&0\end{pmatrix}  \tag{9}\]

is the skew-symmetric matrix of the rotation vector of the Earth, \(\mathbf{\omega}_{ie}\), resolved in the navigation frame,

\[\mathbf{\Omega}_{en}^{n}=[\mathbf{\omega}_{en}^{n}]_{\times}\Leftarrow\mathbf{\omega}_{ en}^{n} \begin{pmatrix}v_{eb,E}^{n}/R_{E}(L_{b})+h_{b}\\ -v_{eb,E}^{n}/R_{N}(L_{b})+h_{b}\\ v_{eb,E}^{n}\tan(L_{b})/R_{E}(L_{b})+h_{b}\end{pmatrix}  \tag{10}\]

is the skew-symmetric matrix of the _transport rate_ vector due to the rotation of the navigation frame with respect to the Earth, \(R_{E}\) and \(R_{N}\) are respectively the radius of transverse curvature and the radius of curvature of the meridian at that point.