Particle Filter with Novel Nonlinear Error Model for Miniature Gyroscope-Based Measurement While Drilling Navigation

Abstract

The derivation of a conventional error model for the miniature gyroscope-based measurement while drilling (MGWD) system is based on the assumption that the errors of attitude are small enough so that the direction cosine matrix (DCM) can be approximated or simplified by the errors of small-angle attitude. However, the simplification of the DCM would introduce errors to the navigation solutions of the MGWD system if the initial alignment cannot provide precise attitude, especially for the low-cost microelectromechanical system (MEMS) sensors operated in harsh multilateral horizontal downhole drilling environments. This paper proposes a novel nonlinear error model (NNEM) by the introduction of the error of DCM, and the NNEM can reduce the propagated errors under large-angle attitude error conditions. The zero velocity and zero position are the reference points and the innovations in the states estimation of particle filter (PF) and Kalman filter (KF). The experimental results illustrate that the performance of PF is better than KF and the PF with NNEM can effectively restrain the errors of system states, especially for the azimuth, velocity, and height in the quasi-stationary condition.

1. Introduction

Horizontal drilling (HD), especially reentry multilateral horizontal well drilling (RMHWD), receives considerable attention because it provides great economic value in the oil and gas industry. The RMHWD technology was originally proposed in an industry consortium of operators and service companies in 1997 [1] and RMHWD can significantly increase the production of oil and gas by extending the contact areas between oil and gas reservoirs and the well drilling equipment and by revisiting the existing wellbores [2]. RMHWD is also readily available under challenging drilling sites such as offshore zones, mountain areas, and downtown centers [3]. The branches of the drill pipe in RMHWD radiate from the main wellbore to the targeted reservoir sections [4]. The high pressure waterjet drilling technology promotes the development of RMHWD since the conventional large rotary drill bit can be replaced by a small-diameter waterjet drill bit. The conventional measurement-while-drilling (MWD) tool combines three-axis gyroscopes and three-axis magnetometers to provide the attitude (tool face, inclination, and azimuth) of the drill bit. However, the performance of the MWD tools would deteriorate under the strong magnetic interference caused by the following factors: (1) flow of currents in the atmosphere and moving of solar wind [5]; (2) existing drilling fluid and debris [6,7]; (3) ferromagnetic deposits in the proximity of bottom hole assembly (BHA) [8]. Non-magnetic drill collars can effectively eliminate the magnetic disturbance; however, the high cost, high weight, and weak fracture drill collars fail to be widely used.

Alternatively, gyroscope-based MWD (GWD) tools can continuously provide the attitude, velocity, and position of the drill bit regardless of the strong magnetic interferences. GWD tools with pure strapdown inertial navigation algorithm were proposed in [9,10] and the tools can save considerable rig time and remove the blind during the kickoff [11]. The size of the fiber optic gyroscope or the laser optic gyroscope limits their application to the surveying of downhole drilling. The emergence of a low-cost, small-size, and high-performance MEMS sensor brings the gyroscopes to the small-diameter RMHWD. The MGWD device with two-axis gyroscopes and three-axis accelerometers in the application of small-diameter space (less than 24 millimeters) was proposed in [12], in which the angular rate (Y-axis) of MGWD tools is calculated by the accelerometers in the horizontal plane when the drilling equipment is under quasi-stationary conditions. The MGWD system can provide high-precision navigation solutions in short-term [13] and can perform real-time data transmission between the MGWD tools and the ground operation center. However, the integration calculation within resolving the mechanization of navigation solutions accumulates large errors because of biases of gyroscopes and accelerometers. The errors of inertial sensors are composed of two parts: deterministic errors and stochastic errors [14]. The deterministic errors can partially be reduced by the calibration approach. Therefore, the residual of deterministic errors and the stochastic errors are main error sources of inertial sensors. To sum up, the problems should be solved for MGWD system are the elimination of the residual of deterministic errors and stochastic errors.

The stochastic errors of inertial sensors can be modeled by the first-order Gaussian–Markov (GM) model or the high-order autoregressive (AR) model [15]. The Levenberg–Marquardt iterative least squares can fit the nonlinear parametric model of stochastic errors as well [16]. The traditional error models of the MGWD system, such as the Phi-angle or Psi angle model, are based on the assumption that the attitude errors are small enough that the DCM can be simplified by the small-angle attitude errors. The linear modified error model for large heading uncertainty was proposed in [17] and the model of moving alignment with large errors was put forward in [18]. In the application of the Kalman filter (KF), both error models neglect the high-order error terms that may introduce errors when the attitude errors are large. In addition, the system noises and the observation noises are not Gaussian for MGWD system in sophisticated downhole drilling environments (high temperature and high pressure). Therefore, the KF is suboptimal compared with the particle filter (PF), which is the recursive approximation of the posterior probability density function (pdf) with particles and weights [19]. The conventional in-drilling alignment (IDA) approaches include the zero velocity update (ZUPT) alignment, velocity matching alignment, rotary modulation alignment, etc. The ZUPT alignment can limit the position errors up to 40 m during a 90-min experiment by using a Litton LTN90-100 inertial measurement unit (IMU) [20]. The velocity matching alignment merges velocity from global positioning system (GPS) or other velocity sensors to the filtering for compensating the errors [21]. An IDA approach was introduced to restrict the azimuth error of the drill bit in [22,23]; the azimuth error was reduced 25 times smaller than the conventional magnetometer based MWD tools. The rotary-in-drilling alignment (RIDA) was proposed in terms of the theory of rotary modulation alignment in an inertial navigation system (INS) to minimize the dynamic position errors and the azimuth error of the MWD system [24,25]. However, GPS signals cannot reach to a deep downhole and the size of IDA or RIDA is beyond the dimension requirements of the RMHWD.

This paper proposes a NNEM for the MGWD system under large-angle attitude error conditions. The error matrix of DCM is introduced to represent the attitude errors. Because of the nonlinearity of the system state space model (SSM), the PF is selected as the state estimation approach. The body frame is the right, forward, and up direction of the MGWD system and the navigation frame is the north, east, and down direction in this paper.

In Section 2, the NNEM of the MGWD system is proposed. Section 3 and Section 4 introduce the recursive Bayesian estimation theory and PF algorithm, respectively. Results and experiments are explained in Section 5. Eventually, conclusions are presented in Section 6.

2. NNEM of the MGWD System

The error model of the strapdown inertial navigation system (SINS), based on the discrete time differential equation and the error model, plays a fundamental role in the estimation of system states. Conventional linear error models (LEM) of SINS, such as the Phi-angle model, the Psi-angle model [26,27], and the quaternion error model [28] are widely used in aerospace and marine navigation. The Phi-angle error model analyzes the SINS in the true navigation frame while the Psi-angle model is performed in the computer frame. In this paper, the Psi-angle model is selected for the states estimation of KF while the error model in [29] is used as the NEM for the states estimation of the PF.

The derivation of a linear model by the perturbation method alone can be implemented in light of the assumption that the attitude errors are small enough. The initial alignment of the MGWD system cannot provide an accurate attitude because of the low performance of the MEMS sensors and the sophisticated downhole drilling environments. Therefore, this paper proposes an accurate NNEM of the MGWD system under large-angle attitude error conditions.

The key variable for the derivation of the SINS error model is the rotation vector, which is the eigenvector of the DCM associated with eigenvalue 1. The eigenvector of the DCM can be expressed as follows [30,31]:

$$({\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}-\mathrm{I})\mathsf{\Phi }=0$$

(1)

where ${\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}$ is the DCM from the body frame to the navigation frame$, \mathsf{\Phi }$ is the rotation vector, and I is a $3\times 3$ identity matrix. The rotation vector is proposed in [32], and the relationship between rotation vector and DCM can be expressed as:

$${\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}=\mathrm{I}+\frac{\sin \mathsf{\Phi }}{\mathsf{\Phi }}\left(\mathsf{\Phi }\times \right)+\frac{1-\text{cos}\mathsf{\Phi }}{{\mathsf{\Phi }}^2}{\left(\mathsf{\Phi }\times \right)}^2$$

(2)

where $\left(\mathsf{\Phi }\times \right)$ is the skew symmetric matrix of $\mathsf{\Phi }$, i.e.,:

