## 2 System Model
### Inertial Navigation System Model
\[\mathbf{m}_{n}=\left[0\quad\sqrt{{{m_{n}}_{x}}^{2}+{{m_{n}}_{y}}^{2}}\quad{{m_{n }}_{z}}\right] \tag{5}\]  
The modified geomagnetic field vector can be converted into the body frame through the directional cosine matrix as the observation vector, and the equation for this is:  
\[\mathbf{m}_{b}=\mathbf{C}_{n}^{b}\mathbf{m}_{n} \tag{6}\]  
According to Equations (3) and (6), the observation equation is:  
\[\mathbf{z}=h(\mathbf{x}_{t})+\mathbf{V} \tag{7}\]  
where the observation vector \(\mathbf{z}\) is made up of the accelerometer measurements and the magnetometer measurements, \(h()=\left[\mathbf{C}_{n}^{b}\mathbf{g}_{n}\quad\mathbf{C}_{n}^{b}\mathbf{m}_{n}\right]^{T}\) is the nonlinear function of \(\mathbf{x}_{t}\), and \(\mathbf{V}\) is the observation noise with a zero mean Gaussian distribution. The Jacobian matrix \(\mathbf{H}\) of the observation equation needs to be obtained during the filter update, which can be obtained on the basis of the following chain rule:  
\[\mathbf{H}=\frac{\partial h}{\partial\delta x}|_{x}=\frac{\partial h}{\partial x _{t}}|_{x}\frac{\partial\mathbf{x}_{t}}{\partial\delta x}|_{x}=\mathbf{H}_{x}\mathbf{X}_ {\delta x} \tag{8}\]  
where \(\mathbf{H}_{x}\) and \(\mathbf{X}_{\delta x}\) can be derived as follows:  
\[\mathbf{H}_{x}=\left[ \begin{array}{cc}\frac{\partial h_{x}}{\partial q}&\mathbf{O} _{3\times 3}\\ \frac{\partial m_{b}}{\partial q}&\mathbf{O}_{3\times 3}\end{array} \right] \tag{9}\]  
\[\mathbf{X}_{\delta x}=\left[ \begin{array}{cc}\frac{\partial(\delta qx-q)}{ \partial\delta\theta}&\mathbf{O}_{4\times 3}\\ \mathbf{O}_{3\times 3}&\frac{\partial(\mathbf{x}_{b}+\delta\mathbf{x}_{b})}{\partial\delta\bm {x}_{b}}\end{array} \right] \tag{10}\]  
Based on the state and observation equations, the ESKF can be implemented using the following equation. The prediction equations of ESKF can be expressed as follows:  
\[\delta\mathbf{x}_{k|k-1}=\mathbf{F}_{k}\delta\mathbf{x}_{k-1} \tag{11}\]  
\[\mathbf{P}_{k|k-1}=\mathbf{F}_{k}\mathbf{P}_{k-1}\mathbf{F}_{k}^{T}+\mathbf{Q}_{k} \tag{12}\]  
where \(\mathbf{P}_{k-1}\) represents the error covariance matrix associated with the error states while \(\mathbf{Q}_{k}\) represents the covariance matrix of the process noise. The updated equations for ESKF can be expressed as follows:  
\[\mathbf{K}_{k}=\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T}\big{(}\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{ T}+\mathbf{R}_{k}\big{)}^{-1} \tag{13}\]  
\[\delta\mathbf{x}_{k}=\mathbf{K}_{k}(\mathbf{z}_{k}-h(\mathbf{x}_{k|k-1})) \tag{14}\]  
\[\mathbf{P}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k|k-1} \tag{15}\]  
where \(\mathbf{K}_{k}\) is the Kalman gain used to provide the correction, and \(\mathbf{R}_{k}\) denotes the measurement noise covariance matrix. Following each ESKF update, we utilized the estimated error state \(\delta\mathbf{x}\) to correct the state \(\mathbf{x}\) and obtain the true state \(\mathbf{x}_{t}\). This compensation process can be expressed by the following equations:  
\[\mathbf{q}_{t}=\mathbf{q}\big{\{}\delta\mathbf{\theta}\big{\}}\circ\mathbf{q} \tag{16}\]  
\[\mathbf{\omega}_{bt}=\mathbf{\omega}_{b}+\delta\mathbf{\omega}_{b} \tag{17}\]  
To avoid introducing the error estimation in the next update, the error states need to be reset as soon as the true value has been determined. The attitude can be estimated by the above filter estimation, and then the speed and position can be obtained using the following equations. \[\mathbf{v}_{k}=\mathbf{v}_{k-1}+(\mathbf{C}_{b(k-1)}^{n}\mathbf{a}_{k-1}-\mathbf{g})\Delta t \tag{18}\]  
\[\mathbf{p}_{k}=\mathbf{p}_{k-1}+\frac{\mathbf{v}_{k}+\mathbf{v}_{k-1}}{2}\Delta t \tag{19}\]  
where the subscript \(k\) and \(k-1\) represent time, \(\mathbf{a}\) is the acceleration the in body frame, \(\mathbf{g}\) is the gravity acceleration in the navigation frame, \(\Delta t\) is the sampling time interval, and \(\mathbf{v}\) and \(\mathbf{p}\) are the velocity and positions, respectively.