Constrained Unscented Particle Filter for SINS/GNSS/ADS Integrated Airship Navigation in the Presence of Wind Field Disturbance

Abstract

Due to the disturbance of wind field, it is difficult to achieve precise airship positioning and navigation in the stratosphere. This paper presents a new constrained unscented particle filter (UPF) for SINS/GNSS/ADS (inertial navigation system/global navigation satellite system/atmosphere data system) integrated airship navigation. This approach constructs a wind speed model to describe the relationship between airship velocity and wind speed using the information output from ADS, and further establishes a mathematical model for SINS/GNSS/ADS integrated navigation. Based on these models, it also develops a constrained UPF to obtain system state estimation for SINS/GNSS/ADS integration. The proposed constrained UPF uses the wind speed model to constrain the UPF filtering process to effectively resist the influence of wind field on the navigation solution. Simulations and comparison analysis demonstrate that the proposed approach can achieve optimal state estimation for SINS/GNSS/ADS integrated airship navigation in the presence of wind field disturbance.

1. Introduction

The stratosphere, which is located at the bottom of the near space with the altitude between 10 km to 50 km, is a new region of human activities in space [1,2]. As a typical aircraft flying in the stratosphere, an airship has the advantages of high flight altitude, large coverage, and low-cost [2,3]. However, due to the disturbance of wind field in the stratosphere, airship positioning and navigation remain a challenging research problem.

The airship relies on an integrated navigation system for positioning and navigation. Inertial navigation system (INS)/global navigation satellite system (GNSS) integration is the most widely used integrated navigation system [4,5]. However, due to the near-far and ionosphere effects, satellite signals may be shielded or submerged. The signal reception rate declines to 60% or even lower in environments with valleys, underground areas, or business districts [5,6]. When satellite signals cannot be received normally, the error of INS will be accumulated over time, deteriorating the navigation accuracy [7,8,9].

The atmosphere data system (ADS) utilizes pressure, temperature, and geographic information (such as position calibration data, airspeed, airflow data, and time) provided by the atmospheric sensor to calculate the velocity and altitude of an aerial vehicle [10,11]. ADS does not rely on external conditions to obtain the navigation information. It has high reliability and its performance is not affected by height, topography, and other factors [11,12]. Given its advantages, ADS is an ideal auxiliary for INS/GNSS navigation to compensate the error of INS, especially when GNSS signal is poor, rendering INS/GNSS/ADS integration as a promising scheme to improve the navigation accuracy and reliability [13]. However, when using INS/GNSS/ADS integration for airship navigation, the navigation performance is significantly disturbed by wind field due to the long-span flexible balloon structure and low speed of the airship.

The performance of INS/GNSS/ADS integration is dominated by the filtering algorithm used [14,15,16]. The particle filter (PF) is an optimal recursive Bayesian filtering method based on Monte Carlo simulation by producing a sample of independent random variables according to a conditional probability distribution [17,18]. It is easy to implement, suitable for high-dimensional problems, and capable of handling nonlinear and non-Gaussian models [18,19]. However, the accuracy of PF relies on importance sampling. PF also requires the design of a proposal distribution to accurately approximate the posterior distribution, while in practice it is difficult to find such a proposal distribution [20,21].

The unscented Kalman filter (UKF) is able to generate a proposal distribution with larger high-order moment and the mean that is close to the true mean of the target distribution [22,23]. The combination of UKF into PF results in the so-called unscented particle filter (UPF), which is widely used in many fields including aircraft navigation, underwater navigation, GPS precise point positioning, nonlinear system identification, and audio source separation [24,25,26,27,28]. However, UPF is incapable of handling the disturbance of wind field for INS/GNSS/ADS integrated airship navigation. The robust adaptive UPF (RAUPF) is a method for system state estimation in the presence of abnormal observations and kinematic model noise [29,30]. This method adaptively determines the equivalent weight function according to robust estimation and adaptively adjusts the adaptive factor constructed from predicted residuals to inhibit the disturbances of abnormal observation and kinematic model noise. However, the wind field in the stratosphere has a unique form of disturbance, which is completely different from kinematic model noise and observation noise, requiring a special way to handle. Further, it cannot guarantee the covariance matrices in the filtering process are positive-definite, leading to the stability issue.

This paper presents a new constrained UPF for SINS/GNSS/ADS integrated airship navigation under the disturbance of wind field. A model of wind speed is established using ADS output information to describe the relationship between airship velocity and wind speed. Further, a mathematical model is also established for SINS/GNSS/ADS integrated airship navigation. Based on these models, a constrained UPF is developed to fuse SINS and GNSS measurements to generate the optimal state estimation for SINS/GNSS/ADS integration. This constrained UPF applies the wind speed model as a constraint to the UPF filtering process to effectively inhibit the influence of wind filed on the navigation solution. Simulations and comparison analysis have been conducted to comprehensively evaluate the performance of the proposed constrained UPF for SINS/GNSS/ADS integrated airship navigation in the presence of wind disturbance.

2. Mathematical Model of SINS/GNSS/ADS Integrated Navigation

2.1. Wind Speed Model

In the E–N–U (East-North-Up) geography coordinate system, the wind speed model can be expressed as

$${\mathit{v}}_W={\mathit{v}}_{Wc}+{\mathit{v}}_{Wr}$$

(1)

where, ${\mathit{v}}_W$ is the wind speed, ${\mathit{v}}_{Wc}$ is a random constant wind, and ${\mathit{v}}_{Wr}$ is a random wind expressed as the first-order Markov process, and

$$\begin{array}{l}{v}_{WE}=v_{WcE}+v_{WrE}\\ v_{WN}=v_{WcN}+v_{WrN}\\ v_{WU}=v_{WcU}+v_{WrU}\\ {\dot{v}}_{Wci}=0\\ {\dot{v}}_{Wri}=-\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{${\tau }_{Wri}$}\right.v_{Wri}+{\omega }_i\end{array}$$