$$\left(\mathsf{\Phi }\times \right)=\left[\begin{array}{ccc}0& -{\mathsf{\Phi }}_{\mathrm{z}}& {\mathsf{\Phi }}_{\mathrm{y}}\\ {\mathsf{\Phi }}_{\mathrm{z}}& 0& -{\mathsf{\Phi }}_{\mathrm{x}}\\ -{\mathsf{\Phi }}_{\mathrm{y}}& {\mathsf{\Phi }}_{\mathrm{x}}& 0\end{array}\right]$$

(3)

where ${\mathsf{\Phi }}_{\mathrm{x}}, {\mathsf{\Phi }}_{\mathrm{y}}$, and ${\mathsf{\Phi }}_{\mathrm{z}}$ are the components of $\mathsf{\Phi }$ projected in X-axis, Y-axis, and Z-axis.

The magnitude of $\mathsf{\Phi }$ equals:

$$\mathsf{\Phi }={\left({\mathsf{\Phi }}^{\mathrm{T}}\mathsf{\Phi }\right)}^{1/2}$$

(4)

So far, the problem of seeking an attitude solution equates to seeking a solution for the rotation vector. The derivation of the kinematic equation of rotation vector was reviewed in [33], and the conventional differential equation of rotation vector was given by [34]:

$${\displaystyle \dot{\mathsf{\Phi }}}=\left(\mathrm{I}+\frac{1}{2}\left(\mathsf{\Phi }\times \right)+\frac{1}{{\mathsf{\Phi }}^2}\left(1-\frac{\mathsf{\Phi }\sin \mathsf{\Phi }}{2\left(1-\text{cos}\mathsf{\Phi }\right)}\right){\left(\mathsf{\Phi }\times \right)}^2\right){\mathsf{\omega }}_{\text{nb}}^{\mathrm{b}}$$

(5)

where ${\mathsf{\omega }}_{\text{nb}}^{\mathrm{b}}$ is the angular rate of the body frame with respect to the navigation frame resolved in the body frame.

This section reviews the nonlinear error model of MGWD according to [27], in which the attitude error model is derived according to the linear quaternion error model. The attitude errors are defined in terms of the rotation vector:

$$\mathsf{\Phi }=\left(\mathsf{\delta }\mathsf{\varphi },\mathsf{\delta }\mathsf{\theta },\mathsf{\delta }\mathsf{\psi }\right)$$

(6)

where $\mathsf{\delta }\mathsf{\varphi },\mathsf{\delta }\mathsf{\theta },\text{ and }\mathsf{\delta }\mathsf{\psi }$ are the roll error, pitch error, and azimuth error. The NNEM of the attitude can be expressed as follows:

$$\dot{\mathsf{\Phi }}={\left(\mathrm{I}-{\mathrm{c}}_{\mathrm{n}}{\mathsf{\Phi }\mathsf{\Phi }}^{\mathrm{T}}\right)}^{-1}\left(-{\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}\times \mathsf{\Phi }-\frac{1}{2{\mathrm{s}}_{\mathrm{n}}}\left({\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\omega }}_{\text{ib}}^{\mathrm{b}}-{\mathsf{\delta }\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}\right)\right)$$

(7)

where ${\mathrm{c}}_{\mathrm{n}}$ and ${\mathrm{s}}_{\mathrm{n}}$ are defined parameters that can be expressed as:

$${\mathrm{c}}_{\mathrm{n}}=\cos \left(\frac{{\mathsf{\Phi }}_{\mathrm{n}}}{2}\right)-1$$

(8)

$${\mathrm{s}}_{\mathrm{n}}=\frac{1}{{\mathsf{\Phi }}_{\mathrm{n}}}\sin \left(\frac{{\mathsf{\Phi }}_{\mathrm{n}}}{2}\right)$$

(9)

$${\mathsf{\Phi }}_{\mathrm{n}}=\sqrt{\mathsf{\delta }{\mathsf{\varphi }}^2+{\mathsf{\delta }\mathsf{\theta }}^2+{\mathsf{\delta }\mathsf{\psi }}^2}$$

(10)

where ${\mathsf{\omega }}_{\text{ib}}^{\mathrm{b}}$ is the angular rate obtained from the gyroscope, ${\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}$ is the angular rate of the navigation frame with respect to the inertial frame resolved in the navigation frame, and ${\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}$ can be expressed as:

