## 3 Integrated Navigation
### Aiding Methods
#### 3.2.1 GNSS-Derived Heading Aiding  
A GNSS-derived aiding can be used to improve the heading estimation. To incorporate heading measurements into the measurement model of the GNSS/INS filter, an equation relating the heading errors with the system error states is required [5]. Such an error equation is derived starting from the rotation matrix from East-North-Up (ENU) to body frame \(R_{n}^{b}\):  
\[R_{n}^{b}= \begin{pmatrix}\cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi&-\cos\phi \sin\psi+\sin\phi\sin\theta\cos\psi&-\sin\phi\cos\theta\\ \cos\theta\sin\psi&\cos\theta\cos\psi&\sin\theta\\ \sin\phi\cos\psi-\cos\phi\sin\theta\sin\psi&-\sin\phi\sin\psi-\cos\phi\sin \theta\cos\psi&\cos\phi\cos\theta\end{pmatrix} \]  
where \(\phi\), \(\theta\), \(\psi\) are respectively roll, pitch and azimuth angles. The expression of the azimuth as function of the elements of the matrix \(R_{n}^{b}\) and of its inverse \(R_{b}^{n}\) is:  
\[\varPhi=\arctan\left(\frac{\hat{R}_{n}^{b}(2,1)}{\hat{R}_{n}^{b}(2,2)} \right)=\ \arctan\left(\frac{\hat{R}_{n}^{b}(1,2)}{\hat{R}_{n}^{b}(2,2)}\right) \tag{6}\]  
where the hats on the top remark that the quantities are estimated. Figure 2: Tightly coupled scheme. The relationship between estimated and actual rotation matrix from body to ENU frame is  
\[\hat{R}_{\text{b}}^{n}=\left(I-E^{n}\right)R_{\text{b}}^{n}\]  
with \(E^{n}=\left[\varepsilon_{E}\,\varepsilon_{N}\,\varepsilon_{U}\right]^{T}\) skew-symmetric matrix of the vector of the attitude errors in the ENU frame. With this in mind the Equation (6) becomes:  
\[\hat{\psi}=\arctan\left(\frac{\hat{R}_{\text{b}}^{n}\left(1,2\right)}{\hat{R}_ {\text{b}}^{n}\left(2,2\right)}\right)=\arctan\left(\frac{R_{\text{b}}^{n} \left(1,2\right)+\varepsilon_{U}R_{\text{b}}^{n}\left(2,2\right)-\varepsilon_ {N}R_{\text{b}}^{n}\left(3,2\right)}{-\varepsilon_{U}R_{\text{b}}^{n}\left(1,2 \right)+R_{\text{b}}^{n}\left(2,2\right)+\varepsilon_{E}R_{\text{b}}^{n} \left(3,2\right)}\right)\]  
Computing the differential of the previous equation, the heading error equation is obtained as:  
\[\delta\psi=\frac{\hat{\varphi}\psi}{\hat{\sigma}_{\varepsilon_{E}}}\hat{ \sigma}_{\varepsilon_{E}}+\frac{\hat{\varphi}}{\hat{\sigma}_{\varepsilon_{N}}} \hat{\sigma}_{\varepsilon_{N}}+\frac{\hat{\varphi}}{\hat{\sigma}_{\varepsilon_ {U}}}\hat{\sigma}_{\varepsilon_{U}} \tag{7}\]  
with:  
\[\frac{\partial\hat{\varphi}}{\partial\varepsilon_{E}}=\frac{\hat{R }_{\text{b}}^{n}\left(1,2\right)R_{\text{b}}^{n}\left(3,2\right)}{\left[\hat{R }_{\text{b}}^{n}\left(2,2\right)\right]^{2}+\left[\hat{R}_{\text{b}}^{n}\left( 1,2\right)\right]^{2}}\approx\frac{\hat{R}_{\text{b}}^{n}\left(1,2\right)\hat{R }_{\text{b}}^{n}\left(3,2\right)}{\left[\hat{R}_{\text{b}}^{n}\left(2,2 \right)\right]^{2}+\left[\hat{R}_{\text{b}}^{n}\left(1,2\right)\right]^{2}}\] \[\frac{\partial\hat{\varphi}}{\partial\varepsilon_{U}}=\frac{\hat {R}_{\text{b}}^{n}\left(1,2\right)R_{\text{b}}^{n}\left(1,2\right)+\hat{R}_{ \text{b}}^{n}\left(2,2\right)R_{\text{b}}^{n}\left(2,2\right)}{\left[\hat{R }_{\text{b}}^{n}\left(2,2\right)\right]^{2}+\left[\hat{R}_{\text{b}}^{n}\left( 1,2\right)\right]^{2}}\approx 1\]  
In Equation (7) the measurement is:  
\[\delta\psi=\left[\psi_{\text{\tiny{$\alpha$ss}}}-\psi_{\text{\tiny{$\alpha$ss }}}\right]=\left[\psi_{\text{\tiny{$\alpha$ss}}}-\arctan\left(\frac{V_{E}}{V_{ N}}\right)\right] \tag{8}\]  
with \(\psi_{\text{\tiny{$\alpha$ss}}}\) being the azimuth estimated in the GNSS/INS filter, \(\psi_{\text{\tiny{$\alpha$ss}}}\) being the GNSS-derived azimuth depending on East and North velocity components and whose accuracy is expressed as:  
Figure 2:0  
where \(V_{\text{\tiny{Figure 2:1ss}}}\) is the horizontal speed (estimated by GNSS) and \(\sigma_{\text{\tiny{Figure 2:2ss}}}^{2}\) is the associated variance. The external heading equation can be embedded in the measurement model of the GNSS/INS KF and is used when the horizontal speed of the vehicle is sufficiently high. For this work, this measurement is only used when the vehicle's speed exceeds 5 m/s, as this gave reasonable results.