(2)

where $v_{Wi}(i=E,N,U)$ stands for the projection of wind speed ${\mathit{v}}_W$ in the East, North, and Up directions, as shown in Figure 1; the symbols $v_{Wci}$ and $v_{Wri}$ have the similar meanings as $v_{Wi}$; and ${\omega }_i(i=E,N,U)$ is the white noise in the corresponding direction.

Figure 1. The components of the wind speed in the coordinate system E–N–U.

As shown in Figure 2, via the wind speed, the airship velocity relative to the Earth can be expressed as

$${\mathit{v}}_e={\mathit{v}}_a+{\mathit{v}}_W$$

(3)

where ${\mathit{v}}_e$ and ${\mathit{v}}_a$ denote the airship velocities relative to the Earth and atmosphere, ${\alpha }_1$ is the angle between the horizontal axis and wind speed ${\mathit{v}}_W$, and ${\alpha }_2$ and ${\alpha }_3$ are the angles from ${\mathit{v}}_e$ and ${\mathit{v}}_a$ to the horizontal axis, respectively.

Figure 2. Wind speed synthetic relationship.

2.2. System State Equation of SINS/GNSS/ADS Integrated Navigation

The base coordinate system for establishment of the system state model is the E–N–U geography coordinate system. The system state $\mathit{X}\left(t\right)$ of SINS/GNSS/ADS is defined as

$$\begin{gathered} \mathit{X}\left(t\right)=\left[\begin{array}{ccc}{\mathit{x}}_{SINS}\left(t\right)& {\mathit{x}}_{BD}\left(t\right)& {\mathit{x}}_{ADS}\left(t\right)\end{array}\right] \\ \mathit{X}\left(t\right)=\left[\begin{array}{ccc}{\mathit{x}}_{SINS}\left(t\right)& {\mathit{x}}_{BD}\left(t\right)& {\mathit{x}}_{ADS}\left(t\right)\end{array}\right] \\ {\mathit{x}}_{SINS}\left(t\right)={\left[{\varphi }_E{\varphi }_N{\varphi }_U\delta v_{SE}\delta v_{SN}\delta v_{SU}\delta L_S\delta {\lambda }_S\delta h_S\right]}^{\mathrm{T}} \\ {\mathit{x}}_{GNSS}\left(t\right)={\left[\delta v_{GE}\delta v_{GN}\delta v_{GU}\delta L_G\delta {\lambda }_G\delta h_G\right]}^{\mathrm{T}} \\ {\mathit{x}}_{ADS}\left(t\right)={\left[\delta v_{WcE}\delta v_{WcN}\delta v_{WcU}\delta v_{WrE}\delta v_{WrN}\delta v_{WrU}\right]}^{\mathrm{T}} \end{gathered}$$

(4)

where ${\varphi }_E$, ${\varphi }_N$ and ${\varphi }_U$ represent the platform misalignment angle of SINS; $\delta v_{SE}$, $\delta v_{SN}$ and $\delta v_{SU}$ the velocity error of SINS; $\delta {\lambda }_S$, $\delta L_S$ and $\delta h_S$ the position error of SINS; $\delta v_{GE}$, $\delta v_{GN}$ and $\delta v_{GU}$ the velocity error of GNSS; $\delta {\lambda }_G$, $\delta L_G$ and $\delta h_G$ the position error of GNSS; $\delta v_{WcE}$, $\delta v_{WcN}$ and $\delta v_{WcU}$ the constant wind speed error; and $\delta v_{WrE}$, $\delta v_{WrN}$ and $\delta v_{WrU}$ the random wind speed error.

The system state equation is

$$\dot{\mathit{X}}\left(t\right)=f\left(\mathit{X}\left(t\right)\right)+\mathit{W}\left(t\right)$$

(5)

where $f\left(\mathit{X}\left(t\right)\right)$ is a nonlinear function of the state and is expressed by

$$f\left(\mathit{X}\left(t\right)\right)=\left[\begin{array}{l}{\mathit{C}}_{\omega }^{-1}\left[\left(I-{\mathit{C}}_n^c\right){\widehat{\omega }}_{in}^n+{\mathit{C}}_n^c\delta {\omega }_{in}^n-{\mathit{C}}_b^c\delta {\omega }_{ib}^b\right]\\ \left[I-{\left({\mathit{C}}_n^c\right)}^{\mathrm{T}}\right]{\mathit{C}}_b^c{\widehat{\mathit{f}}}_{sf}^b+{\left({\mathit{C}}_n^c\right)}^{\mathrm{T}}{\mathit{C}}_b^c\delta {\mathit{f}}_{sf}^b-\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)\times \mathit{v}\\ -\left(2{\widehat{\omega }}_{ie}^n+{\widehat{\omega }}_{en}^n\right)\times \delta \mathit{v}+\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times \delta \mathit{v}+\delta \mathit{g}\\ \frac{v_N}{R_M+h}-\frac{\left(v_N-\delta v_N\right)}{\left(R_M-\delta R_M\right)+\left(h-\delta h\right)}\\ \frac{v_E\sec L}{R_N+h}-\frac{\left(v_E-\delta v_E\right)\sec \left(L-\delta L\right)}{\left(R_N-\delta R_N\right)+\left(h-\delta h\right)}\\ \delta v_U\\ -\delta v_{GE}/{\tau }_{GvE}\\ -\delta v_{GN}/{\tau }_{GvN}\\ -\delta v_{GU}/{\tau }_{GvU}\\ -\delta L_G/{\tau }_{GL}\\ -\delta {\lambda }_G/{\tau }_{G\lambda }\\ -\delta h_G/{\tau }_{Gh}\\ {\mathit{O}}_{3\times 1}\\ -\delta v_{WrE}/{\tau }_{wE}\\ -\delta v_{WrN}/{\tau }_{wN}\\ -\delta v_{WrU}/{\tau }_{wU}\end{array}\right]$$

(6)