$${\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}={\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$$

where ${\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}$ is the Earth rotation rate in the navigation frame and ${\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}$ can be written as follows:

$${\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}={\left[\begin{array}{ccc}{\mathsf{\omega }}_{\text{ie}}\text{cos}\mathsf{\phi }& 0& -{\mathsf{\omega }}_{\text{ie}}\sin \mathsf{\phi }\end{array}\right]}^{\mathrm{T}}$$

(11)

where ${\mathsf{\omega }}_{\text{ie}}$ is the Earth rotation rate $7.292115\times {10}^{-5}\text{rad}/\mathrm{s}\approx 15.041067°/\mathrm{h}$; $\mathsf{\phi }$ is the latitude; ${\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$ is the angular rate of the navigation frame with respect to the Earth frame resolved in the navigation frame; and ${\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$ can be written as:

$${\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}={\left[\begin{array}{ccc}\frac{{\mathrm{V}}_{\mathrm{E}}}{{\mathrm{R}}_{\mathrm{N}}+\mathrm{h}}& -\frac{{\mathrm{V}}_{\mathrm{N}}}{{\mathrm{R}}_{\mathrm{M}}+\mathrm{h}}& -\frac{{\mathrm{V}}_{\mathrm{E}}\text{tan}\mathsf{\phi }}{{\mathrm{R}}_{\mathrm{N}}+\mathrm{h}}\end{array}\right]}^{\mathrm{T}}$$

(12)

where ${\mathrm{V}}_{\mathrm{E}}\text{ and }{\mathrm{V}}_{\mathrm{N}}$ represent the east velocity and north velocity, respectively, in the navigation frame; h is the ellipsoidal height; ${\mathrm{R}}_{\mathrm{N}}\text{ and }{\mathrm{R}}_{\mathrm{M}}$ are the radii of curvature in the meridian and the prime vertical, respectively; and ${\mathrm{R}}_{\mathrm{N}}, {\mathrm{R}}_{\mathrm{M}}$ can be expressed as follows [35]:

$${\mathrm{R}}_{\mathrm{N}}=\frac{\mathrm{R}}{{\left(1-{\mathrm{e}}^2{\sin }^2\mathsf{\phi }\right)}^{1/2}}$$

(13)

$${\mathrm{R}}_{\mathrm{M}}=\frac{\mathrm{R}\left(1-{\mathrm{e}}^2\right)}{{\left(1-{\mathrm{e}}^2{\sin }^2\mathsf{\phi }\right)}^{3/2}}$$

(14)

where $\mathrm{R}, \mathsf{e}$ are the semi-major axis and linear eccentricity of the reference ellipsoid, respectively, in which $\mathrm{R}=6378137$ m and $\mathrm{e}=0.0818191908426$.

The output errors of accelerometers and gyroscopes (${\mathsf{\delta }\mathsf{f}}_{\text{ib}}^{\mathrm{b}}$ and ${\mathsf{\delta }\mathsf{\omega }}_{\text{ib}}^{\mathrm{b}}$) can be expressed as [36]:

$${\mathsf{\delta }\mathsf{\omega }}_{\text{ib}}^{\mathrm{b}}=\left[\begin{array}{ccc}-{\Delta \text{SF}}_{\mathrm{x},\mathrm{g}}& {\Delta \mathsf{\delta }}_{\text{xz},\mathrm{g}}& -{\Delta \mathsf{\delta }}_{\text{xy},\mathrm{g}}\\ -{\Delta \mathsf{\delta }}_{\text{yz},\mathrm{g}}& -{\Delta \mathrm{SF}}_{\mathrm{y},\mathrm{g}}& {\Delta \mathsf{\delta }}_{\text{yx},\mathrm{g}}\\ {\Delta \mathsf{\delta }}_{\text{zy},\mathrm{g}}& -{\Delta \mathsf{\delta }}_{\text{zx},\mathrm{g}}& -{\Delta \mathrm{SF}}_{\mathrm{z},\mathrm{g}}\end{array}\right]{\mathsf{\omega }}_{\text{ib}}^{\mathrm{b}}+\left[\begin{array}{c}{\mathsf{ϵ}}_{\mathrm{x},\mathsf{g}}\\ {\mathsf{ϵ}}_{\mathrm{y},\mathsf{g}}\\ {\mathsf{ϵ}}_{\mathrm{z},\mathsf{g}}\end{array}\right]$$

(15)

$${\mathsf{\delta }\mathsf{f}}_{\text{ib}}^{\mathrm{b}}=\left[\begin{array}{ccc}-{\Delta \text{SF}}_{\mathrm{x},\mathrm{a}}& {\Delta \mathsf{\delta }}_{\text{xz},\mathrm{a}}& -{\Delta \mathsf{\delta }}_{\text{xy},\mathrm{a}}\\ -{\Delta \mathsf{\delta }}_{\text{yz},\mathrm{a}}& -{\Delta \text{SF}}_{\mathrm{y},\mathrm{a}}& {\Delta \mathsf{\delta }}_{\text{yx},\mathrm{a}}\\ {\Delta \mathsf{\delta }}_{\text{zy},\mathrm{a}}& -{\Delta \mathsf{\delta }}_{\text{zx},\mathrm{a}}& -{\Delta \mathrm{SF}}_{\mathrm{z},\mathrm{a}}\end{array}\right]{\mathrm{f}}_{\text{ib}}^{\mathrm{b}}+\left[\begin{array}{c}{\mathsf{ϵ}}_{\mathrm{x},\mathsf{a}}\\ {\mathsf{ϵ}}_{\mathrm{y},\mathsf{a}}\\ {\mathsf{ϵ}}_{\mathrm{z},\mathsf{a}}\end{array}\right]$$

(16)

where ${\Delta \mathrm{SF}}_{\mathrm{x},\mathrm{g}}$ and ${\Delta \mathrm{SF}}_{\mathrm{z},\mathrm{g}}$ are the scale factor errors of X-axis gyroscope and Z-axis gyroscope. ${\Delta \mathrm{SF}}_{\mathrm{y},\mathrm{g}}$ is defined as the scale factor error of a virtual gyroscope along the Y-axis. ${\Delta \mathsf{\delta }}_{\text{xy},\mathrm{g}},{\Delta \mathsf{\delta }}_{\text{xz},\mathrm{g}},{\Delta \mathsf{\delta }}_{\text{yz},\mathrm{g}},{\Delta \mathsf{\delta }}_{\text{yx},\mathrm{g}},{\Delta \mathsf{\delta }}_{\text{zy},\mathrm{g}},$ and ${\Delta \mathsf{\delta }}_{\text{zx},\mathrm{g}}$ are non-orthogonal gyroscope errors along the X-axis, Y-axis, and Z-axis. ${\Delta \mathrm{SF}}_{\mathrm{x},\mathrm{a}},{\Delta \mathrm{SF}}_{\mathrm{y},\mathrm{a}},$ and ${\Delta \mathrm{SF}}_{\mathrm{z},\mathrm{a}}$ are the scale factor errors of the X-axis accelerometer, Y-axis accelerometer, and Z-axis accelerometer, respectively. ${\Delta \mathsf{\delta }}_{\text{xy},\mathrm{a}},{\Delta \mathsf{\delta }}_{\text{xz},\mathrm{a}},{\Delta \mathsf{\delta }}_{\text{yz},\mathrm{a}},{\Delta \mathsf{\delta }}_{\text{yx},\mathrm{a}},{\Delta \mathsf{\delta }}_{\text{zy},\mathrm{a}},\text{ and }{\Delta \mathsf{\delta }}_{\text{zx},\mathrm{g}}$ are non-orthogonal acceleration errors along the X-axis, Y-axis, and Z-axis. ${\mathsf{ϵ}}_{\mathrm{x},\mathrm{g}},{\mathsf{ϵ}}_{\mathrm{y},\mathrm{g}},{\mathsf{ϵ}}_{\mathrm{z},\mathrm{g}}$ and ${\mathsf{ϵ}}_{\mathrm{x},\mathrm{a}},{\mathsf{ϵ}}_{\mathrm{y},\mathrm{a}},{\mathsf{ϵ}}_{\mathrm{z},\mathrm{a}}$ are the angular drift errors of gyroscopes and drift errors of accelerometers along the X-axis, Y-axis, and Z-axis, respectively.

Through differentiating the Equations (11) and (12), ${\mathsf{\delta }\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}$ can be written as [37]:

$${\mathsf{\delta }\mathsf{\omega }}_{\text{in}}^{\mathrm{n}}={\mathsf{\delta }\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\delta }\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$$

(17)

$${\mathsf{\delta }\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}=\left[\begin{array}{c}-{\mathsf{\omega }}_{\text{ie}}\sin \mathsf{\phi }\mathsf{\delta }\mathsf{\phi }\\ 0\\ -{\mathsf{\omega }}_{\text{ie}}\text{cos}\mathsf{\phi }\mathsf{\delta }\mathsf{\phi }\end{array}\right]$$

(18)

$${\mathsf{\delta }\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}=\left[\begin{array}{c}\frac{1}{{\mathrm{R}}_{\mathrm{N}}+\mathrm{h}}{\mathsf{\delta }\mathrm{V}}_{\mathrm{E}}-\frac{{\mathrm{V}}_{\mathrm{E}}}{{\left({\mathrm{R}}_{\mathrm{N}}+\mathrm{h}\right)}^2}\mathsf{\delta }\mathrm{h}\\ -\frac{1}{{\mathrm{R}}_{\mathrm{M}}+\mathrm{h}}{\mathsf{\delta }\mathrm{V}}_{\mathrm{N}}+\frac{{\mathrm{V}}_{\mathrm{N}}}{{\left({\mathrm{R}}_{\mathrm{M}}+\mathrm{h}\right)}^2}\mathsf{\delta }\mathrm{h}\\ -\frac{\text{tan}\mathsf{\phi }}{{\mathrm{R}}_{\mathrm{N}}+\mathrm{h}}{\mathsf{\delta }\mathrm{V}}_{\mathrm{E}}-\frac{{\mathrm{V}}_{\mathrm{E}}{\sec }^2\mathsf{\phi }}{{\mathrm{R}}_{\mathrm{N}}+\mathrm{h}}\mathsf{\delta }\mathsf{\phi }+\frac{{\mathrm{V}}_{\mathrm{E}}\text{tan}\mathsf{\phi }}{{\left({\mathrm{R}}_{\mathrm{N}}+\mathrm{h}\right)}^2}\mathsf{\delta }\mathrm{h}\end{array}\right]$$

(19)

where ${\mathsf{\delta }\mathrm{V}}_{\mathrm{E}}$ and${\mathsf{\delta }\mathrm{V}}_{\mathrm{N}}$ are the errors of east velocity and north velocity, respectively; $\mathsf{\delta }\mathsf{\phi }$ and $\mathsf{\delta }\mathrm{h}$ are the errors of latitude and height, respectively; and ${\mathsf{\delta }\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}$ and ${\mathsf{\delta }\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$ are the errors of ${\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}$ and ${\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}$, respectively.

The conventional velocity update equation can be written as follows [21]:

$${{\displaystyle \dot{\mathrm{V}}}}^{\mathrm{n}}={\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}{\mathrm{f}}_{\text{ib}}^{\mathrm{b}}-\left(2{\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}\right){\mathrm{V}}^{\mathrm{n}}+{\mathrm{g}}^{\mathrm{n}}$$

(20)

where ${\mathrm{V}}^{\mathrm{n}}$ is the velocity in the navigation frame; ${\mathrm{f}}_{\text{ib}}^{\mathrm{b}}$ is the output of accelerometers; and ${\mathrm{g}}^{\mathrm{n}}$ is the gravity expressed in the navigation frame (the model of ${\mathrm{g}}^{\mathrm{n}}$ is proposed in [38]).

The velocity error model can be obtained by perturbing Equation (20), and the DCM can be approximated by the following expression:

$${{\displaystyle \tilde{\mathrm{R}}}}_{\mathrm{n}}^{\mathrm{b}}={\mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}\left(\mathrm{I}-\mathsf{\Xi }\right)$$

(21)

where the designator $~$ in the above variables represents variables with errors. $\mathsf{\Xi }$ is a skew symmetric matrix of attitude error and $\mathsf{\Xi }$ is given by:

$$\mathsf{\Xi }=\left[\begin{array}{ccc}0& -\mathsf{\delta }\mathsf{\psi }& \mathsf{\delta }\mathsf{\theta }\\ \mathsf{\delta }\mathsf{\psi }& 0& -\mathsf{\delta }\mathsf{\varphi }\\ -\mathsf{\delta }\mathsf{\theta }& \mathsf{\delta }\mathsf{\varphi }& 0\end{array}\right]$$

(22)

However, the above equation may introduce errors if large attitude errors exist, so the error matrix of DCM $\left({\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}\right)$ is introduced for the derivation of velocity error equation. The DCM in Equation (21) can be replaced by:

$${{\displaystyle \tilde{\mathrm{R}}}}_{\mathrm{n}}^{\mathrm{b}}={\mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}-{\mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}{\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}$$

(23)

The velocity update equation with error interferences can be written as:

$${{\displaystyle \dot{\tilde{\mathrm{V}}}}}^{\mathrm{n}}={{\displaystyle \tilde{\mathrm{R}}}}_{\mathrm{b}}^{\mathrm{n}}{{\displaystyle \tilde{\mathrm{f}}}}_{\text{ib}}^{\mathrm{b}}-\left(2{{\displaystyle \tilde{\mathsf{\omega }}}}_{\text{ie}}^{\mathrm{n}}+{{\displaystyle \tilde{\mathsf{\omega }}}}_{\text{en}}^{\mathrm{n}}\right){{\displaystyle \tilde{\mathrm{V}}}}^{\mathrm{n}}+{{\displaystyle \tilde{\mathrm{g}}}}^{\mathrm{n}}$$

(24)

The velocity errors $\left({\mathsf{\delta }{\displaystyle \dot{\mathrm{V}}}}^{\mathrm{n}}\right)$ can be obtained by the difference between Equations (20) and (24), ignoring the second-order error product ${\mathrm{R}}_{\mathrm{n}}^{\mathrm{b}}{\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\delta }\mathrm{f}}_{\text{ib}}^{\mathrm{b}}$ and $\left(2{\mathsf{\delta }\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\delta }\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}\right){\mathsf{\delta }\mathrm{V}}^{\mathrm{n}}$, yields:

$${\mathsf{\delta }{\displaystyle \dot{\mathrm{V}}}}^{\mathrm{n}}={\dot{{\displaystyle \tilde{\mathrm{V}}}}}^{\mathrm{n}}-{{\displaystyle \dot{\mathrm{V}}}}^{\mathrm{n}}={\text{ R}}_{\mathrm{b}}^{\mathrm{n}}{\mathsf{\delta }\mathrm{f}}_{\text{ib}}^{\mathrm{b}}-{\mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}{\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}{\mathrm{f}}_{\text{ib}}^{\mathrm{b}}-\left(2{\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}\right){\mathsf{\delta }\mathrm{V}}^{\mathrm{n}}-\left(2{\mathsf{\delta }\mathsf{\omega }}_{\text{ie}}^{\mathrm{n}}+{\mathsf{\delta }\mathsf{\omega }}_{\text{en}}^{\mathrm{n}}\right){\mathrm{V}}^{\mathrm{n}}+{\mathsf{\delta }\mathrm{g}}^{\mathrm{n}}$$

(25)

The only unknown parameter in Equation (25) is ${\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}$, which can be obtained in terms of $\mathsf{\Phi }$, and the relationship between ${\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}$ and $\mathsf{\Phi }$ can be expressed as [27]:

$${\Delta \mathrm{R}}_{\mathrm{b}}^{\mathrm{n}}=2\left[\begin{array}{ccc}-{\mathrm{s}}_{\mathrm{n}}^2({\left(\mathsf{\delta }\mathsf{\theta }\right)}^2+{\left(\mathsf{\delta }\mathsf{\psi }\right)}^2)& {\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\psi }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\varphi }\mathsf{\delta }\mathsf{\theta }& -{\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\theta }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\varphi }\mathsf{\delta }\mathsf{\psi }\\ -{\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\psi }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\varphi }\mathsf{\delta }\mathsf{\theta }& -{\mathrm{s}}_{\mathrm{n}}^2({\left(\mathsf{\delta }\mathsf{\varphi }\right)}^2+{\left(\mathsf{\delta }\mathsf{\psi }\right)}^2)& {\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\varphi }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\theta }\mathsf{\delta }\mathsf{\psi }\\ {\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\theta }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\varphi }\mathsf{\delta }\mathsf{\psi }& -{\mathrm{s}}_{\mathrm{n}}(1+{\mathrm{c}}_{\mathrm{n}})\mathsf{\delta }\mathsf{\varphi }+{\mathrm{s}}_{\mathrm{n}}^2\mathsf{\delta }\mathsf{\theta }\mathsf{\delta }\mathsf{\psi }& -{\mathrm{s}}_{\mathrm{n}}^2({\left(\mathsf{\delta }\mathsf{\varphi }\right)}^2+{\left(\mathsf{\delta }\mathsf{\theta }\right)}^2)\end{array}\right]$$

