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

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.


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.

Wind Speed Model
In the E-N-U (East-North-Up) geography coordinate system, the wind speed model can be expressed as where, v W is the wind speed, v Wc is a random constant wind, and v Wr is a random wind expressed as the first-order Markov process, and where v Wi (i = E, N, U) stands for the projection of wind speed 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 is the white noise in the corresponding direction.
, , ) stands for the projection of wind speed W v in the East, North, and Up directions, as shown in Figure 1; the symbols Wci v and Wri v have the similar meanings as Wi v ; and 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 where e v and a v denote the airship velocities relative to the Earth and atmosphere, 1 α is the angle between the horizontal axis and wind speed W v , and 2 α and 3 α are the angles from e v and a v to the horizontal axis, respectively.

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 As shown in Figure 2, via the wind speed, the airship velocity relative to the Earth can be expressed as where v e and v a denote the airship velocities relative to the Earth and atmosphere, α 1 is the angle between the horizontal axis and wind speed v W , and α 2 and α 3 are the angles from v e and v a to the horizontal axis, respectively.  As shown in Figure 2, via the wind speed, the airship velocity relative to the Earth can be expressed as where e v and a v denote the airship velocities relative to the Earth and atmosphere, 1 α is the angle between the horizontal axis and wind speed W v , and 2 α and 3 α are the angles from e v and a v to the horizontal axis, respectively.

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

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 X(t) of SINS/GNSS/ADS is defined as where φ E , φ N and φ U represent the platform misalignment angle of SINS; δv SE , δv SN and δv SU the velocity error of SINS; δλ S , δL S and δh S the position error of SINS; δv GE , δv GN and δv GU the velocity error of GNSS; δλ G , δL G and δh G the position error of GNSS; δv WcE , δv WcN and δv WcU the constant wind speed error; and δv WrE , δv WrN and δv WrU the random wind speed error.
The system state equation is .
where f (X(t)) is a nonlinear function of the state and is expressed by where C ω is the Euler platform error angle matrix; C c n and C c b are the attitude transformation matrices; δg is the errors of gravity; δω b ib is the measurement error of the gyro; ω n ie is the rotational angular velocity of the Earth; ω n en is the angular velocity of the vehicle relative to the Earth; ω n ie is the rotational angular velocity of the Earth;ω n ie ,ω n en andω n in are the actual values of ω n ie , ω n en and ω n in in the actual navigation frame; δω n ie , δω n en and δω n in represent the corresponding errors; δv i represents the velocity error in the corresponding direction; L and h are the longitude and height of the airship; δL and δh are the errors of L and h, respectively; τ i (i = E, N, U) is the relevant time;f b s f and δf b s f are the specific force and its associated error, respectively; R M and R N are the meridian and prime vertical radiuses of curvature; and δR M and δR N are the errors of R M and R N , respectively.
The system noise vector W(t) is described as where W i , i = 1, 2, · · · , 21 are the random noise of the state.

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 where L S , λ S and h S are the latitude, longitude and altitude of SINS, respectively; L G , λ G and h G are the latitude, longitude and altitude of GNSS, respectively; and V δL , V δλ and V δ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
where v EI , v N I and v U I are the velocities of SINS and v EA , v N A and v U A 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 Thus, (5) and (12) provide the mathematical model for SINS/GNSS/ADS integrated navigation.

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] δ .
x and δ . y are the airship velocity error on the x and y axes, respectively; δv a and δv e are the airship velocity errors relative to the atmosphere and Earth; δv WE and δv W N are the projections of the wind speed on the East and North directions, respectively, and ω is the angle rate. By combining (15) with (4), the constraint equation can be expressed as where the state constraint matrix D and the constraint vector quantity d are expressed as where δv WE and δv W N are the wind speeds in the East and North directions, respectively.

Conventional Unscented Particle Filter
Consider the state equation of a discrete system where X k−1 denotes the state vector at epoch k − 1, f (·) is a nonlinear function, and W k is the process noise. The measurement equation of the discrete system is where Z k denotes the measurement vector, h(·) is also a nonlinear function, and V k is the measurement noise. The conventional UPF includes the following steps.
Step 1. Initialization: k = 0 For i = 1, 2, · · · , N, draw the states X i 0 from the prior p X i 0 and let The sigma points can be selected as where ω (m) i and ω (c) i are the importance weights of the mean and covariance; α is a coefficient to control the distribution of sampling points; β is a non-negative weighting coefficient for describing the prior distribution of X; λ 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 X k , process noise W k and measurement noise V k , respectively. Step 2. For k = 1, 2, · · · (I) Importance sampling For i = 1, 2, · · · , N, update the particles with UKF: Calculate the sigma points (b) Time update (c) Measurement update The particles are sampled byX and normalize the importance weights.
(II) Resampling Ignore the samplesX i 0:k with low importance weights. To obtain N random samples X i 0:k approximately distributed according to p X i 0:k Z 1:k , we duplicate the particles having high weights and set ω i k = ω i k = N −1 . (III) Output

Convergence of Constrained UPF
Suppose system state X is subject to the following constraint where D denotes the state constraint matrix and d the constraint vector quantity.
If the estimate X i k of UPF is projected on the constraint surface, this minimum projection X i k * is given as (for clarity, we substitute X i k and X i k * with x and x * , respectively) To solve the above minimum problem, the Lagrange function is constructed as From (42), we get and According to (43), we obtain From (45), we readily have Substituting (46) into (45) yields From (45) we also have Substituting (47) into (48) yields Thus, the state estimate by the constrained UPF can be obtained as where X i k 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).

Lemma 1.
For the nonlinear dynamic system described by (19) and (20), the relationship among nonlinear function f , prior distribution p po (k−1|k−1) , and posterior distribution p pr (k|k−1) of state X can be expressed as The proof of Lemma 1 can be found in Appendix A.

Lemma 2. Assume that for any function f and constant c,
Then, we readily have 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 (k|k−1) , and posterior distribution p po (k|k) of state X can also be expressed as 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 , The proof of Theorem 1 can be found in Appendix D.

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.                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. Figures 9 and 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.
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.

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.

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.