where ${\mathit{C}}_{\omega }^{}$ is the Euler platform error angle matrix; ${\mathit{C}}_n^c$ and ${\mathit{C}}_b^c$ are the attitude transformation matrices; $\delta \mathit{g}$ is the errors of gravity; $\delta {\mathit{\omega }}_{ib}^b$ is the measurement error of the gyro; ${\mathit{\omega }}_{ie}^n$ is the rotational angular velocity of the Earth; ${\mathit{\omega }}_{en}^n$ is the angular velocity of the vehicle relative to the Earth; ${\mathit{\omega }}_{ie}^n$ is the rotational angular velocity of the Earth; ${\widehat{\mathit{\omega }}}_{ie}^n$, ${\widehat{\mathit{\omega }}}_{en}^n$ and ${\widehat{\mathit{\omega }}}_{in}^n$ are the actual values of ${\mathit{\omega }}_{ie}^n$, ${\mathit{\omega }}_{en}^n$ and ${\mathit{\omega }}_{in}^n$ in the actual navigation frame; $\delta {\mathit{\omega }}_{ie}^n$, $\delta {\mathit{\omega }}_{en}^n$ and $\delta {\mathit{\omega }}_{in}^n$ represent the corresponding errors; $\delta {\mathit{v}}_i$ represents the velocity error in the corresponding direction; $L$ and $h$ are the longitude and height of the airship; $\delta L$ and $\delta h$ are the errors of $L$ and $h$, respectively; ${\tau }_i$ ($i=E,N,U$) is the relevant time; ${\widehat{\mathit{f}}}_{sf}^b$ and $\delta {\mathit{f}}_{sf}^b$ are the specific force and its associated error, respectively; $R_M$ and $R_N$ are the meridian and prime vertical radiuses of curvature; and $\delta R_M$ and $\delta R_N$ are the errors of $R_M$ and $R_N$, respectively.

The system noise vector $\mathit{W}\left(t\right)$ is described as

$$\mathit{W}\left(t\right)={\left[W_i\right]}^{\mathrm{T}}i=1,2,\cdots ,21$$

(7)

where $W_i,i=1,2,\cdots ,21$ are the random noise of the state.

2.3. Measurement Equation of SINS/GNSS/ADS Integrated Navigation

SINS/GNSS/ADS integration consists of the SINS/GNSS subsystem and SINS/ADS subsystem. The measurement equation of the SINS/GNSS subsystem is obtained based on the position information integration, which is expressed by

$${\mathit{z}}_1\left(t\right)=\left[\begin{array}{c}\left(L_S-L_G\right)R_M+V_{\delta L}\\ \left({\lambda }_S-{\lambda }_G\right)R_N\cos L+V_{\delta \lambda }\\ h_S-h_G+V_{\delta h}\end{array}\right]$$

(8)

where $L_S$, ${\lambda }_S$ and $h_S$ are the latitude, longitude and altitude of SINS, respectively; $L_G$, ${\lambda }_G$ and $h_G$ are the latitude, longitude and altitude of GNSS, respectively; and $V_{\delta L}$, $V_{\delta \lambda }$ and $V_{\delta h}$ are the errors of the GNSS. It should be noted that the errors of these sensors are considered to be known in this paper.

The measurement equation of SINS/ADS subsystem is expressed by

$${\mathit{v}}_A=\left[\begin{array}{ccc}1& {\varphi }_U& -{\varphi }_N\\ -{\varphi }_U& 1& {\varphi }_E\\ {\varphi }_N& -{\varphi }_E& 1\end{array}\right]\left[\begin{array}{c}{v}_E+v_{wcE}+v_{wrE}\\ v_N+v_{wcN}+v_{wrN}\\ v_U+v_{wcU}+v_{wrU}\end{array}\right]$$

(9)

$$\begin{array}{l}{v}_{AE}=v_E+v_{wcE}+v_{wrE}+{\varphi }_U{v}_N+{\varphi }_U{v}_{wcN}+{\varphi }_U{v}_{wrN}-{\varphi }_N{v}_U-{\varphi }_N{v}_{wcU}-{\varphi }_N{v}_{wrU}\\ v_{AN}=-{\varphi }_U{v}_E-{\varphi }_U{v}_{wcE}-{\varphi }_U{v}_{wrE}+v_N+v_{wcN}+v_{wrN}+{\varphi }_E{v}_U+{\varphi }_E{v}_{wcU}+{\varphi }_E{v}_{wrU}\\ v_{AU}={\varphi }_N{v}_E+{\varphi }_N{v}_{wcE}+{\varphi }_N{v}_{wrE}-{\varphi }_E{v}_N-{\varphi }_E{v}_{wcN}-{\varphi }_E{v}_{wrN}+v_U+v_{wcU}+v_{wrU}\end{array}$$

(10)

$$\begin{array}{ll}{\mathit{z}}_2\left(t\right)& =\left[\begin{array}{l}{v}_{SE}-v_{ADSE}\\ v_{SN}-v_{ADSN}\\ v_{SU}-v_{ADSU}\end{array}\right]\\ & =\left[\begin{array}{l}\delta v_E-v_{wcE}-v_{wrE}-{\varphi }_U{v}_N-{\varphi }_U{v}_{wcN}-{\varphi }_U{v}_{wrN}+{\varphi }_N{v}_U+{\varphi }_N{v}_{wcU}+{\varphi }_N{v}_{wrU}+V_{vE}\\ \delta v_N+{\varphi }_U{v}_E+{\varphi }_U{v}_{wcE}+{\varphi }_U{v}_{wrE}-v_{wcN}-v_{wrN}-{\varphi }_E{v}_U-{\varphi }_E{v}_{wcU}-{\varphi }_E{v}_{wrU}+V_{vN}\\ \delta v_U-{\varphi }_N{v}_E-{\varphi }_N{v}_{wcE}-{\varphi }_N{v}_{wrE}+{\varphi }_E{v}_N+{\varphi }_E{v}_{wcN}+{\varphi }_E{v}_{wrN}-v_{wcU}-v_{wrU}+V_{vU}\end{array}\right]\end{array}$$

