## 3 The Sensor Integration
### Kalman Filter Update  
The Kalman filter update is based on an error state extended Kalman filter, which means that the estimated state is not the full state vector itself, as described in Equation (22), but the error vector between the SDA predicted state and the full state. To estimate this error, the following measurements are used:  
* the GPS position (Pos) \(\tilde{\mathbf{x}}_{a}^{e}\) of the GPS antenna reference point, expressed in the e-frame,
* the GPS attitude baseline vector (Base) \(\Delta\tilde{\mathbf{x}}_{b}^{e}\), expressed in the e-frame and,
* the magnetic field vector (Mag) \(\tilde{\mathbf{h}}^{b}\), measured in the b-frame. The error state vector \(\delta\mathbf{x}\) includes the error vectors for the position \(\delta\mathbf{x}_{p}^{e}\), the velocity \(\delta\mathbf{v}_{eb}^{e}\), the attitude \(\delta\psi\), the accelerometer bias \(\delta\mathbf{b}_{a}\) and the gyro bias \(\delta\mathbf{b}_{\omega}\):  
\[\delta\mathbf{x}=\left[\delta\mathbf{x}_{p}^{e,T}\quad\delta\mathbf{v}_{eb}^{e,T}\quad\delta\psi^{T}\quad\delta\mathbf{b}_{a}^{b,T}\quad\delta\mathbf{b}_{ \omega}^{b,T}\right] \tag{31}\]  
System dynamics model: The derivation of the error dynamics equations can, for example, be found in [42] or [45]. Hence, the linearized continuous system dynamics model specialized to the e-frame is:  
\[\dot{\delta\mathbf{x}} =\mathbf{F}\delta\mathbf{x}+\mathbf{G}n\] \[= \begin{bmatrix}\mathbf{0}&\mathbf{I}&\mathbf{0}&\mathbf{0}& \mathbf{0}\\ \mathbf{0}&\mathbf{0}&-[\hat{a}_{ib}^{e}\times]&-\hat{\mathbf{C}}_{b}^{e}& \mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&-\hat{\mathbf{C}}_{b}^{e}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix}\begin{bmatrix} \delta\mathbf{x}_{p}^{e}\\ \delta\mathbf{v}_{eb}^{e}\\ \delta\psi\\ \delta\mathbf{b}_{a}^{b}\\ \delta\mathbf{b}_{\omega}^{b}\end{bmatrix} + \begin{bmatrix}\mathbf{0}& \mathbf{0}&\mathbf{0}&\mathbf{0}\\ \hat{\mathbf{C}}_{b}^{e}&\mathbf{0}&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{I}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\mathbf{I}\end{bmatrix}\begin{bmatrix}n_{a} \\ n_{\omega}\\ n_{b_{a}}\\ n_{b_{\omega}}\end{bmatrix} \]  
where \(\mathbf{0}\) is always a 3 \(\times\) 3 zero matrix and \(\mathbf{I}\) a 3 \(\times\) 3 identity matrix. \([\hat{a}_{ib}^{e}\times]\) denotes a 3 \(\times\) 3 skew symmetric matrix with elements of the bias-corrected acceleration vector \(a_{ib}^{b}\), which has been rotated into the e-frame. The vector \(n\) includes the stochastic variables. Measurement model: For the integration of the GPS positions the lever arm \(\mathbf{l}^{b}\) between the GPS antenna reference point \(\mathbf{x}_{a}^{e}\) and the system reference point \(\mathbf{x}_{p}^{e}\), which can be identical to the IMU reference point, has to be regarded [45]:  
\[\mathbf{\tilde{x}}_{a}^{e} =\mathbf{x}_{p}^{e}+\mathbf{C}_{b}^{e}\mathbf{l}^{b}+\vartheta_{x _{a}}=\mathbf{x}_{p}^{e}+(\mathbf{I}+\mathbf{\Psi})\hat{\mathbf{C}}_{b}^{e} \mathbf{l}^{b}+\vartheta_{x_{a}} \tag{32}\] \[=\mathbf{x}_{p}^{e}+(\mathbf{I}+\mathbf{\Psi})\hat{\mathbf{l}}^{ e}+\vartheta_{x_{a}}=\mathbf{x}_{p}^{e}+\hat{\mathbf{l}}^{e}-\left[\hat{ \mathbf{l}}^{e}\times\right]\psi+\vartheta_{x_{a}}\]  
where \(\vartheta_{x_{a}}\) denotes the GPS position measurement noise. Due to the coupling between the GPS position and the measured accelerations from the IMU, the yaw angle error is bounded in the presence of horizontal accelerations. However, in the absence of horizontal accelerations, for example when the UAV is hovering, the yaw angle is still unobserved [12]. To overcome the problem of an undetermined yaw angle, the GPS attitude baseline is arranged on the UAV.