# OBU for Accurate Navigation through Sensor Fusion in the Framework of the EMERGE Project
## 3 Sensor Fusion: Loosely Coupled Algorithm Implementation
### The Initialisation Loop
### Kalman Filter

The state vector of the Kalman filter includes orientation, velocity and position errors used in Equations (1)-(5), as well as the biases of the accelerometer and gyroscope, and is given by

\[\mathbf{\hat{x}}_{k}=[\delta\mathbf{\hat{v}}_{eb}^{n}\quad\delta\mathbf{v}_{ eb}^{n}\quad\delta\mathbf{p}_{b}\quad\mathbf{b}_{a}\quad\mathbf{b}_{g}]^{T}, \tag{20}\]

where the position error is

\[\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}\]0

Whenever corrections from the integration filter are applied, the values of the corresponding states are reset to zero. The algorithm implemented for the Kalman filter is as follows. 1. Calculation of the covariance matrix \(\mathbf{P}_{k}^{-}\) of the prediction error at instant \(k\): \[\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}\]1 (22) \[\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}\]2 (23)

where \(\mathbf{Q}\) is the system noise covariance matrix, and the transition matrix \(\mathbf{\Phi}\) is obtained by computing the expected value of the time derivative for each state. Calculation of the Kalman gain matrix: \[\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}\]3 (24) where \(\mathbf{H}_{k}\) is the measurement matrix and \(\mathbf{R}_{k}\) is the measurement noise covariance matrix at epoch \(k\). 3. Filtering of the system state at instant \(k\) based on GNSS measurements, and calculation of the covariance matrix of the filtered estimation error: \[\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}\]4 (25) \[\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}\]5 (26)

where \(\delta\mathbf{z}_{k}^{-}\) is the vector of observations (i.e., the difference between position and velocity measured by the GNSS and the corresponding values estimated by the inertial navigation system). It's important to note that all estimated quantities are derived based on preceding correction. The matrices \(\mathbf{\Phi}_{k-1}\), \(\mathbf{Q}_{k-1}\), \(\mathbf{R}_{k}\) used in this study are resolved in a local navigation (NED) frame and can be found in Appendix A of this paper. ## 4 Experimental Setup

This section describes the materials and methods used to test the navigation system. The testing process covered all the elements: sensors, services, communication elements, power consumption, synchronisation and especially the data integration/fusion for the PNT process. In the first part, this section describes the experimental activities to evaluate the performance of sensors/services used in the navigation process: GNSS receiver, GNSS Augmentation (PP) service and inertial sensor. Afterwards, it is shown the experimental methodology carried out during the final test, evaluating the impact of the sensor fusion algorithm on the navigation process in an on-road environment. Figure 6 presents a detailed schematic of the experimental verification process conducted for each key system sensor.