(11)

where $v_{EI}$, $v_{NI}$ and $v_{UI}$ are the velocities of SINS and $v_{EA}$, $v_{NA}$ and $v_{UA}$ are the velocities of ADS. It should be noted that the velocity used in this paper is relative to the geographic frame and is calculated from the airspeed.

The system measurement equation of SINS/GNSS/ADS integration is described as

$$\mathit{Z}\left(t\right)=\left[\begin{array}{l}{\mathit{z}}_1\left(t\right)\\ {\mathit{z}}_2\left(t\right)\end{array}\right]=h\left(\mathit{X}\left(t\right)\right)+\mathit{V}\left(t\right)$$

(12)

where

$$h\left(\mathit{X}\left(t\right)\right)=\left[\begin{array}{l}{R}_M\cdot \delta L\\ R_N\cos L\cdot \delta \lambda \\ \delta h\\ \delta v_E-v_{wcE}-v_{wrE}-{\varphi }_U{v}_N-{\varphi }_U{v}_{wcN}-{\varphi }_U{v}_{wrN}+{\varphi }_N{v}_U+{\varphi }_N{v}_{wcU}+{\varphi }_N{v}_{wrU}\\ \delta v_N+{\varphi }_U{v}_E+{\varphi }_U{v}_{wcE}+{\varphi }_U{v}_{wrE}-v_{wcN}-v_{wrN}-{\varphi }_E{v}_U-{\varphi }_E{v}_{wcU}-{\varphi }_E{v}_{wrU}\\ \delta v_U-{\varphi }_N{v}_E-{\varphi }_N{v}_{wcE}-{\varphi }_N{v}_{wrE}+{\varphi }_E{v}_N+{\varphi }_E{v}_{wcN}+{\varphi }_E{v}_{wrN}-v_{wcU}-v_{wrU}\end{array}\right]$$

(13)

$$\mathit{V}\left(t\right)={\left[V_{\delta L}{V}_{\delta \lambda }{V}_{\delta h}{V}_{vE}{V}_{vN}{V}_{vU}\right]}^{\mathrm{T}}$$

(14)

Thus, (5) and (12) provide the mathematical model for SINS/GNSS/ADS integrated navigation.

2.4. Wind Field-Based Constraint Model

In the most cases of airship flight, the vertical wind speed in the stratosphere is stable and close to a constant value (~20 km/s), and the airship does not change the altitude often. Thus, for simplicity, the vertical wind speed is neglected and it is also assumed that the altitude of the airship remains constant. Based on this, the airship state error equation can be written as [31]

