# OBU for Accurate Navigation through Sensor Fusion in the Framework of the EMERGE Project
## 3 Sensor Fusion: Loosely Coupled Algorithm Implementation
IMU measurements are accumulated for levelling purposes and also GNSS data, when available, is accumulated to initialize position parameters. Roll and pitch are computed based on the averaged specific force measurements through the levelling process. The estimated position is initialized using the averaged GNSS positions. ```
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:if\((i>\)init_imu_samples_level and \(\text{isempty}(GNSS\_r\_init))\)then
25:\(ave\_{\_}{fib}^{b}\leftarrow\text{mean}(f\_{\_}{leveling}_{ib}^{b},2)\);
26:\(roll\leftarrow\text{atan2}(-ave\_{\_}{fib}^{b}(2),-ave\_{\_}{fib}^{b}(3))\);
27:\(pitch\leftarrow\text{atan}(ave\_{\_}{fib}^{b}(1)/\sqrt{ave\_}{f_{ib}^{b}(2)^{2} +ave\_}{f_{ib}^{b}(3)^{2}})\);
28:\(est\_r_{\_}{eb}^{n}\leftarrow\text{mean}(GNSS\_r\_init,2)\); %Initializes the estimated position
29:endif
30:endif
31:endfor
```  
**Algorithm 3** Initialisation Loop  
### The Initialisation Loop  
As a systematic iteration over IMU samples, this Loop (2) calculates essential parameters and initializes the navigation solution when the system is stationary. Its execution sets the stage for optimal accuracy and performance of the Kalman filter during subsequent integration, establishing a robust foundation for a precise and reliable navigation solution. Within each iteration over the IMU measurement taken in the first seconds, the initialisation loop calculates the IMU sampling interval (\(tor_{i}\)) considering the IMU frequency, facilitating precise temporal alignment. Specific force (\(\mathbf{f}_{ib}^{b}\)) and angular velocity (\(\omega_{ib}^{b}\)) measurements are extracted from the IMU, providing essential motion-related data. The Algorithm 2 checks for the availability of new GNSS measurements within the current Inertial Navigation System (INS) time. If available, it updates the GNSS position. To account for synchronisation issues between the IMU and GNSS data, this check is performed within a time window of duration \(2\epsilon=5\) ns. IMU measurements are accumulated for levelling purposes and also GNSS data, when available, is accumulated to initialize position parameters. Roll and pitch are computed based on the averaged specific force measurements through the levelling process. The estimated position is initialized using the averaged GNSS positions.