(26)

The velocity error equation is obtained by inserting Equation (26) into Equation (25). There is no relationship between the model position error $\left(\mathsf{\delta }{\displaystyle \dot{\mathrm{P}}}\right)$ and the attitude, so the position error model is still linear as [39]:

$$\mathsf{\delta }{\displaystyle \dot{\mathrm{P}}}=\left[\begin{array}{c}\frac{{\mathsf{\delta }\mathrm{V}}_{\mathrm{N}}}{{\mathrm{R}}_{\mathrm{M}}+\mathrm{h}}-\frac{{\mathrm{V}}_{\mathrm{N}}}{{\left({\mathrm{R}}_{\mathrm{M}}+\mathrm{h}\right)}^2}\mathsf{\delta }\mathrm{h}\\ \frac{{\mathsf{\delta }\mathrm{V}}_{\mathrm{E}}}{\left({\mathrm{R}}_{\mathrm{N}}+\mathrm{h}\right)\text{cos}\mathsf{\phi }}-\frac{{\mathrm{V}}_{\mathrm{E}}\mathsf{\delta }\mathrm{h}}{{\left({\mathrm{R}}_{\mathrm{N}}+\mathrm{h}\right)}^2\text{cos}\mathsf{\phi }}-\frac{{\mathrm{V}}_{\mathrm{E}}\mathsf{\delta }\mathsf{\phi }}{\left({\mathrm{R}}_{\mathrm{N}}+\mathrm{h}\right){\cos }^2\mathsf{\phi }}\\ -{\mathsf{\delta }\mathrm{V}}_{\mathrm{D}}\end{array}\right]$$

(27)

where ${\mathsf{\delta }\mathrm{V}}_{\mathrm{D}}$ is the error of the down velocity.

The nonlinear error model of the MGWD system can be achieved through Equations (7), (25) and (27). The observation equation is the linear ZUPT equation, which is expressed as follows:

$${\mathrm{Z}}_{\mathrm{k}}={\text{HX}}_{\mathrm{k}}+{\mathrm{v}}_{\mathrm{k}}$$

(28)

where ${\mathrm{Z}}_{\mathrm{k}}$ is the difference between the estimate state $({\mathrm{V}}_{\mathrm{N}},{\mathrm{V}}_{\mathrm{E}},{\mathrm{V}}_{\mathrm{D}},\mathsf{\phi },\mathsf{\lambda },\text{ h})$ and the zero velocity and zero position (${\mathrm{V}}_{\mathrm{N},0},{\mathrm{V}}_{\mathrm{E},0},{\mathrm{V}}_{\mathrm{D},0},{\mathsf{\phi }}_0,{\mathsf{\lambda }}_0,{\mathrm{h}}_0$):

$${\mathrm{Z}}_{\mathrm{k}}=\left[\begin{array}{c}\begin{array}{c}{\mathrm{V}}_{\mathrm{N}}-{\mathrm{V}}_{\mathrm{N},0}\\ {\mathrm{V}}_{\mathrm{E}}-{\mathrm{V}}_{\mathrm{E},0}\\ {\mathrm{V}}_{\mathrm{D}}-{\mathrm{V}}_{\mathrm{D},0}\end{array}\\ \begin{array}{c}\mathsf{\phi }-{\mathsf{\phi }}_0\\ \mathsf{\lambda }-{\mathsf{\lambda }}_0\\ \mathrm{h}-{\mathrm{h}}_0\end{array}\end{array}\right]$$

(29)

$$\mathrm{H}=\left[\begin{array}{ccc}{\mathrm{H}}_{11}& {\mathrm{H}}_{12}& {\mathrm{H}}_{13}\\ {\mathrm{H}}_{21}& {\mathrm{H}}_{22}& {\mathrm{H}}_{23}\end{array}\right]$$

