# Sensor Fusion of GNSS and IMU Data for Robust Localization via Smoothed Error State Kalman Filter
## 2 GNSS and IMU Integrated Filter Design
### Error State Kalman filter
#### 2.2.5 The ESKF Observation Process  
Once the GNSS information becomes available, the observation is incorporated to continuously update the filter. This process also involves calibrating for the accelerometer and gyroscope biases.  
The observation equation is typically expressed in a more uniform form within the filter in the form of  
\[\mathbf{z}_{k}=\mathbf{h}(\mathbf{x}_{tk})+\mathbf{w}_{mk}, \tag{20}\]  
where \(\mathbf{z}_{k}\) is the measurement signal vector, \(\mathbf{w}_{mk}\) represents the Gaussian white noise of the measurement signal, and its covariance is \(\mathbf{V}\). The error calibration update equation is derived as  
\[\begin{split}\mathbf{K}_{k+1}=\widetilde{\mathbf{P}}_{k+1}^{-}\mathbf{H}_{k+1 }^{T}(\mathbf{H}_{k+1}\widetilde{\mathbf{P}}_{k+1}^{-}\mathbf{H}_{k+1}^{T}+\mathbf{V}_{k+1})^{ -1}\\ \mathbf{f}\mathbf{\ddot{\mathbf{\chi}}}_{k+1}=\mathbf{K}_{k+1}(\mathbf{Z}_{k+1}-\mathbf{H} (\widetilde{\mathbf{\chi}}_{k+1}))\\ \mathbf{\ddot{P}}_{k+1}=\mathbf{(I}-\mathbf{K}_{k+1}\mathbf{H}_{k+1})\widetilde{\mathbf{P}}_{k+1 }^{-}\end{split} \tag{21}\]  
The Jacobian matrix \(\mathbf{H}\) is required to be defined with respect to the error state _f_fix, and evaluated at the best true state estimate \(\hat{\mathbf{x}}_{t}=\mathbf{x}\ \oplus\ \mathbf{f}\mathbf{\ddot{\mathbf{\eta}}}\mathbf{\dot{\mathbf{x}}}\). As the error state mean is zero at this stage (we have not observed it yet), we have \(\mathbf{x}_{t}=\mathbf{x}\), and we can use the nominal error x as the evaluation point, leading to  
\[\mathbf{H}=\left.\frac{\partial\mathbf{h}}{\partial\mathbf{x}_{t}}\right|_{x}\left.\frac{ \partial\mathbf{x}_{t}}{\partial\mathbf{f}\mathbf{\dot{\mathbf{\eta}}}\mathbf{\dot{\mathbf{x}}}} \right|_{x}=\mathbf{H}_{x}\mathbf{X}_{\mathbf{f}\mathbf{\ddot{\mathbf{\eta}}}\mathbf{x}}, \tag{22}\]  
in which  
\[\mathbf{X}_{\mathbf{f}\mathbf{\ddot{\mathbf{\eta}}}\mathbf{x}}\triangleq\left.\frac{\partial\mathbf{x }_{t}}{\partial\mathbf{f}\mathbf{\dot{\mathbf{\eta}}}\mathbf{x}}\right|_{x}=\begin{bmatrix} \mathbf{I}_{6}&0&0\\ 0&\mathbf{Q}_{\mathbf{f}\mathbf{\ddot{\mathbf{\eta}}}\mathbf{\dot{\mathbf{\theta}}}}&0\\ 0&\mathbf{0}&\mathbf{I}_{6}\end{bmatrix}, \tag{23}\]  
\[\mathbf{Q}_{\mathbf{f}\mathbf{\ddot{\mathbf{\eta}}}\mathbf{\dot{\mathbf{\theta}}}}=\frac{1}{2}\begin{bmatrix} -q_{x}&-q_{y}&-q_{z}\\ q_{w}&-q_{z}&q_{y}\\ q_{z}&q_{w}&-q_{x}\\ -q_{y}&q_{x}&q_{w}\end{bmatrix}. \tag{24}\]2.2.6. Combination of ESKF Error State and Nominal State Process  
Based on the recurrence of the a priori system state and the calibration of the error state, the updated form is obtained as follows:  
\[\widehat{\mathbf{x}}_{k}=\widehat{\mathbf{x}}_{k}^{-}\oplus\textit{ffi}\widehat{\mathbf{x} }_{k}. \tag{25}\]  
Each of these system states corresponds to the following:  
\[\begin{array}{l}\widehat{\mathbf{p}}_{k}=\widehat{\mathbf{p}}_{k}^{-}+\textit{ffi} \widehat{\mathbf{p}}_{k}\\ \widehat{\mathbf{v}}_{k}=\widehat{\mathbf{v}}_{k}^{-}+\textit{ffi}\widehat{\mathbf{v}}_{k} \\ \widehat{\mathbf{q}}_{k}=\widehat{\mathbf{q}}_{k}^{-}\otimes\widehat{\textit{ffi}} \widehat{\mathbf{o}}_{k}.\\ \widehat{\mathbf{a}}_{\textit{b}k}=\widehat{\mathbf{a}}_{\textit{b}k}+\textit{ffi} \widehat{\mathbf{a}}_{\textit{b}k}\\ \widehat{\mathbf{\omega}}_{\textit{b}k}=\widehat{\mathbf{\omega}}_{\textit{b}k}+ \textit{ffi}\widehat{\mathbf{\omega}}_{\textit{b}k}\end{array} \tag{26}\]  
2.2.7. ESKF Error State Reset  
After injecting the error state into the nominal state, the a priori error state and the corresponding covariance need to be reset, as  
\[\textit{ffi}\widehat{\mathbf{x}}=0 \tag{27}\]  
where \(\mathbf{G}\) is the reset function \(\textit{g}(\textit{ffi}\mathbf{x})=\textit{ffi}\mathbf{x}\ominus\textit{ffi}\mathbf{x}\) of the Jacobi matrix, which is defined in the form of  
\[\mathbf{G}\triangleq\left.\frac{\partial\textit{g}}{\partial\textit{ffi}\mathbf{x}} \right|_{\textit{ffi}\widehat{\mathbf{x}}}=\begin{pmatrix}\mathbf{I}_{6}&0&0\\ 0&\mathbf{I}-\left(\frac{1}{2}\textit{ffi}\widehat{\mathbf{\theta}}\right)&0\\ 0&0&\mathbf{I}_{9}\end{pmatrix}. \tag{28}\]