$$\{\begin{array}{l}\delta \dot{x}=\delta v_a\cos {\alpha }_3+\delta v_{WE}=\delta v_e\cos {\alpha }_2\\ \delta \dot{y}=\delta v_a\sin {\alpha }_3+\delta v_{WN}=\delta v_e\sin {\alpha }_2\end{array}$$

(15)

where $\delta \dot{x}$ and $\delta \dot{y}$ are the airship velocity error on the x and y axes, respectively; $\delta v_a$ and $\delta v_e$ are the airship velocity errors relative to the atmosphere and Earth; $\delta v_{WE}$ and $\delta v_{WN}$ are the projections of the wind speed on the East and North directions, respectively, and $\omega$ is the angle rate.

By combining (15) with (4), the constraint equation can be expressed as

$$\mathit{D}\mathit{X}=\mathit{d}$$

(16)

where the state constraint matrix $\mathit{D}$ and the constraint vector quantity $\mathit{d}$ are expressed as

$$\mathit{D}=\left[\begin{array}{ccccccccc}0& 0& 0& \cos {\varphi }_U& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \sin {\varphi }_U& 0& 0& 0& 0\end{array}\right]$$

(17)

$$\mathit{d}={\left[\begin{array}{cc}\delta v_{WE}+\delta v_a\cos {\varphi }_U& \delta v_{WN}+\delta v_a\sin {\varphi }_U\end{array}\right]}^{\mathrm{T}}$$

(18)

where $\delta v_{WE}$ and $\delta v_{WN}$ are the wind speeds in the East and North directions, respectively.

3. Constrained Unscented Particle Filter

3.1. Conventional Unscented Particle Filter

Consider the state equation of a discrete system

$${\mathit{X}}_k=f\left({\mathit{X}}_{k-1}\right)+{\mathit{W}}_k$$

(19)

where ${\mathit{X}}_{k-1}$ denotes the state vector at epoch $k-1$, $f(\cdot )$ is a nonlinear function, and ${\mathit{W}}_k$ is the process noise.

The measurement equation of the discrete system is

$${\mathit{Z}}_k=h\left({\mathit{X}}_k\right)+{\mathit{V}}_k$$

(20)

where ${\mathit{Z}}_k$ denotes the measurement vector, $h(\cdot )$ is also a nonlinear function, and ${\mathit{V}}_k$ is the measurement noise.

The conventional UPF includes the following steps.

Step 1. Initialization: $k=0$

For $i=1,2,\cdots ,N$, draw the states ${\mathit{X}}_0^i$ from the prior $p\left({\mathit{X}}_0^i\right)$ and let

$${\overline{\mathit{X}}}_0^i=\mathrm{E}\left[{\mathit{X}}_0^i\right]$$

(21)

$${\mathit{P}}_0^i=\mathrm{E}\left[({\mathit{X}}_0^i-{\overline{\mathit{X}}}_0^i){({\mathit{X}}_0^i-{\overline{\mathit{X}}}_0^i)}^{\mathrm{T}}\right]$$

(22)

The sigma points can be selected as

$$\{\begin{array}{l}{\omega }_0^{(m)}=\frac{1}{n_a+\lambda }\\ {\omega }_0^{(c)}=\frac{1}{n_a+\lambda }+(1+{\alpha }^2+\beta )\\ {\omega }_i^{(m)}={\omega }_i^{(c)}=\frac{1}{2(n_a+\lambda )}i=1,2,\dots ,2N\end{array}$$

(23)

where ${\omega }_i^{(m)}$ and ${\omega }_i^{(c)}$ are the importance weights of the mean and covariance; $\alpha$ is a coefficient to control the distribution of sampling points; $\beta$ is a non-negative weighting coefficient for describing the prior distribution of X; $\lambda$ is a scaling parameter; and $n_a=n_X+n_W+n_V$ is the dimension of the augmented state, where $n_X$, $n_W$ and $n_V$ are the dimensions of state vector ${\mathit{X}}_k$, process noise ${\mathit{W}}_k$ and measurement noise ${\mathit{V}}_k$, respectively.

Step 2. For $k=1,2,\cdots$

(I)

Importance sampling

For $i=1,2,\cdots ,N$, update the particles with UKF:

(a)

Calculate the sigma points

$${\mathit{\chi }}_{k-1}^i=\left[{\overline{\mathit{X}}}_{k-1}^i{\overline{\mathit{X}}}_{k-1}^i+\sqrt{\left(n_a+\lambda \right){\mathit{P}}_{k-1}^i}{\overline{\mathit{X}}}_{k-1}^i-\sqrt{\left(n_a+\lambda \right){\mathit{P}}_{k-1}^i}\right]$$

(24)

(b)

Time update

$${\mathit{\chi }}_{k-1}^i=f\left({\mathit{\chi }}_{k-1}^{iX},{\mathit{\chi }}_{k-1}^{iW}\right)$$

(25)

$${\mathit{\chi }}_{k-1}^i=f\left({\mathit{\chi }}_{k-1}^{iX},{\mathit{\chi }}_{k-1}^{iW}\right)$$

(26)

$${\mathit{P}}_{k|k-1}^i={\displaystyle \sum _{j=0}^{2n_a}{w}_j^{\left(\mathit{c}\right)}\left[{\mathit{\chi }}_{j,k|k-1}^{iX}-{\overline{\mathit{X}}}_{k|k-1}^i\right]{\left[{\mathit{\chi }}_{j,k|k-1}^{iX}-{\overline{\mathit{X}}}_{k|k-1}^i\right]}^{\mathrm{T}}}$$

(27)

$${\mathit{Z}}_{k|k-1}^i=h\left({\mathit{\chi }}_{k|k-1}^{iX},{\mathit{\chi }}_{k-1}^{iV}\right)$$

(28)

$${\overline{\mathit{Z}}}_{k|k-1}^i={\displaystyle \sum _{j=0}^{2n_a}{w}_j^{(m)}{Z}_{j,k|k-1}^i}$$

(29)

(c)

Measurement update

$${\mathit{P}}_Z={\displaystyle \sum _{j=0}^{2n_a}{w}_j^{\left(\mathit{c}\right)}\left[{\mathit{Z}}_{j,k|k-1}^i-{\overline{\mathit{Z}}}_{k|k-1}^i\right]{\left[{\mathit{Z}}_{j,k|k-1}^i-{\overline{\mathit{Z}}}_{k|k-1}^i\right]}^{\mathrm{T}}}$$

(30)

$${\mathit{P}}_{XZ}={\displaystyle \sum _{j=0}^{2n_a}{w}_j^{\left(\mathit{c}\right)}\left[{\mathit{\chi }}_{j,k|k-1}^i-{\overline{\mathit{X}}}_{k|k-1}^i\right]{\left[{\mathit{Z}}_{j,k|k-1}^i-{\overline{\mathit{Z}}}_{k|k-1}^i\right]}^{\mathrm{T}}}$$

(31)

$${\mathit{K}}_k={\mathit{P}}_{XZ}{\mathit{P}}_Z^{-1}$$

(32)

$${\overline{\mathit{X}}}_k^i={\overline{\mathit{X}}}_{k|k-1}^i+{\mathit{K}}_k\left({\mathit{Z}}_k^{}-{\overline{\mathit{Z}}}_{k|k-1}^i\right)$$

(33)

$${\widehat{\mathit{P}}}_k^i={\mathit{P}}_{k|k-1}^i-{\mathit{K}}_k{\mathit{P}}_Z{\mathit{K}}_k^{\mathrm{T}}$$

(34)

The particles are sampled by ${\widehat{\mathit{X}}}_k^i\sim q\left({\mathit{X}}_k^i|{\mathit{X}}_{0:k-1}^i,{\mathit{Z}}_{1:k}^{}\right)=N\left({\overline{\mathit{X}}}_k^i,{\widehat{\mathit{P}}}_k^i\right)$. Subsequently, set ${\widehat{\mathit{X}}}_{0:k-1}^i\sim \left({\mathit{X}}_{0:k-1}^i,{\mathit{X}}_k^i\right)$ and ${\widehat{\mathit{P}}}_{0:k-1}^i\sim \left({\mathit{P}}_{0:k-1}^i,{\mathit{P}}_k^i\right)$, and normalize the importance weights.

$${\omega }_k^i={\omega }_{k-1}^i\frac{p\left({\mathit{Z}}_k|{\widehat{\mathit{X}}}_k^i\right)p\left({\widehat{\mathit{X}}}_k^i|{\mathit{X}}_{k-1}^i\right)}{q\left({\widehat{\mathit{X}}}_k^i|{\mathit{X}}_{0:k-1}^i,{\mathit{Z}}_{1:k}\right)}$$

(35)

$${\tilde{\omega }}_k^i={\omega }_k^i/\left({\displaystyle \sum _{j=0}^N{\omega }_k^j}\right)$$

(36)

(II)

Resampling

Ignore the samples ${\widehat{\mathit{X}}}_{0:k}^i$ with low importance weights. To obtain $N$ random samples ${\mathit{X}}_{0:k}^i$ approximately distributed according to $p\left({\widehat{\mathit{X}}}_{0:k}^i|{\mathit{Z}}_{1:k}^{}\right)$, we duplicate the particles having high weights and set ${\tilde{\omega }}_k^i={\omega }_k^i=N^{-1}$.

(III)

Output

$${\tilde{\mathit{X}}}_k^i={\displaystyle \sum _{i=1}^N{\omega }_k^i{\mathit{X}}_k^i}$$

(37)

$${\mathit{P}}_k^i={\displaystyle \sum _{i=1}^N{\omega }_k^i({\mathit{X}}_k^i-{\tilde{\mathit{X}}}_k^i){({\mathit{X}}_k^i-{\tilde{\mathit{X}}}_k^i)}^{\mathrm{T}}}$$

(38)

3.2. Convergence of Constrained UPF

Suppose system state $\mathit{X}$ is subject to the following constraint

$$\mathit{D}\mathit{X}=\mathit{d}$$

(39)

where $\mathit{D}$ denotes the state constraint matrix and $\mathit{d}$ the constraint vector quantity.

If the estimate ${\tilde{\mathit{X}}}_k^i$ of UPF is projected on the constraint surface, this minimum projection ${\tilde{\mathit{X}}}_k^i{}^{*}$ is given as (for clarity, we substitute ${\tilde{\mathit{X}}}_k^i$ and ${\tilde{\mathit{X}}}_k^i{}^{*}$ with $\tilde{\mathit{x}}$ and ${\tilde{\mathit{x}}}^{*}$, respectively)

$${\tilde{\mathit{x}}}^{*}=\min {\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)$$

(40)

such that

$$\mathit{D}{\tilde{\mathit{x}}}^{*}=\mathit{d}$$

(41)

To solve the above minimum problem, the Lagrange function is constructed as

$$L={\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)}^{\mathrm{T}}{\mathit{\Sigma }}^{-1}\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)+2{\lambda }^{\mathrm{T}}\left(\mathit{D}{\tilde{\mathit{x}}}^{*}-\mathit{d}\right)$$