(30)

where $\mathsf{\lambda }$ is the longitude and ${\mathrm{V}}_{\mathrm{N},0},{\mathrm{V}}_{\mathrm{E},0},{\mathrm{V}}_{\mathrm{D},0},{\mathsf{\phi }}_0,{\mathsf{\lambda }}_0,\text{ and }{\mathrm{h}}_0$ represent the zero velocity and zero position. ${\mathrm{H}}_{12},{\mathrm{H}}_{13},{\mathrm{H}}_{21},\text{ and }{\mathrm{H}}_{23}$ are a 3 × 3 zero matrix; ${\mathrm{H}}_{11},{\mathrm{H}}_{22}$ are a 3 × 3 identity matrix.

3. Recursive Bayesian Estimation

In this section, we briefly review the theory of the recursive Bayesian estimation. The system is generally modeled as an SSM, which includes a system equation and an observation equation in the discrete time domain; the SSM can be written as follows:

$${\mathrm{x}}_{\mathrm{k}}={\mathrm{f}}_{\mathrm{k}-1}\left({\mathrm{x}}_{\mathrm{k}-1},{\mathsf{\varpi }}_{\mathrm{k}-1}\right)$$

(31)

$${\mathrm{z}}_{\mathrm{k}}={\mathrm{h}}_{\mathrm{k}}\left({\mathrm{x}}_{\mathrm{k}},{\text{ v}}_{\mathrm{k}}\right)$$

(32)

where ${\mathrm{f}}_{\mathrm{k}-1}:$ ${\mathrm{ℝ}}^{\mathrm{n}}\times {\mathrm{ℝ}}^{\mathrm{m}}\to {\mathrm{ℝ}}^{\mathrm{n}}$ is the system nonlinear function; ${\mathsf{\varpi }}_{\mathrm{k}-1}\in {\mathrm{ℝ}}^{\mathrm{m}}$ is a zero mean, white noise with known pdf $\mathrm{p}\left({\mathsf{\varpi }}_{\mathrm{k}-1}\right)$; ${\mathrm{x}}_{\mathrm{k}}$ and ${\mathrm{x}}_{\mathrm{k}-1}$ are the system state vectors at present time k and previous moment k-1; ${\mathrm{z}}_{\mathrm{k}}\in {\mathrm{ℝ}}^{\mathrm{n}}$ is the measurement from external reference; ${\mathrm{h}}_{\mathrm{k}}:$ ${\mathrm{ℝ}}^{\mathrm{n}}\times {\mathrm{ℝ}}^{\mathrm{r}}\to {\mathrm{ℝ}}^{\mathrm{p}}$ is the measurement function; and ${\mathrm{v}}_{\mathrm{k}}\in {\mathrm{ℝ}}^{\mathrm{r}}$ is a zero mean, white noise with known pdf $\mathrm{p}\left({\mathrm{v}}_{\mathrm{k}}\right)$. Figure 1 illustrates the recursive Bayesian estimation model in graphical form [40,41].

A more general system model (Markov model) and observation model corresponding to the Equations (31) and (32) can be given by the following expressions:

$${\mathrm{x}}_{\mathrm{k}}~\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}\right)$$

(33)

$${\mathrm{z}}_{\mathrm{k}}~\mathrm{p}({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}})$$

(34)

The definition of SSM in Equations (33) and (34) are based on the prior function and likelihood function. Both models are equivalent, but the general model is more convenient for the theoretical derivation.

The assumptions of recursive Bayesian estimation are that (1) the state vector ${\mathrm{x}}_{\mathrm{k}}$ is a hidden Markov process with initial distribution $\mathrm{p}\left({\mathrm{x}}_0\right)$ and ${\mathrm{x}}_{\mathrm{k}}$ is independent of the other state vectors except x_k−1, i.e., the conditional pdf $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{x}}_{0:\mathrm{k}-1}\right)=\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}\right)$; (2) the measurement process ${\mathrm{z}}_{\mathrm{k}}$ only depends upon ${\mathrm{x}}_{\mathrm{k}}$, i.e., the conditional pdf $\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}},{\text{ z}}_{1:\mathrm{k}-1}\right)=\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}\right)$.

To sum up, the evaluation of the Bayesian estimation is given by the following steps.

(1)

Prediction: evaluating the prior pdf $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)$ by the knowledge of the observation before time $\mathrm{k}$ in the light of the Chapman–Kolmogorov equation [42]:

$$\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)={{\displaystyle \int }}^{\text{}}\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}-1}\right)\mathrm{p}({\mathrm{x}}_{\mathrm{k}-1}|{\mathrm{z}}_{1:\mathrm{k}-1}){\text{dx}}_{\mathrm{k}-1}$$

(35)

where the pdf of initial state $\mathrm{p}({\mathrm{x}}_0|{\mathrm{z}}_0)$ can be assigned by $\mathrm{p}\left({\mathrm{x}}_0{|\mathrm{z}}_0\right)=\mathrm{p}\left({\mathrm{x}}_0\right)$. $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}-1}\right)$ is the prior distribution, which is given by [43]:

$$\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}-1}\right)={{\displaystyle \int }}^{\text{}}\mathsf{\delta }\left({\mathrm{x}}_{\mathrm{k}}-{\mathrm{f}}_{\mathrm{k}-1}\left({\mathrm{x}}_{\mathrm{k}-1},{\mathrm{w}}_{\mathrm{k}-1}\right)\right)\mathrm{p}\left({\mathsf{\varpi }}_{\mathrm{k}-1}\right)\mathrm{d}{\mathsf{\varpi }}_{\mathrm{k}-1}$$

(36)

where $\mathsf{\delta }(\cdot )$ is the Dirac delta function.

(2)

Updating: the posterior pdf $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)$ can be updated with the new measurement ${\mathrm{z}}_{\mathrm{k}}$ at time k in terms of the Bayes rule as follows [44]:

$$\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)=\frac{\mathrm{p}({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}})\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)}{\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)}$$

(37)

where $\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}}\right)$ is the likelihood function, which is written by the following equation [43]:

$$\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}}\right)={{\displaystyle \int }}^{\text{}}\mathsf{\delta }\left({\mathrm{y}}_{\mathrm{k}}-{\mathrm{h}}_{\mathrm{k}}\left({\mathrm{x}}_{\mathrm{k}},{\mathrm{v}}_{\mathrm{k}}\right)\right)\mathrm{p}\left({\mathrm{v}}_{\mathrm{k}}\right){\text{dv}}_{\mathrm{k}}$$

(38)

(3)

State estimation: once the posterior is obtained, the estimation of state $\left({{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}\right)$ and the covariance matrix of estimation error (${\mathrm{P}}_{{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}}$) are given by [45]:

$${{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}=\mathrm{E}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}}\right)={{\displaystyle \int }}^{\text{}}{\mathrm{x}}_{\mathrm{k}}\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}}\right){\text{dx}}_{\mathrm{k}}$$

(39)

$${\mathrm{P}}_{{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}}=\mathrm{E}\left({{\displaystyle \tilde{\mathrm{x}}}}_{\mathrm{k}}{{\displaystyle \tilde{\mathrm{x}}}}_{\mathrm{k}}^{\mathrm{T}}\right)={{\displaystyle \int }}^{\text{}}{{\displaystyle \tilde{\mathrm{x}}}}_{\mathrm{k}}{{\displaystyle \tilde{\mathrm{x}}}}_{\mathrm{k}}^{\mathrm{T}}\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}}\right){\text{dx}}_{\mathrm{k}}$$

(40)

where ${{\displaystyle \tilde{\mathrm{x}}}}_{\mathrm{k}}={\mathrm{x}}_{\mathrm{k}}-{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}$ is the estimation error and $\mathrm{E}\left(\cdot \right)$ is the expectation of the random variables.

4. PF Algorithm

Because the function $\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)$ in Equation (37) cannot typically be computed analytically, sequential Monte Carlo (SMC) approximation, the key idea of the PF, was proposed for the hidden states estimation of the dynamic systems [46,47]. The PF uses a finite set of weighted particles or samples to reconstruct the posterior pdf $\mathrm{p}\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)$ of the state vectors regardless of the nonlinear or non-Gaussian systems [48,49]. The implementation of the PF can be summarized in three main steps: sequential importance sampling (SIS), resampling, and roughening. The SIS plays a fundamental rule in the iteration of the PF since searching proper importance function decides the success of PF. Resampling aims at alleviating the particles’ or samples’ degeneracy and the roughing approach can be utilized to avoid the particles’ or samples’ impoverishment.

4.1. SIS Algorithm

The SIS algorithm steps from the literature [50,51]. The basic idea of SIS is the selection of particles or samples with weights to represent the complicated high dimensional distributions [52]. The posterior pdf $\mathrm{p}({\mathrm{x}}_{0:\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}})$ is approximated by the random particles or samples and weights $\left\{{\mathrm{x}}_{0:\mathrm{k}}^{\mathrm{i}},{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}|\mathrm{i}=1,2,\cdots \mathrm{N}\right\}$ at time k as:

$$\mathrm{p}({\mathrm{x}}_{0:\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}})={\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\mathsf{\delta }\left({\mathrm{x}}_{0:\mathrm{k}}-{\mathrm{x}}_{0:\mathrm{k}}^{\mathrm{i}}\right)$$

(41)

where $\mathrm{N}$ is the number of particles and ${\mathrm{x}}_{0:\mathrm{k}}$ is the system state.

The first task to implement the SIS is the selection of the importance function $\mathsf{\pi }\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right),$ which is proportional of the posterior pdf $\mathrm{p}({\mathrm{x}}_{0:\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}})$ and is easy to draw samples compared with $\text{ p}({\mathrm{x}}_{0:\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}})$. The weights can be updated in terms of the following equation:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}=\frac{\mathrm{p}\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)\mathrm{p}\left({\mathrm{z}}_{1:\mathrm{k}}\right)}{\mathsf{\pi }\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)}\propto \frac{\mathrm{p}\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)}{\mathsf{\pi }\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)}$$

(42)

Generally, the importance function is selected such that [53]:

$$\mathsf{\pi }\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)=\mathsf{\pi }\left({\mathrm{x}}_{\mathrm{k}}{|\mathrm{x}}_{\mathrm{k}-1},{\mathrm{z}}_{\mathrm{k}}\right)\mathsf{\pi }\left({\mathrm{x}}_{0:\mathrm{k}-1}{|\mathrm{z}}_{1:\mathrm{k}}\right)$$

(43)

Note that the Equation (41) can be factored as following equation according to the Bayesian rule [54]:

$$\mathrm{p}\left({\mathrm{x}}_{0:\mathrm{k}}{|\mathrm{z}}_{1:\mathrm{k}}\right)\propto \mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{0:\mathrm{k}}\right)\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}\right)\mathrm{p}\left({\mathrm{x}}_{0:\mathrm{k}-1}{|\mathrm{z}}_{1:\mathrm{k}-1}\right)$$

(44)

Therefore, the weights can be updated by substituting Equations (43) and (44) into Equation (42), yielding:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\propto {\mathrm{w}}_{\mathrm{k}-1}^{\left(\mathrm{i}\right)}\frac{\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)}{\mathsf{\pi }\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)}$$

(45)

where ${\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}$ is the variable obtained by the evaluation of ${\text{ x}}_{\mathrm{k}-1}^{\mathrm{i}}$ in the system model and ${\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}$ is the samples drawn from the importance function $\mathsf{\pi }\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right),$ which is related to the previous state ${\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}$ and current observation ${\mathrm{z}}_{\mathrm{k}}$. In the application of the MGWD system, the estimation of the current state is more important than the previous states, so Equation (41) can be rewritten as:

$$\mathrm{p}({\mathrm{x}}_{\mathrm{k}}|{\mathrm{z}}_{1:\mathrm{k}})={\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\mathsf{\delta }\left({\mathrm{x}}_{\mathrm{k}}-{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)$$

(46)

As indicated in Equations (45) and (46), the posterior pdf depends on the likelihood function, the priori pdf, and the importance function. Since the likelihood function and the priori pdf can be obtained in terms of the SSM, the choice of importance function is essential for PF. The optimal function, the likelihood function, and the priori function are the components of the importance function [54].

The optimal function $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)$ was proposed as the importance function in [41]:

$$\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)=\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)$$

(47)

Substituting Equation (47) into Equation (45) yields:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\propto {\mathrm{w}}_{\mathrm{k}-1}^{\left(\mathrm{i}\right)}\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)$$

(48)

The drawbacks of using optimal importance function are: (1) it is difficult to draw samples from the optimal function $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)$; (2) it needs the integration calculation to update the weights. The optimal function can be factorized as the following format based on the Bayesian rule:

$$\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)=\frac{\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)}{\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)}$$

(49)

As indicated in Equation (49), $\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)\propto \mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)$ if the likelihood function $\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)$ is more noisy and it is integral in ${\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}$ [55]. In this case, the importance function can be presented by the likelihood function, namely:

$$\mathsf{\pi }\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)=\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)$$

(50)

Then, the weight in Equation (45) can be updated by the following equation:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\propto {\mathrm{w}}_{\mathrm{k}-1}^{\left(\mathrm{i}\right)}\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)$$

(51)

However, the likelihood function as the importance function can only be used in the high signal-to-noise ratio (SNR) case. In this paper, the priori function is selected as the importance function because of the tradeoff between computational complexity and the feasibility of implementation. The importance function is written by:

$$\mathsf{\pi }\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}{|\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}},{\mathrm{z}}_{\mathrm{k}}\right)=\mathrm{p}({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}})$$

(52)

Then, the weights can be updated by:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\propto {\mathrm{w}}_{\mathrm{k}-1}^{\left(\mathrm{i}\right)}\mathrm{p}\left({\mathrm{z}}_{\mathrm{k}}|{\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)$$

(53)

The above equation shows that the weights only depend on the previous weights and the likelihood function, which are available from the knowledge of the observation equation and the pdf of the observation noise. In this application, we assume that the measurement noise of the MGWD system is additive, Gaussian white noise, so the likelihood function can be evaluated in a simple way [56]:

$$\mathrm{P}\left({\mathrm{z}}_{\mathrm{k}}│{\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\right)\approx \frac{1}{2\mathsf{\pi }{|\mathrm{R}|}^{1/2}}\text{ exp}\left(-{\left({\mathrm{z}}_{\mathrm{k}}-\mathrm{h}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)\right)}^{\mathrm{T}}{\mathrm{R}}^{-1}\left({\mathrm{z}}_{\mathrm{k}}-\mathrm{h}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}\right)\right)\right)$$

(54)

where R is the covariance matrix of measurement noise ${\mathrm{v}}_{\mathrm{k}}$.

The normalization of the particle weights are performed to guarantee the sum of the particle weights equal to 1, and the normalization of the weights can be expressed as follows:

$${\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}=\frac{{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}}{{{\displaystyle \sum }}_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}}$$

(55)

Under the premise that the initial pdf $\mathrm{p}({\mathrm{x}}_0|{\mathrm{z}}_0)$ is known, drawing particles ${\text{ x}}_0^{\mathrm{i}}~\mathrm{p}({\mathrm{x}}_0|{\mathrm{z}}_0),\text{ where i}=1,2,\mathrm{N}$, as:

$${\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}={\mathrm{f}}_{\mathrm{k}-1}\left({\mathrm{x}}_0^{\mathrm{i}},{\mathsf{\varpi }}_0^{\mathrm{i}}\right)$$

(56)

where ${\mathrm{x}}_0^{\mathrm{i}}$ is the initial particles and ${\mathsf{\varpi }}_0^{\mathrm{i}}$ is the noise vector which is drawn from the pdf of system noise $\mathrm{p}\left({\mathsf{\varpi }}_{\mathrm{k}-1}\right)$. The initial weights are given by:

$${\mathrm{w}}_0^{\mathrm{i}}=\mathrm{p}\left({\mathrm{z}}_0|{\mathrm{x}}_0^{\mathrm{i}}\right)$$

(57)

The SIS algorithm can only be implemented in special cases because the variance of the importance weights only increase over time; eventually, they give rise to particle degeneracy. Particle degeneracy means that the updating particles are useless for the approximation of a posterior pdf. In order to solve the particles degeneracy problem, a resampling method is described in next section.

4.2. Resampling Algorithm

The PF would suffer from the particle degeneracy without a resampling algorithm. Figure 2 shows the principle of the particle resampling.