(42)

From (42), we get

$${\mathit{\Sigma }}^{-1}\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)+{\mathit{D}}^{\mathrm{T}}\lambda =0$$

(43)

and

$$\mathit{D}{\tilde{\mathit{x}}}^{*}-\mathit{d}=0$$

(44)

According to (43), we obtain

$$\begin{array}{l}{\mathit{\Sigma }}^{-1}\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)+{\mathit{D}}^{\mathrm{T}}\lambda =0\\ {\mathit{\Sigma }}^{-1}\left({\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}\right)=-{\mathit{D}}^{\mathrm{T}}\lambda \\ {\tilde{\mathit{x}}}^{*}-\tilde{\mathit{x}}=-\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\lambda \end{array}$$

(45)

From (45), we readily have

$${\tilde{\mathit{x}}}^{*}={\mathit{D}}^{-1}\mathit{d}$$

(46)

Substituting (46) into (45) yields

$$\begin{array}{l}{\mathit{D}}^{-1}\mathit{d}-\tilde{\mathit{x}}=-\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\lambda \\ \mathit{d}-\mathit{D}\tilde{\mathit{x}}=-\mathit{D}\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\lambda \\ \mathit{D}\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\lambda =\mathit{D}\tilde{\mathit{x}}-\mathit{d}\\ \lambda ={\left(\mathit{D}\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\right)}^{-1}\left(\mathit{D}\tilde{\mathit{x}}-\mathit{d}\right)\end{array}$$

(47)

From (45) we also have

$${\tilde{\mathit{x}}}^{*}=\tilde{\mathit{x}}-\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\lambda$$

(48)

Substituting (47) into (48) yields

$${\tilde{\mathit{x}}}^{*}=\tilde{\mathit{x}}-\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}{\left(\mathit{D}\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\right)}^{-1}\left(\mathit{D}\tilde{\mathit{x}}-\mathit{d}\right)$$

(49)

Thus, the state estimate by the constrained UPF can be obtained as

$${\tilde{\mathit{X}}}_k^{i*}={\tilde{\mathit{X}}}_k^i-\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}{\left(\mathit{D}\mathit{\Sigma }{\mathit{D}}^{\mathrm{T}}\right)}^{-1}\left(\mathit{D}{\tilde{\mathit{X}}}_k^i-\mathit{d}\right)$$

(50)

where ${\tilde{\mathit{X}}}_k^i$ denotes the system state estimation from UPF.

From the above it can be seen that the state estimate given by (50) is actually an optimal solution under the constraint of the wind field model expressed by (16).

3.3. Convergence of Constrained UPF

Lemma 1.

For the nonlinear dynamic system described by (19) and (20), the relationship among nonlinear function$f$, prior distribution$p_{po}{}_{\left(k-1|k-1\right)}$, and posterior distribution$p_{pr}{}_{\left(k|k-1\right)}$of state$\mathit{X}$can be expressed as

$$\begin{array}{ll}\left(p_{po}{}_{\left(k|k-1\right)},f\right)& ={\displaystyle \int p_{po}{}_{\left(k-1|k-1\right)}{p}_{pr}{}_{\left(k|k-1\right)}f}dx\\ & =\left(p_{po}{}_{\left(k-1|k-1\right)},p_{pr}{}_{\left(k|k-1\right)}f\right)\end{array}$$

(51)

The proof of Lemma 1 can be found in Appendix A.

Lemma 2.

Assume that for any function$f$and constant$c$,

$$\mathrm{E}\left[{\left(\left(p_{po}{}_{\left(k-1|k-1\right)}^N,f\right)-\left(p_{po}{}_{\left(k-1|k-1\right)}^{},f\right)\right)}^2\right]\le c\frac{{\Vert f\Vert }^2}{N}$$

(52)

Then, we readily have