The key of the resampling algorithm is that particles with large weights are replicated and particles with small weights are discarded [57]. The effective sample size ${\mathrm{N}}_{\text{eff}}$ is an indicator of the degree of the particles’ degeneracy and a metric of employing the particles’ resampling. ${\mathrm{N}}_{\text{eff}}$ is provided by the following equation [58]:

$${\mathrm{N}}_{\text{eff}}=\frac{\mathrm{N}}{1+{\mathrm{N}}^2\text{var}\left({\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\right)}\approx \frac{1}{{{\displaystyle \sum }}_{\mathrm{i}=1}^{\mathrm{N}}{\left({\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\right)}^2}$$

(58)

where $var\left({\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}\right)$ is the variation of the weight.

The particles should be resampled if ${\mathrm{N}}_{\text{eff}}$ is less than resample threshold $\left({\mathrm{N}}_{\text{th}}\right)$, which equals $4\mathrm{N}/5$ in this paper. Equation (58) shows that ${\mathrm{N}}_{\text{eff}}$ approaches 1 if the particles are severely degenerated; on the other hand, ${\mathrm{N}}_{\text{eff}}$ approaches $\mathrm{N}$ if the particles have equal weight $\left(1/\mathrm{N}\right)$. The number of the resampled particles is not necessarily the same as the propagated particles$\text{ N}$; however, the number of particles can be the same for computational convenience. Therefore, the total resampling particles are unchanged, still N, so the resampled particles have the same weight $\left(1/\mathrm{N}\right)$.

There are several ways to implement the resampling algorithm such as systematic resampling [59], multinomial resampling [60], residual resampling [61], etc. The resampling strategy in PF is sampling with replacement, that is, the particles drawn from the box will be replaced after the decimation. In this paper, the multinomial resampling approach is selected to alleviate particles’ degeneracy. The multinomial resampling can be considered as a series of Bernoulli trials. $\mathrm{N}$ random samples ${\mathrm{u}}^{\left(\mathrm{n}\right)},\text{ n}=1,2,\cdots \mathrm{N}$ are generated from the uniform distribution over $(0,1]$ and the probabilities of resampling the particles is the same as ${\mathrm{u}}^{\left(\mathrm{n}\right)}$ when the following inequality is satisfied:

$${\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}-1}{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}<{\mathrm{u}}^{\left(\mathrm{n}\right)}\le {\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}$$

(59)

Then, the $\text{nth}$ particle ${\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{n}\right)}$ with the $\text{nth}$ weight is the selection of the new samples or particles ${\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}$ of the resampling.

4.3. Roughing Strategy

The problem of resampling is that only a few particles are drawn, which is also referred to as samples impoverishment. Therefore, the particles lack diversities because the selection may include many repeated particles. In order to address the samples impoverishment problem, the random noise is added to the particles after resampling. The posterior particles ${\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)+}$ are written as:

$${\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)+}={\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)}+{\mathsf{\varrho }}_{\mathrm{k}}^{\left(\mathrm{i}\right)}$$

(60)

where ${\mathsf{\varrho }}_{\mathrm{k}}^{\left(\mathrm{i}\right)}$ is a zero mean random noise.

At this point, the particles after the roughing step can be propagated for the update of the weights and the posterior pdf in the next time cycle. The first two moments of estimation derived from the particles can be approximated by the following equations:

$${{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}=\mathrm{E}\left[{\mathrm{x}}_{\mathrm{k}}\right]\approx {\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\mathrm{i}}\left({\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)+}\right)$$

(61)

$${\mathrm{P}}_{{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}}=\mathrm{E}\left[\left({\mathrm{x}}_{\mathrm{k}}-{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}\right){\left({\mathrm{x}}_{\mathrm{k}}-{{\displaystyle \widehat{\mathrm{x}}}}_{\mathrm{k}}\right)}^{\mathrm{T}}\right]\approx {\displaystyle \sum }_{\mathrm{i}=1}^{\mathrm{N}}{\mathrm{w}}_{\mathrm{k}}^{\mathrm{i}}\left({\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)+}\right){\left({\mathrm{x}}_{\mathrm{k}}^{\left(\mathrm{i}\right)+}\right)}^{\mathrm{T}}$$

(62)

5. Results and Discussion

The objectives of the designed experiments are to demonstrate that: (1) the performance of the PF with NEM or NNEM is better than KF with LEM under small-angle attitude error condition (Experiment 1); (2) PF with NNEM can provide more accurate states estimation for the MGWD system with large-angle attitude error (Experiment 2). The configuration of the MGWD device and the details of the experimental data collection are briefly described in the first part of Section 5.

5.1. MGWD Device for Validation of Experimental Results

The MGWD device consists of the azimuth MEMS sensor, the pitch MEMS sensor, and the microcontroller. Figure 3 shows the configuration of the MGWD device. Azimuth MEMS sensor provides the angular rate of the drill bit along the Z-axis while the angular rate along the X-axis is obtained from the pitch MEMS sensor. For implementation of the pure INS algorithm, the angular rate along the Y-axis is evaluated by the specific force obtained from the pitch MEMS sensor [12].

The MEMS sensor (SCC1300-D04, Murata Manufacturing Co., Ltd, Kyoto, Japan) is a combined gyroscope and accelerometer component and the sensor is characterized by low cost, small diameter, and high performance. The microcontroller (CC2538, Texas Instruments, Dallas, TX, USA) is a high-speed System-on-Chip (SoC) combined with ARM cortex-M3 processor.

Table 1. Technical specifications for SCC1300-D04.

Parameters	Gyroscope	Parameters	Accelerometer
Offset Short Term Instability	<2.1 °/h	Offset Error	±70 mg
Angular Random Walk	0.86 °/$\sqrt{\mathrm{h}}$	Linearity Error	±40 mg
Noise Density	0.02 (°/s/$\sqrt{\mathrm{h}})$	Noise	5 ~ 7 mg
Temperature	−40 ~ +125 °C	Temperature	−40 ~ +125 °C

and

Table 2. Technical specifications for CC2538.

Parameters	Values	Parameters	Values
Processor	ARM Cortex-M3	Debugging	cJTAG and JTAG
Frequency	24 MHz	RF	2.4 GHz IEEE 802.15.4 Transceiver
Peripherals	USB/I2C/SSI/UART	Size	8 mm $\times$ 8 mm
Temperature	−40 ~ +125 °C	Voltage	2 V ~ 3.6 V

provide the technical specification of the MEMS sensor and the microcontroller, respectively.

The types of data communication between the MEMS sensor and microcontroller is the serial peripheral interfaces (SPI) interface with 115,200 bit/s data transmission rate. The data collection software evaluates the navigation solutions with angular rate and specific force received from MGWD device through the universal serial bus (USB) with 20 Hz sample rate.

Figure 4 shows the process of the data collection and the contents of the experiments. The computer continuously reads the angular rate and the specific force of the drill bit from the static MGWD device. Therefore, the zero velocity and zero position are readily available as the reference of experiments, designed in terms of the size of attitude error.

In the experiment, we collect original data from the static MGWD device for the period of 10 min to demonstrate the performance of the PF and the accuracy of the proposed NNEM. Figure 5 shows the original data from two-axis gyroscopes and three-axis accelerometers. The outputs of gyroscopes include the noise and the angular rate of the Earth projected onto the X-axis and Z-axis of the MGWD device. The X-axis and the Y-axis outputs of accelerometers are only the noise since the device is under static condition while the Z-axis output of accelerometer consists of the noise and gravity.

5.2. Experiment 1: Comparison of KF with LEM and PF with NEM under Small-Angle Attitude Error Condition

The aim of Experiment 1 is to confirm that the performance of the PF with NEM is better than the performance of KF with LEM under small-angle attitude error conditions. The assumption of KF is that all the states are Gaussian and the SSM is linear. However, the complicated drilling environments introduce such strong interference to the MGWD device that the linear SSM and Gaussian states are impossible.

The first 50 s are utilized for initial alignment; the results of the initial alignment for roll, pitch, and azimuth are −0.022108$°$, 0.180922$°$, and −10.351435$°$, which are also set as the initial attitude in this experiment. Therefore, the initial attitude error is so small that the DCM can be approximated by the attitude. The rest of parameters related to the KF and PF are given as follows:

• Initial covariance matrix of system noise: R = diag([0.1,0.1, 0.1,0.1, 0.1, 0.1]);
• Initial covariance matrix of observation noise: Q = diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]);
• Initial position: latitude 126.6879$°$; longitude 45.7776$°$; height 124 m;
• Initial velocity: 0.

Figure 6 illustrates the initial particles and initial weights.

Figure 7 a₁, a₂, b₁, b₂, c₁, c₂ show the particles of the state errors together with the corresponding weights drawn from the priori pdf $\left\{\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}^{\mathrm{i}}|{\mathrm{x}}_{\mathrm{k}-1}^{\mathrm{i}}\right)\right\}$ at time k. The posterior pdf $\left\{\mathrm{p}\left({\mathrm{x}}_{\mathrm{k}}|{\mathrm{z}}_{\mathrm{k}}\right)\right\}$ can be evaluated by the Equation (46) after the particles and the corresponding weights are obtained. The number of the particles and weights for each state are 200. The particles and weights are updated at every time circle; meanwhile, the posterior pdf is updated.

Figure 8 presents the evaluation of state density in terms of the updated pdf; the results illustrates that the pdf of states are partially Gaussian. Therefore, the KF is suboptimal since the assumption is that all states are Gaussian in KF while the PF would not be limited by the non-Gaussian assumption.

Figure 9 gives a performance comparison of the KF with LEM and PF with NEM or NNEM under small-angle attitude error. Figure 9 a₁, a₂ show that both the latitude and the longitude estimated by KF and PF converge to 45.7776$°$ and 126.6879$°$, which are the same as the reference values (initial latitude and initial longitude). However, the height estimated by KF is 132.0782 m while the height estimated by PF is 123.9999 m, which is almost the same as the reference height (124 m). Therefore, the NEM or NNEM can restrain the errors of height effectively under small-angle attitude error conditions.

In Figure 9 b₁, b₂ the north velocity, east velocity, and down velocity estimated by PF are close to zero (0.0002 m/s, 0.0001 m/s, and 0.0001 m/s) while the errors of north velocity, east velocity, and down velocity estimated by KF are up to 0.3702 m/s, 0.2839 m/s, and 2.2953 m/s respectively. Therefore, the PF with NEM or NNEM gives better performance than KF with LEM does for the velocity estimation under small-angle attitude error.

In Figure 9 c₁, c₂ the roll, pitch, and azimuth of MGWD system estimated by PF with NEM or NNEM largely converge to −0.022125$°$, 0.180922$°$, and −10.353632$°$, respectively, which are the same as the reference points provided by the initial alignment, while the results of pitch and azimuth estimated by KF deviate from the reference values by about 8$°$ and 10$°$, respectively. Therefore, the PF with NEM or NNEM can restrain the pitch and azimuth error effectively.

5.3. Experiment 2: Validation of the Performance of NNEM with Large-Angle Attitude Error

The objective of Experiment 2 is the validation of the performance of NNEM with large-angle attitude error. The experimental data used in this section are the same as those in Experiment 1 and the initial parameter settings are also the same as in experiment 1 apart from the initial attitude. The assumption of this experiment is that the initial alignment fails to provide an accurate initial attitude. We set the initial roll, pitch, and azimuth as 0$°$. The experiment is practical since the initial attitude is generally difficult to obtain in the application of downhole drilling.

Figure 10 a₁, a₂, b₁, b₂, c₁, c₂ show the estimation of roll, pitch, and azimuth of the MGWD system and the corresponding estimation errors. The roll, pitch, and azimuth converge to $-0.021976°$, $0.184392°,$ and $-9.893302°$, respectively, which are close to the reference values for initial alignment. The selection of initial attitude has little influence on the convergence of the attitude estimation by PF with NNEM since the roll converges to the reference value at around 320 s, the pitch converges to the reference value at around 180 s, and the azimuth converges to the reference value at around 360 s, as illustrated in Figure 10 a₁, b₁, and c₁. Figure 10 a₂, b₂, c₂ show that the error of roll and error of pitch estimated by PF with both NNEM and NEM are less than $0.0001°$ and $0.001°$. The error of azimuth estimated by PF with NEM is $2.459532°$ while the estimation error of azimuth with NNEM is $2.5\times {10}^{-5}°$. In addition, the accurate solutions of roll and pitch presented in Figure 10 demonstrate that the error of roll and error of pitch couple with the accuracy of east velocity and north velocity. Therefore, the PF with NNEM can greatly restrain the azimuth error under large-angle latitude error conditions.

Figure 11 a₁, a₂, b₁, b₂, c₁, c₂ illustrate the results of the north velocity, east velocity, and down velocity and the corresponding estimation errors of the MGWD system. The velocity estimated by PF, regardless of NEM or NNEM, apparently converges to zero velocity while the velocity error estimated by PF with NNEM is approximately 10 times smaller than KF with LEM. Therefore, the velocity errors estimated by PF with NNEM can be reduced effectively.

Figure 12 a₁, a₂, b₁, b₂, c₁, c₂ describe the estimation of latitude, longitude, and height of the MGWD system and the corresponding estimation errors. The latitude and longitude estimated by KF with LEM, PF with NEM, and PF with NNEM are almost close to the reference values $45.7776°$ for latitude, $126.6879°$ for longitude. The height estimated by PF with NNEM is $123.9996\text{ m}$ (reference height: 124 m) while the error of height estimated by KF with LEM is about $5\text{ m}$ and the error of height estimated by PF with NEM is approximately $3\text{ m}$. The results demonstrate that the PF with NNEM can constrain the error growth of height effectively.

Table 3. Navigation solutions and corresponding errors estimated by PF with NNEM, PF with NEM, and KF with LEM in Experiment 2.

Parameters	PF$+$NNEM	PF$+$NEM	KF + LEM
$\mathsf{\phi }$	45.777664$°$	45.777613$°$	45.777935$°$
$\mathsf{\lambda }$	126.687953$°$	126.687934$°$	126.688326$°$
$\mathrm{h}$	123.999686 m	123.652484 m	120.945752 vm
${\mathrm{V}}_{\mathrm{N}}$	0.002984 m/s	0.0197424 m/s	0.983472 m/s
${\mathrm{V}}_{\mathrm{E}}$	−0.000019 m/s	−0.234530 m/s	1.342358 m/s
${\mathrm{V}}_{\mathrm{D}}$	0.005637 m/s	0.287584 m/s	1.846332 m/s
$\mathsf{\varphi }$	−0.021976$°$	−0.034748$°$	−0.053672$°$
$\mathsf{\theta }$	0.184392$°$	−0.068584$°$	−0.026478$°$
$\mathsf{\psi }$	−9.893302$°$	−2.527547$°$	−2.599384$°$
$\mathsf{\delta }\mathsf{\phi }$	−0.362394 × 10−7	−0.000014$°$	−0.000238$°$
$\mathsf{\delta }\mathsf{\lambda }$	−0.457395 × 10−8	−0.000106°	−0.000056$°$
$\mathsf{\delta }\mathrm{h}$	−0.000348 m	−0.196528 m	−8.872501 m
${\mathsf{\delta }\mathrm{V}}_{\mathrm{N}}$	0.075922 m/s	0.128473 m/s	0.248382 m/s
${\mathsf{\delta }\mathrm{V}}_{\mathrm{E}}$	0.048975 m/s	0.056483 m/s	0.287349 m/s
${\mathsf{\delta }\mathrm{V}}_{\mathrm{D}}$	−0.00648 m/s	−0.039320 m/s	0.877265 m/s
$\mathsf{\delta }\mathsf{\varphi }$	−0.000464$°$	0.003502$°$	0.004882$°$
$\mathsf{\delta }\mathsf{\psi }$	0.000353$°$	0.004528$°$	0.005216$°$
$\mathsf{\delta }\mathsf{\psi }$	−0.000025$°$	2.459532$°$	1.879347$°$

lists the results of Experiment 2.

6. Conclusions

This paper proposes a NNEM for MGWD system under large-angle attitude error conditions. The error of DCM as the intermediate variable is introduced for the derivation of NNEM, which can effectively eliminate the approximation of errors existing in the derivation of conventional NEM. The evaluation of the state density shows that the system states are partially Gaussian, so the KF is suboptimal in this application. PF can improve the estimation performance of nonlinear problems by approximating the posterior pdf with the particles and weights. The PF with NEM gives a better performance than KF with LEM for the estimation of height, velocity, pitch, and azimuth in Experiment 1. The NNEM of the MGWD system provides an accurate estimation of position, velocity, and attitude compared with PF with NEM and KF with LEM in Experiment 2.