$$\mathrm{E}\left[{\left(\left(p_{po}{}_{\left(k|k-1\right)}^N,f\right)-\left(p_{po}{}_{\left(k|k-1\right)}^{},f\right)\right)}^2\right]\le c\frac{{\Vert f\Vert }^2}{N}$$

(53)

The proof of Lemma 2 can be found in Appendix B.

Lemma 3.

Given the system equation defined by (19), the relationship among nonlinear function$f$, prior distribution$p_{po}{}_{\left(k|k-1\right)}$, and posterior distribution$p_{po}{}_{\left(k|k\right)}$of state$\mathit{X}$can also be expressed as

$$\left(p_{po}{}_{\left(k|k\right)},f\right)=\frac{\left(p_{po}{}_{\left(k|k-1\right)},hf\right)}{\left(p_{po}{}_{\left(k|k-1\right)},h\right)}$$

(54)

The proof of Lemma 3 can be found in Appendix C.

Theorem 1.

Given the system equation defined by (19), for all$k>0$, there exists a constant$c_1$, which is independent of$N$, such that for any$f$,

$$\mathrm{E}\left[{\left(\left(p_{po\left(k|k\right)}^N,f\right)-\left(p_{po}{}_{\left(k|k\right)}^{},f\right)\right)}^2\right]\le c_1\frac{{\Vert f\Vert }^2}{N}$$

(55)

The proof of Theorem 1 can be found in Appendix D.

4. Simulations and Analysis

Simulations were conducted to evaluate the performance of the proposed constrained UPF algorithm for a SINS/GNSS/ADS integrated airship navigation system. Comparison analysis with the extended Kalman filter (EKF) and the robust adaptive UPF (RAUPF) [29,30] was also conducted to demonstrate the improved performance of the proposed algorithm.

Suppose the airship with a SINS/GNSS/ADS integrated navigation system is flying in the stratosphere. The airship moves to the East at a speed of 20 m/s, and the initial position of the airship is at East longitude 108.9°, North latitude 34.2°, and altitude 20 km. The flight time is 800 s. The Earth’s rotation angular velocity is ${\omega }_{ie}=15{ }^{\mathrm{o}}/\mathrm{h}$, the radius of the earth is $R_e=6378 \mathrm{km}$, and the acceleration of gravity is $\mathrm{g}=9.780\mathrm{m}/{\mathrm{s}}^2$. The sampling periods of SINS, GNSS, and ADS are 0.01 s, 0.2 s, and 1 s, respectively. The filtering period is 1s. The initial position error is ($\delta \lambda =0^{\prime },\delta L=0^{\prime },\delta h=0 \mathrm{m}$) and the velocity error is $\delta v_E=\delta v_N=\delta v_U=0.1$ m/s. The gyro’s random drift and walk are 0.1°/h and 0.01°/$\sqrt{\mathrm{h}}$, respectively. The accelerometer’s zero offset and random walk are ${10}^{-3}\mathrm{g}$ and $5\times {10}^{-4}\mathrm{g}/\sqrt{\mathrm{s}}$, respectively. The horizontal and altitude positioning errors of GNSS are 0.6 m and 2 m, the barometric altimeter error is 20 m, and the velocity error is 2 m/s. The airspace above sea level within the altitude of 20 km is mainly dominated by west wind with a speed below 20 m/s. The gust wind in the airspace is in the North by East 10° with the speed of 0–2 m/s.

To study the performance of the proposed constrained UPF, trials were conducted under different average wind speeds of 10 m/s, 15 m/s, and 20 m/s, respectively. The initial state variance, system noise covariance and observation noise covariance are set as

$$\begin{array}{l}{\mathit{P}}_0=diag\{{\left({0.1}^{\prime }\right)}^2,{\left({0.1}^{\prime }\right)}^2,{\left({0.1}^{\prime }\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0\mathrm{m}\right)}^2,{\left(0\mathrm{m}\right)}^2,{\left(0\mathrm{m}\right)}^2,\\ {\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(2\mathrm{m}\right)}^2,\\ {\left(9.8\mathrm{m}/\mathrm{s}\right)}^2,{\left(1.7\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(1.9\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.3\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2\}\\ {\mathit{P}}_0=diag\{{\left({0.1}^{\prime }\right)}^2,{\left({0.1}^{\prime }\right)}^2,{\left({0.1}^{\prime }\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(0\mathrm{m}\right)}^2,{\left(0\mathrm{m}\right)}^2,{\left(0\mathrm{m}\right)}^2,\\ {\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(2\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(2\mathrm{m}\right)}^2,\\ {\left(9.8\mathrm{m}/\mathrm{s}\right)}^2,{\left(1.7\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2,{\left(1.9\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.3\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2\}\\ {\mathit{R}}_1=diag\left\{{\left(0.6\mathrm{m}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(2\mathrm{m}\right)}^2,{\left(9.8\mathrm{m}/\mathrm{s}\right)}^2,{\left(1.7\mathrm{m}/\mathrm{s}\right)}^2{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2\right\}\\ {\mathit{R}}_2=diag\left\{{\left(0.6\mathrm{m}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(2\mathrm{m}\right)}^2,{\left(14.8\mathrm{m}/\mathrm{s}\right)}^2,{\left(2.6\mathrm{m}/\mathrm{s}\right)}^2{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2\right\}\\ {\mathit{R}}_3=diag\left\{{\left(0.6\mathrm{m}\right)}^2,{\left(0.6\mathrm{m}\right)}^2,{\left(2\mathrm{m}\right)}^2,{\left(19.7\mathrm{m}/\mathrm{s}\right)}^2,{\left(3.5\mathrm{m}/\mathrm{s}\right)}^2{\left(0.1\mathrm{m}/\mathrm{s}\right)}^2\right\}\end{array}$$

(56)

where ${\mathit{R}}_i(i=1,2,3)$ stands for the observation noise covariances for the cases of different average wind speeds.

The estimated velocity and position errors under different wind speeds are shown in Figure 3, Figure 4, Figure 5, Figure 6, Figure 7 and Figure 8, and their corresponding mean values are listed in Table 1, Table 2 and Table 3. In the case of 10 m/s constant wind, during the time period from 0 s to 600 s, the velocity and position errors obtained by EKF beyond ±2 m/s and ±20 m, respectively. This is because the linearization of system model causes a large navigation error. RAUPF improves the performance of EKF. The velocity and position errors obtained by RAUPF are within (−0.7 m/s, +0.7 m/s) and (−7 m, +7 m). They are also much larger than those errors by the constrained UPF, which are within (−0.3 m/s, +0.3 m/s) and (−4 m, +4 m). Considering that the performance of EKF is too poor for the wind field disturbance, in the following we shall focus on the comparison of RAUPF with the proposed constrained UPF for the cases of 15 m/s and 20 m/s wind speeds. The similar trend can also be observed for these two cases. In the case of 15m/s constant wind, the velocity and position errors obtained by RAUPF are within (−1.1 m/s, +1.1 m/s) and (−8 m, +8 m), while those by the constrained UPF are within (−0.6 m/s, +0.6 m/s) and (−5.5 m, +5.5 m). In the case of 20 m/s constant wind, the velocity and position errors obtained by RAUPF are within (−1.6m/s, +1.6 m/s) and (−9.5 m, +9.5 m), while those by the constrained UPF are within (−0.7 m/s, +0.7 m/s) and (−6 m, +6 m). Therefore, it is evident that the velocity and position errors obtained by the proposed constrained UPF are much smaller than those by RAUPF.

Figure 3. East velocity and longitude errors under the wind speed of 10 m/s.

Figure 4. North velocity and latitude errors under the wind speed of 10 m/s.

Figure 5. East velocity and longitude errors under the wind speed of 15 m/s.

Figure 6. North velocity and latitude errors under the wind speed of 15 m/s.

Figure 7. East velocity and longitude errors under the wind speed of 20 m/s.

Figure 8. North velocity and latitude errors under the wind speed of 20 m/s.

Table 1. Mean errors of velocity and position under the wind speed of 10 m/s.

Filtering Methods	East Velocity Error (m/s)	North Velocity Error (m/s)	Longitude Error (m)	Latitude Error (m)
EKF	0.8532	0.7955	8.2235	8.3465
RAUPF	0.5679	0.3324	4.6657	4.7968
Constrained UPF	0.2123	0.2198	2.8123	2.9456

Table 2. Mean errors of velocity and position under the wind speed of 15 m/s.

Filtering Methods	East Velocity Error (m/s)	North Velocity Error (m/s)	Longitude Error (m)	Latitude Error (m)
RAUPF	0.8136	0.6180	5.4120	5.5852
Constrained UPF	0.3058	0.4767	3.8606	3.8769

Table 3. Mean errors of velocity and position under the wind speed of 20 m/s.

Filtering Methods	East Velocity Error (m/s)	North Velocity Error (m/s)	Longitude Error (m)	Latitude Error (m)
RAUPF	1.1127	1.0092	6.8033	6.4456
Constrained UPF	0.5269	0.4388	4.5319	4.1869

It is also observed that the latitude and North velocity errors are smaller than the longitude and East velocity for RAUPF. The reason is that the gust wind is in the North by East 10° and thus its East velocity component is larger than its North velocity component, more greatly affecting the East velocity and longitude of the airship. However, the constrained UPF does not suffer from such an effect caused by the wind field disturbance. The North velocity and latitude are in the similar accuracy as the East velocity and longitude, without any obvious disturbance by the wind field. This demonstrates that the proposed constrained UPF is able to resist the disturbance of wind field.

In addition, only a slight change in the accuracy of the constrained UPF was observed due to the increase of wind speed. This means that the ability of the constrained UPF to suppress wind field disturbance becomes stronger with the increase of the wind speed.

Figure 9 and Figure 10 show the fitting curves for the horizontal velocity and position errors of the airship by both RAUPF and constrained UPF. It can be seen that the horizontal velocity and position errors obtained by RAUPF are linearly increased with the increase of the wind speed. This demonstrates that RAUPF lacks the capability to resist the wind disturbance. In contrast, the slopes of the fitting curves of the horizontal velocity and position errors by the constrained UPF are decreased, demonstrating that the larger the wind speed is the stronger UPF’s resistance to the wind disturbance.

Figure 9. Horizontal velocity errors for both RAUPF and constrained UPF.

Figure 10. Horizontal position errors for both RAUPF and constrained UPF.

The above simulation results demonstrate that the proposed constrained UPF can effectively inhibit the disturbance of wind field, leading to improved positioning accuracy for SINS/GNSS/ADS integrated airship navigation in comparison with RAUPF.

5. Conclusions

This paper presents a new constrained UPF for SINS/GNSS/ADS integration to improve the performance of airship positioning and navigation under the disturbance of wind field. The contributions of this paper are that (i) the wind speed model and navigation mathematical model are established for SINS/GNSS/ADS integration; and (ii) a constrained UPF is developed using the wind speed model as a constraint to fuse SINS and GNSS measurements to generate system state estimation for airship navigation based on SINS/GNSS/ADS integration, leading to the optimal state estimation in the presence of wind disturbance. Simulations and comparison analysis verify that the proposed constrained UPF can effectively inhibit the influence of wind field, leading to the improved accuracy comparing to EKF and ARUPF for SINS/GNSS/ADS integrated airship navigation in the presence of wind disturbance.

Future research work will focus on two aspects. One is the experimental evaluation of the proposed constrained UPF. Practical experiments on airship flight based on SINS/GNSS/ADS integrated navigation will be conducted to further evaluate the performance of the proposed algorithm. The other is on improvement of the proposed constrained UPF. The proposed algorithm will be combined with advanced artificial intelligence technologies such as pattern recognition, neural network, and advanced expert systems, thus establishing an intelligent algorithm to automatically deal with the disturbances of wind field for the airship navigation.