# A Novel Two-Stage Adaptive Filtering Model-Based GNSS/INS Tightly Coupled Precise Relative Navigation Algorithm for Autonomous Aerial Refueling

Authors: Ling Wu; Yongrong Sun; Xiyu Fu; Rui Xu
Journal: International Journal of Aerospace Engineering
 (2022)

---

## Abstract

Autonomous aerial refueling (AAR) has generated great interest in recent years. However, much research has focused on the vision-based close docking stage; few studies have been conducted on the navigation algorithm for the rendezvous and following stages. High-precision relative navigation in following stage can provide favourable conditions for successful docking. Aiming at precise relative navigation in the complex high dynamic environment of aerial refueling rendezvous and following stages, a two-stage adaptive filtering architecture is exploited in this paper. An adaptive main Kalman filter (AKF) is realized for ambiguity eliminated GNSS/INS tightly coupled integrated system, and a robust adaptive subfilter is developed for GNSS individually. Particularly, aiming at the influence of pseudorange observation multipath outliers and state abnormal disturbances in unmanned air vehicle- (UAV-) tanker proximity, an INS-aided bifactor robust and classified factor adaptive filtering (IBRCAF) algorithm for single-frequency ambiguity resolution is proposed. Finally, the effectiveness of the algorithm is verified by the simulation experiments for UAV-tanker. The results indicate that the IBRCAF algorithm can efficiently suppress the influence of pseudorange multipath gross errors and abnormal state disturbances and greatly raise the success rate of ambiguity resolution, and the two-stage adaptive filtering algorithm of IBRCAF-AKF can significantly improve relative navigation performance and achieve centimeter-level accuracy.


---

## Body

## 1. Introduction

The autonomous aerial refueling (AAR) has played an important part in the military field, which is considered a “technical challenge” by the US Air Force [1]. Compared with manned aerial refueling technology, which has a higher risk factor and greater difficulty in operation, the unmanned air vehicle (UAV) technology has greatly reduced operational costs and risks [2]. There are mainly two methods in AAR: the boom-and-receptacle refueling system and the probe-and-drogue refueling system. The operation procedure is different from each other. In the boom-and-receptacle refueling method, the tanker is an active part that steers a rigid retractable boom fitted to the rear of the tanker to a socket installed on the top of UAV; while in the probe-and-drogue refueling method, the tanker drags a refueling drogue linked in the end of the flexible hose, and then, the refueling probe attached on the UAV is controlled to dock into the drogue. The characteristics of the former method are that the speed of oil delivery is fast and it is not sensitive to air turbulence, but the structure of refueling device is complex and it only fuels one aircraft at a time [3]. Compared to the boom-based refueling method, the structure of the drogue-based refueling method is simpler and more flexible, which can refuel two or more aircraft at the same time, but the drogue is susceptible to atmospheric turbulence [4].The AAR maneuver process is commonly divided into five stages: rendezvous phase, following phase, docking phase, refueling phase, and separating phase. In the rendezvous and following stage, the UAV flies from several kilometers far away with maneuver flight to following the tanker in ten-meter distance with level flight; then, the UAV approaches the tanker from below and behind and gets docked with the tanker or the drogue; during the refueling phase, the fuel oil is pumped from the tanker into the UAV, and the UAV should try to keep a stationary position and velocity with the tanker; finally, the separating phase begins as soon as fuel oil transfer completes, and the UAV decelerates and becomes detached from the tanker. Since there is no direct operation of pilots for UAV, a key role in successful refueling is to estimate the position of the UAV relative to the tanker very accurately and in real time. In addition, to ensure safety and operational usefulness, the navigation strategy must provide high level of integrity. Particularly, along with UAV-tanker proximity, dynamic mobility and safety demand that the relative navigation system meets stringent requirements on accuracy, reliability, and continuity [5–7].To meet the requirements in AAR, several instruments and methods for relative navigation have been pursued. Most of the previous research in AAR focused on active or passive visual sensors in close docking stage, which are mainly interested in two aspects. One is the detection and tracking of the refueling drogue by the near circular feature of the inner refueling port [8, 9]; the other is the relative pose measurement between the UAV and refueling drogue in close range [10, 11]. The active vision system introduces obvious features, such as installation of special light sources or marking points on the drogue to assist visual navigation [12], while the passive vision system makes no changes to the drogue, only relying on the characteristics of itself for detection and positioning [13]. In order to improve the reliability and robustness for AAR, relative navigation architecture based on a multisensor combination system, such as inertial navigation system (INS), vision-based navigation (VisNav), global position system (GPS), and infrared search and track system (IRST), is studied [14–16], which is mainly concentrated on vision-based navigation in close range or relative inertial navigation algorithm with low accuracy. Few studies have been done for the precise relative navigation algorithm in the rendezvous and following phases, which is essential for AAR as well.Generally speaking, there are global navigation satellite system (GNSS) and INS sensors installed onboard the UAV and the tanker [17]. So, there are no weight or cost penalties in expanding their use. However, traditional high precise relative positioning (RP) of real-time kinematic (RTK) based on carrier phase observation is limited by the fixed position of the reference station and limited coverage, which is difficult to meet the requirement for AAR. Hence, the RP method based on the moving reference station (also called kinematic-to-kinematic relative positioning, KKRP) is used to obtain precise RP results [18, 19]. In addition, one of the key technologies based on differential carrier phase is the reliable solution of integer ambiguity. The speed and quality of ambiguity resolution (AR) directly affect the accuracy of positioning [20]. There are usually four steps for AR: least square (LS) or extended Kalman filtering (EKF) is always utilized to estimate the ambiguity floats firstly. Then, the least-square ambiguity decorrelation adjustment (LAMBDA) method is used to search and verify the integer ambiguity with corresponding covariance matrix. Thirdly, an integer ambiguity reliability test is performed. Finally, the exact position solution is obtained with “best” ambiguity [21]. Among them, improving the accuracy of ambiguity floats with additional constraints can increase the success rate of AR significantly. Baseline length constraint approaches for enhancing GNSS ambiguity resolution are studied in [22]. Multivariate constraints for improving ambiguity resolution success rate of GNSS-based attitude determination and relative positioning are developed in [23]. A sequential ambiguity resolution method with a float solution substitution and double-different iterative correction is proposed in [24]. Since the GNSS/INS tightly coupled (TC) integrated system can effectively improve the performance of navigation and positioning results, therefore, the short-term output high-precision position information of INS can provide prior coordinate information which is used to assist the rapid solution of GNSS dynamic ambiguity [25]. The improvement in integer ambiguity resolution with INS aiding for kinematic precise point positioning is revealed by sufficient theoretical analysis and performance assessment in [26]. A new inertial-aided ambiguity resolution method that directly rounds the float ambiguity of the BeiDou triple-frequency-combined observations is proposed in [27]. A MEMS-IMU-assisted BeiDou triple-frequency ambiguity resolution method in poor satellite geometry and severe multipath environments is studied in [28]. However, the fixed efficiency of GNSS ambiguity is also easily affected by observation gross errors and state predicted outliers, which directly affect the optimal value of parameter estimation and further influence the positioning performance [29]. In the rendezvous and following stages of AAR, the UAV flies more difficult than normal flight due to the tanker wake turbulence. Particularly, in the UAV-tanker proximity, the GNSS signal is occluded partially by the tanker, the multipath effect is enhanced by body reflection, and the state disturbance appears by accidental airflow. Thus, the relative navigation performance is reduced due to low success rate and poor reliability of ambiguity resolution in a complex high dynamic environment [7, 30]. Therefore, a practical relative navigation algorithm is important for the rendezvous and following stages of AAR.In this contribution, an economical and practical precise relative navigation scheme is proposed. Conventional tightly coupled with pseudorange and Doppler observations is realized on the tanker as moving reference; a two-stage hierarchical adaptive filtering architecture is operated on the UAV as rover station. Thus, carrier phase-based kinematic-to-kinematic precise relative navigation is developed. Particularly, in order to solve the key issue of ambiguity resolution with single-frequency GNSS under the influence of pseudorange multipath gross errors and model state abnormal disturbances in the UAV-tanker proximity, an INS-aided bifactor robust and classified factor adaptive filtering algorithm based on adaptive Kalman filtering of tightly coupled with eliminated ambiguity parameter is proposed.The paper is organized as follows. In Section2, the algorithm models are designed. Firstly, the algorithm overview is illustrated. Then, the carrier phase-based KKRP model is presented. Afterwards, the GNSS/INS tightly coupled integrated system is introduced and an INS-aided bifactor robust and classified factor adaptive filtering algorithm for single-frequency AR is elaborated. In Section 3, the algorithm is tested by simulation experiments and results are discussed in detail. Finally, Section 4 presents our conclusions and future work.


## 2. The Design of Algorithm Models

### 2.1. Algorithm Overview

As shown in Figure1, the relative position for AAR is the typical KKRP problem, in which the tanker is the moving reference station, and the UAV is the rover station. The raw GNSS observations of tanker including code pseudorange, carrier phase, and Doppler as well as position and velocity calculated by conventional TC with pseudorange and Doppler measurements (due to the limited pages, the conventional TC algorithm is not covered in this article.) are transmitted to the UAV by data link. Then, the observations are double-difference (DD) processed in order to reduce the time and space errors except for the multipath error and observation noise. Next, the instantaneous high-precise INS information is used to assist the DD single-frequency single epoch AR in the subfilter, which is realized by a bifactor robust and classified factor adaptive filtering algorithm (see Section 2.4). Afterwards, the GNSS/INS tightly coupled integrated system by adaptive Kalman filtering in main-filter is conducted with known ambiguity parameters (see Section 2.3). Meanwhile, inertial errors are feedback corrected by filtering estimation parameters, and the position, velocity, and attitude of UAV are output. Finally, the carrier phase-based kinematic-to-kinematic precise relative position is realized with known ambiguity (see Section 2.2).Figure 1
The algorithm roadmap of the two-stage adaptive filtering architecture for precise KKRP in AAR.


### 2.2. The Carrier Phase-Based KKRP Model

In the GNSS-based relative position, the code pseudorange, carrier phase, and Doppler observations are used to consist DD observation equations as follows:(1)Δ∇Purij=Δ∇ρurij+Δ∇Turij+Δ∇Iurij+εΔ∇P,urij,Δ∇Φurij=Δ∇ρurij+Δ∇Turij−Δ∇Iurij+λΔ∇N+εΔ∇Φ,urij,−λΔ∇Durij=Δ∇ρ̇urij+Δ∇Ṫurij+Δ∇İurij+εΔ∇D,urij,where Δ∇ denotes the symbol of DD operator; subscripts u and r represent the moving reference and rover station; superscripts i and j are the observed satellites consisting of DD; P, Φ, and D are code pseudorange, carrier phase (in distance), and Doppler observations, respectively. ρ and ρ̇ are the geometric distance and its changing rate between station and satellite. T, I, and Ṫ, İ represent tropospheric delay, ionospheric delay, and their changing rates. λ and N represent the carrier wavelength and initial integer ambiguity; εP, εΦ, and εD are the residual errors including multipath and measurement noises of code pseudorange, carrier phase, and Doppler observations. For a short baseline, the ionospheric delay and tropospheric delay in DD are too small to be neglected.Linearing equation (1) with baseline vector and its changing rate as unknown parameter, thus, it can be written as
(2)Δ∇Purij=−Ιri−Ιrj⋅bur+εΔ∇P,urij,Δ∇Φurij=−Ιri−Ιrj⋅bur+λ⋅Δ∇Nurij+εΔ∇Φ,urij,−λ⋅Δ∇Durij=−Ιri−Ιrj⋅ḃur+εΔ∇D,urij,where bur and ḃur are the baseline vector and velocity vector; Ιri is the unit direction vector between satellite and rover calculated by Iri=ri−rr/ri−rr.If the DD integer ambiguityΔ∇Nurij can be solved on fly accurately, the high-precision baseline vector and velocity vector can be determined by carrier phase and Doppler observations [31].


### 2.3. GNSS/INS Tightly Coupled Model

For the GNSS/INS tightly coupled navigation system, the system model is derived from INS instruments, and the observation model is composed of the differences between GNSS DD observations and INS-derived counterparts. The integrated system is implemented by adaptive Kalman filtering with feedback correction to INS sensor biases.


#### 2.3.1. System Model

The system state-space model depends on the INS error model and system error description of inertial sensors. The integrated system model can be described as the following psi-angle error equations [32] without considering satellite system errors:
(3)δṙ=−ωenn×δr+δv,δv̇=−2ωien+ωenn×δv−ψ×fn+Cbn∇b,ψ̇=−ωien+ωenn×ψ−Cbnεb,where the subscripts i, e, n, and b represent the inertial, Earth, navigation (east-north-up), and body frame, respectively; δr, δv, and ψ represent the position, velocity, and attitude angle error vectors, respectively; fn is the specific force vector; ωien is the Earth rotation rate with respect to the inertial frame; ωenn is the rotation rate of the navigation frame with respect to Earth. Cbn is the transformation matrix from body frame to navigation frame. ∇b represents the accelerometer error vector; εb represents the gyro drift error vector.Accurate models of accelerometer and gyro errors are important to improve the navigation performance, which are augmented into the filtering states for real-time parameter estimation. In this paper, the accelerometer error is modeled as first-order Gauss-Markov process; gyro drift error is modeled as random constant and first-order Gauss-Markov process [33] as follows.
(4)∇̇a=−1Ta∇a+ωa,ε=εb+εr+ωg,(5)ε̇r=−1Tgεr+ωr,where εb is the random constant of gyro error with ε̇b=0; Ta and Tg are the correlation time of the random process of accelerometer and gyro, respectively; ωa, ωg, and ωr are the driving white noise.Therefore, the system state equation can be expressed as follows:(6)Ẋt=FtXt+GtWt,where F is the state coefficient matrix; G is the noise coefficient matrix. W is the noise vector. X is the state vector, which includes 9 navigation error states and 9 inertial sensor bias errors, written as
(7)X=δrEδrNδrUδvEδvNδvUφEφNφUεbxεbyεbzεrxεryεrz∇ax∇ay∇azT.


#### 2.3.2. Measurement Model

The measurement model describes the relationship between the measurements and the unknown parameters. The differences between GNSS DD code pseudorange, carrier phase and Doppler observations, and INS predicted satellite-to-ground distance and relative velocity consist the measurements. Therefore, the measurement equations are written as follows:(8)∇ΔP−∇ΔPINS=Aa⋅Cne⋅δr+ε∇ΔP,∇ΔΦ−∇ΔPINS−λ⋅∇ΔN=Aa⋅Cne⋅δr+ε∇ΔΦ,−λ⋅∇ΔD−∇ΔvINS=Aa⋅Cne⋅δv+ε∇ΔD,where PINS is the INS predicted satellite-to-ground distance calculated by PINS=xINS−xs2+yINS−ys2+zINS−zs2; vINS is the relative velocity between rover station and satellite calculated by vINS=ẋINS−ẋs2+ẏINS−ẏs2+żINS−żs2; Aa is the design matrix calculated by −IINSi−IINSj; Cne is the transformation matrix from navigation frame to Earth frame. Write the measurement equations into matrix form as follows:
(9)Zt=HtXt+Vt,where Z is the measurement vector; H is the measurement model coefficient matrix; V is the measurement noise matrix.


#### 2.3.3. Adaptive Kalman Filtering Model

Since the integrated navigation performance of GNSS/INS is always affected by the abnormal predicted states such as UVA maneuvering or wrong ambiguity fixed values in GNSS, thus, the adaptive Kalman filtering algorithm is used to improve the reliability of the TC integrated system. The estimation of state parameters can be expressed as(10)X^k,k=X^k,k−1+K̄kZk−HkX^k,k−1,(11)K̄k=1αkPk,k−1HkTHk1αkPk,k−1HkT+Rk−1,where X^k,k−1 and Pk,k−1 are the predicted state vector and its covariance matrix. Rk is the measurement covariance matrix. K̄k is the adaptive filtering gain matrix, and αk is the adaptive factor. The adaptive factor is constructed by Huber weight function [34] as follows:
(12)αk=1ΔM~k≤k0,k0ΔM~kΔM~k>k0,where k0 is the threshold constant, which is valued at 0.85~3.0 [35]. ΔM~k is the test statistic, which is calculated as
(13)ΔMk=VkTVktrQVk,where Vk is the innovation vector calculated by Vk=HkX^k,k−1−Zk and QVk is its covariance matrix calculated by QVk=HkPk,k−1HkT+Rk.


### 2.4. INS-Aided Bifactor Robust and Classified Factor Adaptive Filtering AR Model

As mentioned in Section2.1, accurate ambiguity parameters are the premise of high precision positioning for KKRP. However, it is difficult to resolve with a single-frequency carrier phase, especially in complex high dynamic environments. Since INS can provide instantaneous high precision and continuous positioning ability with the GNSS/INS TC integrated system, the accuracy of ambiguity floats solved by INS-aided can be improved, which can constrain the ambiguity search space and improve the efficiency of AR.


#### 2.4.1. Single-Epoch INS-Aided GNSS AR Filtering Model

The system model is constructed by GNSS only with a constant acceleration (CA) kinematic model, and the carrier phase ambiguity parameters are modeled as a random walk process. The state vector is defined by(14)X=δx,δy,δz,δvx,δvy,δvz,δax,δay,δaz,Δ∇N1,Δ∇N2,⋯,Δ∇NmT,where δx,δy,δz,δvx,δvy,δvz,δax,δay,andδaz are the components of the position, velocity, and acceleration increment, respectively. Δ∇Nii=1,2,⋯,m represent the DD carrier phase ambiguities.In the INS-aided AR process, the DD pseudorange and carrier phase observation equations are linearized at the INS-derived position. Moreover, the INS-derived position is used to construct additional coordinate constraint by a virtual observation. Therefore, the observation equations can be written as [36]
(15)Δ∇P−Δ∇PINSΔ∇Φ−Δ∇PINSXins−X0=Aa0m×30m×30m×mAa0m×30m×3λIm×mI3×303×303×303×m⋅X+εΔ∇PεΔ∇ϕεins,where Xins represents the INS-derived position; X0 represents the initial position; I is the identity matrix; εins is the observation noise of the INS system obtained by the tightly coupled filtering prior equation.Discrete state equations and observation equations are as follows:(16)Xk=Φk,k−1Xk−1+Wk−1,Zk=HkXk−1+Vk,where Φk,k−1 is the one-step state transition matrix, which is defined by
(17)Φk,k−1=F9×909×m0m×9Im×m,F9×9=I3×3TI3×30.5T2I3×303×3I3×3TI3×303×303×3I3×3.Wk−1 is the system noise vector, whose covariance matrix is Qk−1. They are constructed as follows:
(18)Wk−1=0,0,0,0,0,0,ωax,ωay,ωaz,ωΔ∇N1,⋯,ωΔ∇NmT,(19)Qk−1=diag0,0,0,0,0,0,Tσax2,Tσay2,Tσaz2,TσΔ∇N12,⋯,TσΔ∇Nm2,where σax=σay=σaz=σa and σΔ∇Ni=σΔ∇Ni=1,⋯,m are the standard deviation of acceleration process noise and DD carrier phase ambiguity process noise, respectively. T is the time interval.Zk is the observation vector; Hk is the observation matrix; Vk is the observation noise vector. Among them, the covariance matrix of Vk is Rk.Here, the random function model based on the elevation angle is used forRk. The variance of undifferenced observation is calculated as [37]
(20)σ2=σ02+σ02sin2el,where σ0 is the standard deviation factor, which is valued according to the type of observation.


#### 2.4.2. Bifactor Robust and Classified Factor Adaptive Filtering for AR

In the UAV-tanker proximity, the multipath effect due to the strong reflection and state abnormal disturbance by accidental airflow reduce the accuracy of ambiguity floats, which affect the success rate of ambiguity fixing greatly. Therefore, the bifactor robust and classified factor adaptive filtering for AR is developed.As shown in Figure2, the Kalman filtering solution is obtained with the code pseudorange and carrier phase observations update sequentially; then, the residual statistic and state discrepancy statistic are calculated for testing the outliers in the observations and predicted states. If the statistics are both smaller than the thresholds, it means that the observations and predicted states are acceptable. Otherwise, if the statistics are larger than the threshold, robust bifactor and classified adaptive factors are calculated. Then, covariance matrices are reconstructed by the equivalent weight matrices and updated in the filter. Thus, the robust adaptive filtering solution is obtained. Finally, the ambiguity floats are fixed with the LAMBDA method and ratio test for reliability verification. If the fixed solution passes the ratio test, the fixed solution is adopted. Otherwise, the float solution is used.
(1)
Sequential Kalman filteringFigure 2
The bifactor robust and classified factor adaptive filtering AR algorithm flowchart.Assuming that the observations are not related to each other, the classified observations sequential Kalman filtering parameter estimation method is employed. For one thing, sequential processing can improve the computational efficiency as well as the stability of numerical calculation; for another, it enables to detect the outliers in individual measurement channels.In the sequential Kalman filtering, the time update step is as follows:(21)X^k,k−1=Φk,k−1X^k−1,k−1,Pk,k−1=Φk,k−1Pk−1,k−1Φk,k−1T+Qk−1.The measurement update steps are then processed with code pseudorange and carrier phase observations sequentially.Firstly, the measurement update steps are processed with DD code pseudorange and INS virtual observations.(22)X^k,k1=X^k,k−1+Kk1Zk1−Hk1X^k,k−1,Kk1=Pk,k−1Hk1THk1Pk,k−1Hk1T+Rk1−1,Pk,k1=I−Kk1Hk1Pk,k−1,where
(23)Zk1=Δ∇P−Δ∇PINSXins−X0,Hk1=Aa0m×30m×30m×mI3×303×303×303×m,Rk1=RΔ∇PRins.Secondly, the measurement update step is processed with DD carrier phase observations.(24)X^k,k2=X^k,k1+Kk2Zk2−Hk2X^k,k1,Kk2=Pk,k1Hk2THk2Pk,k1Hk1T+Rk2−1,Pk,k2=I−Kk2Hk2Pk,k1,where Zk2=Δ∇Φ−Δ∇PINS, Hk2=Aa0m×30m×3λIm×m and Rk2=RΔ∇ϕ.
(2)
Design of robust bifactorConsidering that the code pseudorange multipath error is usually much larger than the carrier phase multipath error. The code pseudorange multipath gross errors influence the accuracy of ambiguity float solution greatly. Therefore, it is necessary to conduct robust processing for code pseudorange observations with multipath gross errors.Due to the correlation of GNSS double-difference observations, the robust bifactor is calculated by the IGGIII model [38] as follows:
(25)rij,k1=rii,k1⋅rjj,k1,with
(26)rii,k1=1,v~i,k1≤c0,c0v~i,k1c1−v~i,k1c1−c02,c0<v~i,k1≤c1,10−12,v~i,k1>c1,where v~i,k1 is the standardized residual statistic; c0 and c1 are the threshold parameters, which are valued as c0=1.0~2.5 and c1=3.5~8.0.Therefore, the equivalent observation covariance matrix is given by the corresponding observation equivalent weight matrix.(27)R̄k1=p11,k1r11,k1p12,k1r12,k1⋯p1m,k1r1m,k1p21,k1r21,k1p22,k1r22,k1⋯p2m,k1r2m,k1⋮⋮⋱⋮pm1,k1rm1,k1pm2,k1rm2,k1⋯pmm,k1rmm,k1−1,where pij,k1 represents the DD code pseudorange observation vector weight matrix element at k time.
(3)
Design of classified adaptive factorsTo reduce the influence of dynamic model disturbances on filtering results, classified adaptive factors are introduced to adjust the dynamic model real-timely. The state parameters are classified into two categories, the first class includes the position, velocity, and acceleration that are not always reliable due to the aircraft maneuvering motion. The second class includes the DD carrier phase ambiguities that are reliable when there are no cycle slips.Therefore, the classified adaptive factors are calculated as(28)γk=γk1γk2, with
(29)γki=1,ΔX~ki≤c2,c2ΔX~ki,ΔX~ki>c2,where i represents classification type; ΔX~ki is the state discrepancy statistic of corresponding class [38]; c2 is the regulation factor, and the value is usually 0.85~3.0.Therefore, the equivalent predicted state covariance matrix is provided by corresponding equivalent weight matrixΣXk,k−1, as [39]
(30)P̄k,k−1=γk1/2⋅ΣXk,k−1⋅γk1/2−1.Finally,R̄k1 and P̄k,k−1 are used to update the Rk1 and Pk,k−1 in the filtering (Equation (22)). Therefore, the accurate ambiguity floats are obtained. Then, the LAMBDA method is adopted to fix the ambiguities and the ratio test is used to check the reliability.


## 2.1. Algorithm Overview

As shown in Figure1, the relative position for AAR is the typical KKRP problem, in which the tanker is the moving reference station, and the UAV is the rover station. The raw GNSS observations of tanker including code pseudorange, carrier phase, and Doppler as well as position and velocity calculated by conventional TC with pseudorange and Doppler measurements (due to the limited pages, the conventional TC algorithm is not covered in this article.) are transmitted to the UAV by data link. Then, the observations are double-difference (DD) processed in order to reduce the time and space errors except for the multipath error and observation noise. Next, the instantaneous high-precise INS information is used to assist the DD single-frequency single epoch AR in the subfilter, which is realized by a bifactor robust and classified factor adaptive filtering algorithm (see Section 2.4). Afterwards, the GNSS/INS tightly coupled integrated system by adaptive Kalman filtering in main-filter is conducted with known ambiguity parameters (see Section 2.3). Meanwhile, inertial errors are feedback corrected by filtering estimation parameters, and the position, velocity, and attitude of UAV are output. Finally, the carrier phase-based kinematic-to-kinematic precise relative position is realized with known ambiguity (see Section 2.2).Figure 1
The algorithm roadmap of the two-stage adaptive filtering architecture for precise KKRP in AAR.


## 2.2. The Carrier Phase-Based KKRP Model

In the GNSS-based relative position, the code pseudorange, carrier phase, and Doppler observations are used to consist DD observation equations as follows:(1)Δ∇Purij=Δ∇ρurij+Δ∇Turij+Δ∇Iurij+εΔ∇P,urij,Δ∇Φurij=Δ∇ρurij+Δ∇Turij−Δ∇Iurij+λΔ∇N+εΔ∇Φ,urij,−λΔ∇Durij=Δ∇ρ̇urij+Δ∇Ṫurij+Δ∇İurij+εΔ∇D,urij,where Δ∇ denotes the symbol of DD operator; subscripts u and r represent the moving reference and rover station; superscripts i and j are the observed satellites consisting of DD; P, Φ, and D are code pseudorange, carrier phase (in distance), and Doppler observations, respectively. ρ and ρ̇ are the geometric distance and its changing rate between station and satellite. T, I, and Ṫ, İ represent tropospheric delay, ionospheric delay, and their changing rates. λ and N represent the carrier wavelength and initial integer ambiguity; εP, εΦ, and εD are the residual errors including multipath and measurement noises of code pseudorange, carrier phase, and Doppler observations. For a short baseline, the ionospheric delay and tropospheric delay in DD are too small to be neglected.Linearing equation (1) with baseline vector and its changing rate as unknown parameter, thus, it can be written as
(2)Δ∇Purij=−Ιri−Ιrj⋅bur+εΔ∇P,urij,Δ∇Φurij=−Ιri−Ιrj⋅bur+λ⋅Δ∇Nurij+εΔ∇Φ,urij,−λ⋅Δ∇Durij=−Ιri−Ιrj⋅ḃur+εΔ∇D,urij,where bur and ḃur are the baseline vector and velocity vector; Ιri is the unit direction vector between satellite and rover calculated by Iri=ri−rr/ri−rr.If the DD integer ambiguityΔ∇Nurij can be solved on fly accurately, the high-precision baseline vector and velocity vector can be determined by carrier phase and Doppler observations [31].


## 2.3. GNSS/INS Tightly Coupled Model

For the GNSS/INS tightly coupled navigation system, the system model is derived from INS instruments, and the observation model is composed of the differences between GNSS DD observations and INS-derived counterparts. The integrated system is implemented by adaptive Kalman filtering with feedback correction to INS sensor biases.


### 2.3.1. System Model

The system state-space model depends on the INS error model and system error description of inertial sensors. The integrated system model can be described as the following psi-angle error equations [32] without considering satellite system errors:
(3)δṙ=−ωenn×δr+δv,δv̇=−2ωien+ωenn×δv−ψ×fn+Cbn∇b,ψ̇=−ωien+ωenn×ψ−Cbnεb,where the subscripts i, e, n, and b represent the inertial, Earth, navigation (east-north-up), and body frame, respectively; δr, δv, and ψ represent the position, velocity, and attitude angle error vectors, respectively; fn is the specific force vector; ωien is the Earth rotation rate with respect to the inertial frame; ωenn is the rotation rate of the navigation frame with respect to Earth. Cbn is the transformation matrix from body frame to navigation frame. ∇b represents the accelerometer error vector; εb represents the gyro drift error vector.Accurate models of accelerometer and gyro errors are important to improve the navigation performance, which are augmented into the filtering states for real-time parameter estimation. In this paper, the accelerometer error is modeled as first-order Gauss-Markov process; gyro drift error is modeled as random constant and first-order Gauss-Markov process [33] as follows.
(4)∇̇a=−1Ta∇a+ωa,ε=εb+εr+ωg,(5)ε̇r=−1Tgεr+ωr,where εb is the random constant of gyro error with ε̇b=0; Ta and Tg are the correlation time of the random process of accelerometer and gyro, respectively; ωa, ωg, and ωr are the driving white noise.Therefore, the system state equation can be expressed as follows:(6)Ẋt=FtXt+GtWt,where F is the state coefficient matrix; G is the noise coefficient matrix. W is the noise vector. X is the state vector, which includes 9 navigation error states and 9 inertial sensor bias errors, written as
(7)X=δrEδrNδrUδvEδvNδvUφEφNφUεbxεbyεbzεrxεryεrz∇ax∇ay∇azT.


### 2.3.2. Measurement Model

The measurement model describes the relationship between the measurements and the unknown parameters. The differences between GNSS DD code pseudorange, carrier phase and Doppler observations, and INS predicted satellite-to-ground distance and relative velocity consist the measurements. Therefore, the measurement equations are written as follows:(8)∇ΔP−∇ΔPINS=Aa⋅Cne⋅δr+ε∇ΔP,∇ΔΦ−∇ΔPINS−λ⋅∇ΔN=Aa⋅Cne⋅δr+ε∇ΔΦ,−λ⋅∇ΔD−∇ΔvINS=Aa⋅Cne⋅δv+ε∇ΔD,where PINS is the INS predicted satellite-to-ground distance calculated by PINS=xINS−xs2+yINS−ys2+zINS−zs2; vINS is the relative velocity between rover station and satellite calculated by vINS=ẋINS−ẋs2+ẏINS−ẏs2+żINS−żs2; Aa is the design matrix calculated by −IINSi−IINSj; Cne is the transformation matrix from navigation frame to Earth frame. Write the measurement equations into matrix form as follows:
(9)Zt=HtXt+Vt,where Z is the measurement vector; H is the measurement model coefficient matrix; V is the measurement noise matrix.


### 2.3.3. Adaptive Kalman Filtering Model

Since the integrated navigation performance of GNSS/INS is always affected by the abnormal predicted states such as UVA maneuvering or wrong ambiguity fixed values in GNSS, thus, the adaptive Kalman filtering algorithm is used to improve the reliability of the TC integrated system. The estimation of state parameters can be expressed as(10)X^k,k=X^k,k−1+K̄kZk−HkX^k,k−1,(11)K̄k=1αkPk,k−1HkTHk1αkPk,k−1HkT+Rk−1,where X^k,k−1 and Pk,k−1 are the predicted state vector and its covariance matrix. Rk is the measurement covariance matrix. K̄k is the adaptive filtering gain matrix, and αk is the adaptive factor. The adaptive factor is constructed by Huber weight function [34] as follows:
(12)αk=1ΔM~k≤k0,k0ΔM~kΔM~k>k0,where k0 is the threshold constant, which is valued at 0.85~3.0 [35]. ΔM~k is the test statistic, which is calculated as
(13)ΔMk=VkTVktrQVk,where Vk is the innovation vector calculated by Vk=HkX^k,k−1−Zk and QVk is its covariance matrix calculated by QVk=HkPk,k−1HkT+Rk.


## 2.3.1. System Model

The system state-space model depends on the INS error model and system error description of inertial sensors. The integrated system model can be described as the following psi-angle error equations [32] without considering satellite system errors:
(3)δṙ=−ωenn×δr+δv,δv̇=−2ωien+ωenn×δv−ψ×fn+Cbn∇b,ψ̇=−ωien+ωenn×ψ−Cbnεb,where the subscripts i, e, n, and b represent the inertial, Earth, navigation (east-north-up), and body frame, respectively; δr, δv, and ψ represent the position, velocity, and attitude angle error vectors, respectively; fn is the specific force vector; ωien is the Earth rotation rate with respect to the inertial frame; ωenn is the rotation rate of the navigation frame with respect to Earth. Cbn is the transformation matrix from body frame to navigation frame. ∇b represents the accelerometer error vector; εb represents the gyro drift error vector.Accurate models of accelerometer and gyro errors are important to improve the navigation performance, which are augmented into the filtering states for real-time parameter estimation. In this paper, the accelerometer error is modeled as first-order Gauss-Markov process; gyro drift error is modeled as random constant and first-order Gauss-Markov process [33] as follows.
(4)∇̇a=−1Ta∇a+ωa,ε=εb+εr+ωg,(5)ε̇r=−1Tgεr+ωr,where εb is the random constant of gyro error with ε̇b=0; Ta and Tg are the correlation time of the random process of accelerometer and gyro, respectively; ωa, ωg, and ωr are the driving white noise.Therefore, the system state equation can be expressed as follows:(6)Ẋt=FtXt+GtWt,where F is the state coefficient matrix; G is the noise coefficient matrix. W is the noise vector. X is the state vector, which includes 9 navigation error states and 9 inertial sensor bias errors, written as
(7)X=δrEδrNδrUδvEδvNδvUφEφNφUεbxεbyεbzεrxεryεrz∇ax∇ay∇azT.


## 2.3.2. Measurement Model

The measurement model describes the relationship between the measurements and the unknown parameters. The differences between GNSS DD code pseudorange, carrier phase and Doppler observations, and INS predicted satellite-to-ground distance and relative velocity consist the measurements. Therefore, the measurement equations are written as follows:(8)∇ΔP−∇ΔPINS=Aa⋅Cne⋅δr+ε∇ΔP,∇ΔΦ−∇ΔPINS−λ⋅∇ΔN=Aa⋅Cne⋅δr+ε∇ΔΦ,−λ⋅∇ΔD−∇ΔvINS=Aa⋅Cne⋅δv+ε∇ΔD,where PINS is the INS predicted satellite-to-ground distance calculated by PINS=xINS−xs2+yINS−ys2+zINS−zs2; vINS is the relative velocity between rover station and satellite calculated by vINS=ẋINS−ẋs2+ẏINS−ẏs2+żINS−żs2; Aa is the design matrix calculated by −IINSi−IINSj; Cne is the transformation matrix from navigation frame to Earth frame. Write the measurement equations into matrix form as follows:
(9)Zt=HtXt+Vt,where Z is the measurement vector; H is the measurement model coefficient matrix; V is the measurement noise matrix.


## 2.3.3. Adaptive Kalman Filtering Model

Since the integrated navigation performance of GNSS/INS is always affected by the abnormal predicted states such as UVA maneuvering or wrong ambiguity fixed values in GNSS, thus, the adaptive Kalman filtering algorithm is used to improve the reliability of the TC integrated system. The estimation of state parameters can be expressed as(10)X^k,k=X^k,k−1+K̄kZk−HkX^k,k−1,(11)K̄k=1αkPk,k−1HkTHk1αkPk,k−1HkT+Rk−1,where X^k,k−1 and Pk,k−1 are the predicted state vector and its covariance matrix. Rk is the measurement covariance matrix. K̄k is the adaptive filtering gain matrix, and αk is the adaptive factor. The adaptive factor is constructed by Huber weight function [34] as follows:
(12)αk=1ΔM~k≤k0,k0ΔM~kΔM~k>k0,where k0 is the threshold constant, which is valued at 0.85~3.0 [35]. ΔM~k is the test statistic, which is calculated as
(13)ΔMk=VkTVktrQVk,where Vk is the innovation vector calculated by Vk=HkX^k,k−1−Zk and QVk is its covariance matrix calculated by QVk=HkPk,k−1HkT+Rk.


## 2.4. INS-Aided Bifactor Robust and Classified Factor Adaptive Filtering AR Model

As mentioned in Section2.1, accurate ambiguity parameters are the premise of high precision positioning for KKRP. However, it is difficult to resolve with a single-frequency carrier phase, especially in complex high dynamic environments. Since INS can provide instantaneous high precision and continuous positioning ability with the GNSS/INS TC integrated system, the accuracy of ambiguity floats solved by INS-aided can be improved, which can constrain the ambiguity search space and improve the efficiency of AR.


### 2.4.1. Single-Epoch INS-Aided GNSS AR Filtering Model

The system model is constructed by GNSS only with a constant acceleration (CA) kinematic model, and the carrier phase ambiguity parameters are modeled as a random walk process. The state vector is defined by(14)X=δx,δy,δz,δvx,δvy,δvz,δax,δay,δaz,Δ∇N1,Δ∇N2,⋯,Δ∇NmT,where δx,δy,δz,δvx,δvy,δvz,δax,δay,andδaz are the components of the position, velocity, and acceleration increment, respectively. Δ∇Nii=1,2,⋯,m represent the DD carrier phase ambiguities.In the INS-aided AR process, the DD pseudorange and carrier phase observation equations are linearized at the INS-derived position. Moreover, the INS-derived position is used to construct additional coordinate constraint by a virtual observation. Therefore, the observation equations can be written as [36]
(15)Δ∇P−Δ∇PINSΔ∇Φ−Δ∇PINSXins−X0=Aa0m×30m×30m×mAa0m×30m×3λIm×mI3×303×303×303×m⋅X+εΔ∇PεΔ∇ϕεins,where Xins represents the INS-derived position; X0 represents the initial position; I is the identity matrix; εins is the observation noise of the INS system obtained by the tightly coupled filtering prior equation.Discrete state equations and observation equations are as follows:(16)Xk=Φk,k−1Xk−1+Wk−1,Zk=HkXk−1+Vk,where Φk,k−1 is the one-step state transition matrix, which is defined by
(17)Φk,k−1=F9×909×m0m×9Im×m,F9×9=I3×3TI3×30.5T2I3×303×3I3×3TI3×303×303×3I3×3.Wk−1 is the system noise vector, whose covariance matrix is Qk−1. They are constructed as follows:
(18)Wk−1=0,0,0,0,0,0,ωax,ωay,ωaz,ωΔ∇N1,⋯,ωΔ∇NmT,(19)Qk−1=diag0,0,0,0,0,0,Tσax2,Tσay2,Tσaz2,TσΔ∇N12,⋯,TσΔ∇Nm2,where σax=σay=σaz=σa and σΔ∇Ni=σΔ∇Ni=1,⋯,m are the standard deviation of acceleration process noise and DD carrier phase ambiguity process noise, respectively. T is the time interval.Zk is the observation vector; Hk is the observation matrix; Vk is the observation noise vector. Among them, the covariance matrix of Vk is Rk.Here, the random function model based on the elevation angle is used forRk. The variance of undifferenced observation is calculated as [37]
(20)σ2=σ02+σ02sin2el,where σ0 is the standard deviation factor, which is valued according to the type of observation.


### 2.4.2. Bifactor Robust and Classified Factor Adaptive Filtering for AR

In the UAV-tanker proximity, the multipath effect due to the strong reflection and state abnormal disturbance by accidental airflow reduce the accuracy of ambiguity floats, which affect the success rate of ambiguity fixing greatly. Therefore, the bifactor robust and classified factor adaptive filtering for AR is developed.As shown in Figure2, the Kalman filtering solution is obtained with the code pseudorange and carrier phase observations update sequentially; then, the residual statistic and state discrepancy statistic are calculated for testing the outliers in the observations and predicted states. If the statistics are both smaller than the thresholds, it means that the observations and predicted states are acceptable. Otherwise, if the statistics are larger than the threshold, robust bifactor and classified adaptive factors are calculated. Then, covariance matrices are reconstructed by the equivalent weight matrices and updated in the filter. Thus, the robust adaptive filtering solution is obtained. Finally, the ambiguity floats are fixed with the LAMBDA method and ratio test for reliability verification. If the fixed solution passes the ratio test, the fixed solution is adopted. Otherwise, the float solution is used.
(1)
Sequential Kalman filteringFigure 2
The bifactor robust and classified factor adaptive filtering AR algorithm flowchart.Assuming that the observations are not related to each other, the classified observations sequential Kalman filtering parameter estimation method is employed. For one thing, sequential processing can improve the computational efficiency as well as the stability of numerical calculation; for another, it enables to detect the outliers in individual measurement channels.In the sequential Kalman filtering, the time update step is as follows:(21)X^k,k−1=Φk,k−1X^k−1,k−1,Pk,k−1=Φk,k−1Pk−1,k−1Φk,k−1T+Qk−1.The measurement update steps are then processed with code pseudorange and carrier phase observations sequentially.Firstly, the measurement update steps are processed with DD code pseudorange and INS virtual observations.(22)X^k,k1=X^k,k−1+Kk1Zk1−Hk1X^k,k−1,Kk1=Pk,k−1Hk1THk1Pk,k−1Hk1T+Rk1−1,Pk,k1=I−Kk1Hk1Pk,k−1,where
(23)Zk1=Δ∇P−Δ∇PINSXins−X0,Hk1=Aa0m×30m×30m×mI3×303×303×303×m,Rk1=RΔ∇PRins.Secondly, the measurement update step is processed with DD carrier phase observations.(24)X^k,k2=X^k,k1+Kk2Zk2−Hk2X^k,k1,Kk2=Pk,k1Hk2THk2Pk,k1Hk1T+Rk2−1,Pk,k2=I−Kk2Hk2Pk,k1,where Zk2=Δ∇Φ−Δ∇PINS, Hk2=Aa0m×30m×3λIm×m and Rk2=RΔ∇ϕ.
(2)
Design of robust bifactorConsidering that the code pseudorange multipath error is usually much larger than the carrier phase multipath error. The code pseudorange multipath gross errors influence the accuracy of ambiguity float solution greatly. Therefore, it is necessary to conduct robust processing for code pseudorange observations with multipath gross errors.Due to the correlation of GNSS double-difference observations, the robust bifactor is calculated by the IGGIII model [38] as follows:
(25)rij,k1=rii,k1⋅rjj,k1,with
(26)rii,k1=1,v~i,k1≤c0,c0v~i,k1c1−v~i,k1c1−c02,c0<v~i,k1≤c1,10−12,v~i,k1>c1,where v~i,k1 is the standardized residual statistic; c0 and c1 are the threshold parameters, which are valued as c0=1.0~2.5 and c1=3.5~8.0.Therefore, the equivalent observation covariance matrix is given by the corresponding observation equivalent weight matrix.(27)R̄k1=p11,k1r11,k1p12,k1r12,k1⋯p1m,k1r1m,k1p21,k1r21,k1p22,k1r22,k1⋯p2m,k1r2m,k1⋮⋮⋱⋮pm1,k1rm1,k1pm2,k1rm2,k1⋯pmm,k1rmm,k1−1,where pij,k1 represents the DD code pseudorange observation vector weight matrix element at k time.
(3)
Design of classified adaptive factorsTo reduce the influence of dynamic model disturbances on filtering results, classified adaptive factors are introduced to adjust the dynamic model real-timely. The state parameters are classified into two categories, the first class includes the position, velocity, and acceleration that are not always reliable due to the aircraft maneuvering motion. The second class includes the DD carrier phase ambiguities that are reliable when there are no cycle slips.Therefore, the classified adaptive factors are calculated as(28)γk=γk1γk2, with
(29)γki=1,ΔX~ki≤c2,c2ΔX~ki,ΔX~ki>c2,where i represents classification type; ΔX~ki is the state discrepancy statistic of corresponding class [38]; c2 is the regulation factor, and the value is usually 0.85~3.0.Therefore, the equivalent predicted state covariance matrix is provided by corresponding equivalent weight matrixΣXk,k−1, as [39]
(30)P̄k,k−1=γk1/2⋅ΣXk,k−1⋅γk1/2−1.Finally,R̄k1 and P̄k,k−1 are used to update the Rk1 and Pk,k−1 in the filtering (Equation (22)). Therefore, the accurate ambiguity floats are obtained. Then, the LAMBDA method is adopted to fix the ambiguities and the ratio test is used to check the reliability.


## 2.4.1. Single-Epoch INS-Aided GNSS AR Filtering Model

The system model is constructed by GNSS only with a constant acceleration (CA) kinematic model, and the carrier phase ambiguity parameters are modeled as a random walk process. The state vector is defined by(14)X=δx,δy,δz,δvx,δvy,δvz,δax,δay,δaz,Δ∇N1,Δ∇N2,⋯,Δ∇NmT,where δx,δy,δz,δvx,δvy,δvz,δax,δay,andδaz are the components of the position, velocity, and acceleration increment, respectively. Δ∇Nii=1,2,⋯,m represent the DD carrier phase ambiguities.In the INS-aided AR process, the DD pseudorange and carrier phase observation equations are linearized at the INS-derived position. Moreover, the INS-derived position is used to construct additional coordinate constraint by a virtual observation. Therefore, the observation equations can be written as [36]
(15)Δ∇P−Δ∇PINSΔ∇Φ−Δ∇PINSXins−X0=Aa0m×30m×30m×mAa0m×30m×3λIm×mI3×303×303×303×m⋅X+εΔ∇PεΔ∇ϕεins,where Xins represents the INS-derived position; X0 represents the initial position; I is the identity matrix; εins is the observation noise of the INS system obtained by the tightly coupled filtering prior equation.Discrete state equations and observation equations are as follows:(16)Xk=Φk,k−1Xk−1+Wk−1,Zk=HkXk−1+Vk,where Φk,k−1 is the one-step state transition matrix, which is defined by
(17)Φk,k−1=F9×909×m0m×9Im×m,F9×9=I3×3TI3×30.5T2I3×303×3I3×3TI3×303×303×3I3×3.Wk−1 is the system noise vector, whose covariance matrix is Qk−1. They are constructed as follows:
(18)Wk−1=0,0,0,0,0,0,ωax,ωay,ωaz,ωΔ∇N1,⋯,ωΔ∇NmT,(19)Qk−1=diag0,0,0,0,0,0,Tσax2,Tσay2,Tσaz2,TσΔ∇N12,⋯,TσΔ∇Nm2,where σax=σay=σaz=σa and σΔ∇Ni=σΔ∇Ni=1,⋯,m are the standard deviation of acceleration process noise and DD carrier phase ambiguity process noise, respectively. T is the time interval.Zk is the observation vector; Hk is the observation matrix; Vk is the observation noise vector. Among them, the covariance matrix of Vk is Rk.Here, the random function model based on the elevation angle is used forRk. The variance of undifferenced observation is calculated as [37]
(20)σ2=σ02+σ02sin2el,where σ0 is the standard deviation factor, which is valued according to the type of observation.


## 2.4.2. Bifactor Robust and Classified Factor Adaptive Filtering for AR

In the UAV-tanker proximity, the multipath effect due to the strong reflection and state abnormal disturbance by accidental airflow reduce the accuracy of ambiguity floats, which affect the success rate of ambiguity fixing greatly. Therefore, the bifactor robust and classified factor adaptive filtering for AR is developed.As shown in Figure2, the Kalman filtering solution is obtained with the code pseudorange and carrier phase observations update sequentially; then, the residual statistic and state discrepancy statistic are calculated for testing the outliers in the observations and predicted states. If the statistics are both smaller than the thresholds, it means that the observations and predicted states are acceptable. Otherwise, if the statistics are larger than the threshold, robust bifactor and classified adaptive factors are calculated. Then, covariance matrices are reconstructed by the equivalent weight matrices and updated in the filter. Thus, the robust adaptive filtering solution is obtained. Finally, the ambiguity floats are fixed with the LAMBDA method and ratio test for reliability verification. If the fixed solution passes the ratio test, the fixed solution is adopted. Otherwise, the float solution is used.
(1)
Sequential Kalman filteringFigure 2
The bifactor robust and classified factor adaptive filtering AR algorithm flowchart.Assuming that the observations are not related to each other, the classified observations sequential Kalman filtering parameter estimation method is employed. For one thing, sequential processing can improve the computational efficiency as well as the stability of numerical calculation; for another, it enables to detect the outliers in individual measurement channels.In the sequential Kalman filtering, the time update step is as follows:(21)X^k,k−1=Φk,k−1X^k−1,k−1,Pk,k−1=Φk,k−1Pk−1,k−1Φk,k−1T+Qk−1.The measurement update steps are then processed with code pseudorange and carrier phase observations sequentially.Firstly, the measurement update steps are processed with DD code pseudorange and INS virtual observations.(22)X^k,k1=X^k,k−1+Kk1Zk1−Hk1X^k,k−1,Kk1=Pk,k−1Hk1THk1Pk,k−1Hk1T+Rk1−1,Pk,k1=I−Kk1Hk1Pk,k−1,where
(23)Zk1=Δ∇P−Δ∇PINSXins−X0,Hk1=Aa0m×30m×30m×mI3×303×303×303×m,Rk1=RΔ∇PRins.Secondly, the measurement update step is processed with DD carrier phase observations.(24)X^k,k2=X^k,k1+Kk2Zk2−Hk2X^k,k1,Kk2=Pk,k1Hk2THk2Pk,k1Hk1T+Rk2−1,Pk,k2=I−Kk2Hk2Pk,k1,where Zk2=Δ∇Φ−Δ∇PINS, Hk2=Aa0m×30m×3λIm×m and Rk2=RΔ∇ϕ.
(2)
Design of robust bifactorConsidering that the code pseudorange multipath error is usually much larger than the carrier phase multipath error. The code pseudorange multipath gross errors influence the accuracy of ambiguity float solution greatly. Therefore, it is necessary to conduct robust processing for code pseudorange observations with multipath gross errors.Due to the correlation of GNSS double-difference observations, the robust bifactor is calculated by the IGGIII model [38] as follows:
(25)rij,k1=rii,k1⋅rjj,k1,with
(26)rii,k1=1,v~i,k1≤c0,c0v~i,k1c1−v~i,k1c1−c02,c0<v~i,k1≤c1,10−12,v~i,k1>c1,where v~i,k1 is the standardized residual statistic; c0 and c1 are the threshold parameters, which are valued as c0=1.0~2.5 and c1=3.5~8.0.Therefore, the equivalent observation covariance matrix is given by the corresponding observation equivalent weight matrix.(27)R̄k1=p11,k1r11,k1p12,k1r12,k1⋯p1m,k1r1m,k1p21,k1r21,k1p22,k1r22,k1⋯p2m,k1r2m,k1⋮⋮⋱⋮pm1,k1rm1,k1pm2,k1rm2,k1⋯pmm,k1rmm,k1−1,where pij,k1 represents the DD code pseudorange observation vector weight matrix element at k time.
(3)
Design of classified adaptive factorsTo reduce the influence of dynamic model disturbances on filtering results, classified adaptive factors are introduced to adjust the dynamic model real-timely. The state parameters are classified into two categories, the first class includes the position, velocity, and acceleration that are not always reliable due to the aircraft maneuvering motion. The second class includes the DD carrier phase ambiguities that are reliable when there are no cycle slips.Therefore, the classified adaptive factors are calculated as(28)γk=γk1γk2, with
(29)γki=1,ΔX~ki≤c2,c2ΔX~ki,ΔX~ki>c2,where i represents classification type; ΔX~ki is the state discrepancy statistic of corresponding class [38]; c2 is the regulation factor, and the value is usually 0.85~3.0.Therefore, the equivalent predicted state covariance matrix is provided by corresponding equivalent weight matrixΣXk,k−1, as [39]
(30)P̄k,k−1=γk1/2⋅ΣXk,k−1⋅γk1/2−1.Finally,R̄k1 and P̄k,k−1 are used to update the Rk1 and Pk,k−1 in the filtering (Equation (22)). Therefore, the accurate ambiguity floats are obtained. Then, the LAMBDA method is adopted to fix the ambiguities and the ratio test is used to check the reliability.


## 3. Simulation Experiment and Discussion

### 3.1. Simulation Conditions

In order to verify the feasibility of the algorithm proposed, simulation experiment has been made using Matlab between the tanker and UAV for aerial refueling from rendezvous to following stage. The simulation conditions are set as follows:(1)
Set the gyro error with a constant bias 1°/h. The correlation time constant of random process is 1500 ppm, and driving white noise is 0.01°/h. The correlation time constant of random process for accelerometer is 4000 ppm, and driving white noise is 0.3 mg m/s2. The frequency of INS is 200 Hz(2)
Set the standard deviation of GNSS code pseudorange with 0.37 m, carrier phase with 0.01 m, and Doppler with 0.01 m/s. The frequency of GNSS is 1 Hz. The cutoff degree is 25°(3)
The tanker and UAV fly level from the initial position [118.00014°, 31.73087°, 7010.0] and [117.94924°, 31.75862°, 6913.25] with a velocity of [0, 100 m/s, 0] in the body coordinate frame and attitude of [0, 0, 0] and [0, 0, 90°] in the navigation frame, respectively. The tanker keeps flying level in the designated airspace, while the UAV is approaching from 4.5 km away to 50 m with maneuver flight firstly and then flying level following the tanker. The trajectory parameters of the UAV are shown in Table1. The trajectories of the tanker and the UAV are shown in Figure 3(4)
The GNSS observations are simulated based on B1 frequency with actual BDS satellite ephemeris. The skyplot of visible satellites simulated is shown in Figure4. The Ratio threshold is valued 3 in the success rate reliability testTable 1
The trajectory parameters of the UAV.
Flight phaseFlight actionDuration (s)Acceleration (m/s2)Attitude (°/s) (roll, pitch, yaw angular rate)1Level flight10[0,0,0][0,0,0]2Preturn10[0,0,0][-0.5,0,0]3Turn45[0,0,0][0,0,-2]4Level off10[0,0,0][0.5,0,0]5Level flight65[0,0,0][0,0,0]6Pull up40[0,0.1,0][0,0.01,0]7Climb up80[0,0,0][0,0,0]8Level off40[0,-0.1,0][0,-0.01,0]9Level flight900[0,0,0][0,0,0]Figure 3
The trajectories of the tanker and the UAV.Figure 4
The skyplot of the visible satellites.


### 3.2. Simulation Results and Discussion

#### 3.2.1. The Performance of Various Algorithms for AR

In order to evaluate the AR effects with different algorithms, three indicators of ratio value, ADOP value, and empirical success ratePSE are defined, respectively, as follows:
(31)ratio=ΩsecΩmin,where Ωsec and Ωmin are the suboptimal and optimal quadratic form of ambiguity residual determined in LAMBDA.
(32)ADOP=detQX∧a1/m,where QX^a is the covariance matrix of ambiguity floats; m is the number of ambiguity floats.
(33)PSE=numberofcorrectlyfixedepochstotalnumberofepochs.In order to simulate the multipath gross errors caused by the strong reflection effect and state outliers emerged by airflow interference in UAV-tanker proximity, the data is processed as follows to test the various algorithms for AR.Data 1. Original data.Data 2. Gross errors of 5 m, 10 m, 20 m, and 40 m are added to the pseudorange observations randomly to simulate multipath errors at epochs from 200 s to 210 s, 500 s to 510 s, 800 s to 810 s, and 1100s to 1110s, respectively.Data 3. Velocity error of 5 m/s is added to the velocity predicted parameters to simulate kinematic model abnormality at epochs from 400 s to 410 s and 600 s to 610 s.Data 4. Pseudorange observation gross errors of 20 m and velocity errors of 5 m/s are added simultaneously at epochs from 300 s to 310 s and 700 s to 710 s.The following four schemes are performed for AR, respectively.Scheme 1. Weighted least square algorithm (WLS).Scheme 2. Extend Kalman filtering algorithm (EKF).Scheme 3. INS-aided extend Kalman filtering algorithm (IEKF).Scheme 4. INS-aided bifactor robust Kalman filtering algorithm (IBRKF).Scheme 5. INS-aided bifactor robust and classified factors adaptive filtering algorithm (IBRCAF).The test thresholds are taken as the empirical value ofc0=1.5, c1=4.5, and c2=2.0.Firstly, the performances of WLS, EKF, and IEKF algorithms for AR are compared with Data 1. As shown in Figures5 and 6, the efficiency of AR with EKF is better than the WLS method, which is owing to both the system model and observation model established in dynamic conditions. Moreover, as shown in the subgraph of Figure 5, the ratio values of IEKF are greater than the test threshold, whereas they are smaller with WLS and EKF in comparison to IEKF. The ratio means are 4.35, 6.21, and 13.03 for WLS, EKF, and IEKF, respectively. The ratio mean is improved by 109.8% with INS-aided in comparison to without INS-aided. The ADOP value is decreased obviously by INS-aided as well. Those are benefit from the improvement of accuracy of ambiguity floats and reduction of research space of ambiguity. The success rates are 57.5%, 88.1%, and 94.4% of WLS, EKF, and IEKF for AR, respectively, in which the success rate is improved by 7.2% with INS-aided. Hence, the prior INS-derived position provides a useful constraint to enhance the probability of rightly fixing ambiguity. In addition, since the accuracy of ambiguity floats is closely relative to the pseudorange precision and INS-derived position precision, the ADOP value is further analysed with different pseudorange precisions and INS-derived position precisions at epoch 300 s. As shown in Figure 7, the ADOP value increases obviously as the pseudorange precision decreases with the WLS algorithm. However, they increase gently with EKF and IEKF methods, which can obtain higher precision floats in a dynamic environment. As shown in Figure 8, AR performance with IEKF is better than EKF when INS-derived position precision is within 1.5 m. At this moment, the INS-derived position can play a positive role of coordinate constraint. Otherwise, the INS-aided effect becomes worse [27].Figure 5
The ratio values of various algorithms (Data 1).Figure 6
The ADOP values of various algorithms (Data 1).Figure 7
The ADOP values with pseudorange precision (Data 1).Figure 8
The ADOP values with INS-derived position precision (Data 1).Aiming at the multipath outliers in the observations in the UAV-tanker proximity, the WLS, EKF, IEKF, and IBRKF algorithm effects are compared with Data 2. As shown in Figure9, the ratio values reduce greatly at the epochs of the outliers appearing without a robust algorithm so as to decrease the success rate of AR. In the subgraph of Figure 9, it can be seen that the ratio values calculated by the IBRKF algorithm remain larger than the test threshold at the epochs from 800 s to 810 s when 20 m gross error is added, which is obviously superior to the other methods. The same results can be achieved when gross errors of different magnitudes are added. The ratio means are 3.26, 6.11, 9.21, and 12.11, and the success rates are 43.6%, 73.7%, 81.3%, and 94.6% for the four algorithms, respectively.Figure 9
The ratio values of WLS, EKF, IEKF, and IBRKF (Data 2).Aiming at the state predicted abnormities with the maneuvering or airflow interference, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 3. As shown in Figure10, the ratio values decrease significantly at the epochs of the state abnormities emerging except for the IBRCAF algorithm. Furthermore, it can be seen in the subgraph of Figure 10; the IBRCAF algorithm can produce a good adjustment effect at the epochs from 600 s to 610 when there are moderate deviations in the state predicted parameters. The ratio means are 6.98, 7.92, 9.31, and 12.43, and the success rates are 84.0%, 87.5%, 89.2%, and 93.7% for the four algorithms, respectively.Figure 10
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 3).Furthermore, in order to test the full performance of the IBRCAF algorithm for AR, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 4. As shown in Figure11 and its subgraph, the IBRCAF algorithm can keep better performance for AR than other algorithms in the presence of the observation outliers and state predicted abnormities simultaneously. The ratio means are 6.01, 7.31, 8.77, and 12.17, and the success rates are 81.1%, 84.2%, 87.9%, and 93.8% for the four algorithms, respectively.Figure 11
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 4).The ratio means of various schemes for Data 1–4 are shown in Figure12. And the success rates of various algorithms for Data 1–4 are shown in Table 2.Figure 12
The ratio means of various schemes (Data 1–4).Table 2
The success rates of various algorithms for Data 1–4.
SchemesData 1Data 2Data 3Data 4WLS57.543.6——EKF88.173.784.081.1IEKF94.481.387.584.2IBRKF—94.689.287.9IBRCAF——93.793.8“—” means no use of the algorithm for the data.


#### 3.2.2. The Performance of Two-Stage Adaptive Filtering for RP

In order to verify the two-stage filtering performance for RP, the following ten algorithms are conducted for RP, respectively. The data are the same with Section3.2.1.Algorithm 1a. Weighted least square for AR and standard Kalman filtering for TC (WLS-SKF).Algorithm 1b. Weighted least square for AR and adaptive Kalman filtering for TC (WLS-AKF).Algorithm 2a. Extend Kalman filtering for AR and standard Kalman filtering for TC (EKF-SKF).Algorithm 2b. Extend Kalman filtering for AR and adaptive Kalman filtering for TC (EKF-AKF).Algorithm 3a. INS-aided extend Kalman filtering for AR and standard Kalman filtering for TC (IEKF-SKF).Algorithm 3b. INS-aided extend Kalman filtering for AR and adaptive Kalman filtering for TC (IEKF-AKF).Algorithm 4a. INS-aided bifactor robust Kalman filtering for AR and standard Kalman filtering for TC (IBRKF-SKF).Algorithm 4b. INS-aided bifactor robust Kalman filtering for AR and adaptive Kalman filtering for TC (IBRKF-AKF).Algorithm 5a. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-SKF).Algorithm 5b. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-AKF).Firstly, Algorithms 1a–3b are adopted for Data 1. As shown in Figures13 and 14, compared with WLS-SKF and EKF-SKF algorithms, the baseline error and velocity error decrease with the IEKF-SKF algorithm, which are owing to the success rate of AR increasing with the IEKF algorithm. Compared with WLS/EKF/IEKF-SKF, the baseline error and velocity error show minor fluctuations with the WLS/EKF/IEKF-AKF algorithms. Particularly, in the presence of maneuvering of UAV in the rendezvous phase, the velocity error increases obviously with the SKF algorithm at epoch of 20 s, 65 s, 180 s, and 300 s due to the changing of flight state parameters. However, the phenomenon is improved greatly with the AKF algorithm, which can adaptively adjust the proportion of the abnormal state estimation by model deviation. The RMS errors of baseline and velocity of various algorithms are shown in Figure 15. Compared with IEKF-SKF, the RMS errors of baseline and velocity with the IEKF-AKF algorithm are improved by 11.11% and 30.76%, respectively.Figure 13
The baseline errors of various algorithms (Data 1).Figure 14
The velocity errors of various algorithms (Data 1).Figure 15
The RMS errors of baseline and velocity of various algorithms (Data 1).Algorithms 1a–4b are utilized for Data 2. As shown in Figures16 and 17, the baseline error and velocity error increase rapidly at the epochs in the presence of gross errors with the WLS/EKF/IEKF algorithms. Among them, the performance of WLS is the worst, which mainly depends on the quality of observations. Furthermore, the errors obviously increase as the magnitude of gross errors increases. However, the IBRKF algorithm has a strong ability to resist the gross errors of different magnitudes. The maximum errors of baseline and velocity of the IBRKF-AKF algorithm are 0.76 m and 0.08 m/s, which maintain good performance in the measurement gross error environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 18.Figure 16
The baseline errors of various algorithms (Data 2).Figure 17
The velocity errors of various algorithms (Data 2).Figure 18
The RMS errors of baseline and velocity of various algorithms (Data 2).Algorithms 2a–5b are used for Data 3. As shown in Figures19 and 20, the baseline error and velocity error increase significantly in the presence of state predicted outliers at epoch of 400~410 s and 600~610 s without adaptive adjustment. However, the IBRCAF-AKF algorithm can effectively adjust the contribution of state outliers and model deviation in the two-stage filter. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.55 m and 0.05 m/s. In addition, it can be found that the IBRKF algorithm is inability for state abnormalities, which is insufficient performance in the high dynamic environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 21.Figure 19
The baseline errors of various algorithms (Data 3).Figure 20
The velocity errors of various algorithms (Data 3).Figure 21
The RMS errors of baseline and velocity of various algorithms for Data 3.Furthermore, the performance of the IBRCAF-AKF algorithm is tested with Data 4. As shown in Figures22 and 23, the baseline error and velocity error of the IBRCAF-AKF algorithm can maintain high precision level in the presence of observation gross errors and state predicted abnormities simultaneously. The smoother error curves are achieved than the other algorithms. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.45 m and 0.06 m/s. The RMS errors of baseline and velocity are 0.056 m and 0.009 m/s. The RMS errors of baseline and velocity of various algorithms are shown in Figure 24. The satisfactory results are owing to two main reasons. For one thing, the high success rate of AR with the IBRCAF algorithm is obtained in complex conditions; for another, the adaptive TC algorithm with correctly fixing ambiguity parameters provides a precision INS-derived position. Hence, the IBRCAF-AKF algorithm is well applicable for relative position in the complex high dynamic environment in rendezvous and following stage in the AAR.Figure 22
The baseline errors of various algorithms (Data 4).Figure 23
The velocity errors of various algorithms (Data 4).Figure 24
The RMS errors of baseline and velocity of various algorithms (Data 4).


## 3.1. Simulation Conditions

In order to verify the feasibility of the algorithm proposed, simulation experiment has been made using Matlab between the tanker and UAV for aerial refueling from rendezvous to following stage. The simulation conditions are set as follows:(1)
Set the gyro error with a constant bias 1°/h. The correlation time constant of random process is 1500 ppm, and driving white noise is 0.01°/h. The correlation time constant of random process for accelerometer is 4000 ppm, and driving white noise is 0.3 mg m/s2. The frequency of INS is 200 Hz(2)
Set the standard deviation of GNSS code pseudorange with 0.37 m, carrier phase with 0.01 m, and Doppler with 0.01 m/s. The frequency of GNSS is 1 Hz. The cutoff degree is 25°(3)
The tanker and UAV fly level from the initial position [118.00014°, 31.73087°, 7010.0] and [117.94924°, 31.75862°, 6913.25] with a velocity of [0, 100 m/s, 0] in the body coordinate frame and attitude of [0, 0, 0] and [0, 0, 90°] in the navigation frame, respectively. The tanker keeps flying level in the designated airspace, while the UAV is approaching from 4.5 km away to 50 m with maneuver flight firstly and then flying level following the tanker. The trajectory parameters of the UAV are shown in Table1. The trajectories of the tanker and the UAV are shown in Figure 3(4)
The GNSS observations are simulated based on B1 frequency with actual BDS satellite ephemeris. The skyplot of visible satellites simulated is shown in Figure4. The Ratio threshold is valued 3 in the success rate reliability testTable 1
The trajectory parameters of the UAV.
Flight phaseFlight actionDuration (s)Acceleration (m/s2)Attitude (°/s) (roll, pitch, yaw angular rate)1Level flight10[0,0,0][0,0,0]2Preturn10[0,0,0][-0.5,0,0]3Turn45[0,0,0][0,0,-2]4Level off10[0,0,0][0.5,0,0]5Level flight65[0,0,0][0,0,0]6Pull up40[0,0.1,0][0,0.01,0]7Climb up80[0,0,0][0,0,0]8Level off40[0,-0.1,0][0,-0.01,0]9Level flight900[0,0,0][0,0,0]Figure 3
The trajectories of the tanker and the UAV.Figure 4
The skyplot of the visible satellites.


## 3.2. Simulation Results and Discussion

### 3.2.1. The Performance of Various Algorithms for AR

In order to evaluate the AR effects with different algorithms, three indicators of ratio value, ADOP value, and empirical success ratePSE are defined, respectively, as follows:
(31)ratio=ΩsecΩmin,where Ωsec and Ωmin are the suboptimal and optimal quadratic form of ambiguity residual determined in LAMBDA.
(32)ADOP=detQX∧a1/m,where QX^a is the covariance matrix of ambiguity floats; m is the number of ambiguity floats.
(33)PSE=numberofcorrectlyfixedepochstotalnumberofepochs.In order to simulate the multipath gross errors caused by the strong reflection effect and state outliers emerged by airflow interference in UAV-tanker proximity, the data is processed as follows to test the various algorithms for AR.Data 1. Original data.Data 2. Gross errors of 5 m, 10 m, 20 m, and 40 m are added to the pseudorange observations randomly to simulate multipath errors at epochs from 200 s to 210 s, 500 s to 510 s, 800 s to 810 s, and 1100s to 1110s, respectively.Data 3. Velocity error of 5 m/s is added to the velocity predicted parameters to simulate kinematic model abnormality at epochs from 400 s to 410 s and 600 s to 610 s.Data 4. Pseudorange observation gross errors of 20 m and velocity errors of 5 m/s are added simultaneously at epochs from 300 s to 310 s and 700 s to 710 s.The following four schemes are performed for AR, respectively.Scheme 1. Weighted least square algorithm (WLS).Scheme 2. Extend Kalman filtering algorithm (EKF).Scheme 3. INS-aided extend Kalman filtering algorithm (IEKF).Scheme 4. INS-aided bifactor robust Kalman filtering algorithm (IBRKF).Scheme 5. INS-aided bifactor robust and classified factors adaptive filtering algorithm (IBRCAF).The test thresholds are taken as the empirical value ofc0=1.5, c1=4.5, and c2=2.0.Firstly, the performances of WLS, EKF, and IEKF algorithms for AR are compared with Data 1. As shown in Figures5 and 6, the efficiency of AR with EKF is better than the WLS method, which is owing to both the system model and observation model established in dynamic conditions. Moreover, as shown in the subgraph of Figure 5, the ratio values of IEKF are greater than the test threshold, whereas they are smaller with WLS and EKF in comparison to IEKF. The ratio means are 4.35, 6.21, and 13.03 for WLS, EKF, and IEKF, respectively. The ratio mean is improved by 109.8% with INS-aided in comparison to without INS-aided. The ADOP value is decreased obviously by INS-aided as well. Those are benefit from the improvement of accuracy of ambiguity floats and reduction of research space of ambiguity. The success rates are 57.5%, 88.1%, and 94.4% of WLS, EKF, and IEKF for AR, respectively, in which the success rate is improved by 7.2% with INS-aided. Hence, the prior INS-derived position provides a useful constraint to enhance the probability of rightly fixing ambiguity. In addition, since the accuracy of ambiguity floats is closely relative to the pseudorange precision and INS-derived position precision, the ADOP value is further analysed with different pseudorange precisions and INS-derived position precisions at epoch 300 s. As shown in Figure 7, the ADOP value increases obviously as the pseudorange precision decreases with the WLS algorithm. However, they increase gently with EKF and IEKF methods, which can obtain higher precision floats in a dynamic environment. As shown in Figure 8, AR performance with IEKF is better than EKF when INS-derived position precision is within 1.5 m. At this moment, the INS-derived position can play a positive role of coordinate constraint. Otherwise, the INS-aided effect becomes worse [27].Figure 5
The ratio values of various algorithms (Data 1).Figure 6
The ADOP values of various algorithms (Data 1).Figure 7
The ADOP values with pseudorange precision (Data 1).Figure 8
The ADOP values with INS-derived position precision (Data 1).Aiming at the multipath outliers in the observations in the UAV-tanker proximity, the WLS, EKF, IEKF, and IBRKF algorithm effects are compared with Data 2. As shown in Figure9, the ratio values reduce greatly at the epochs of the outliers appearing without a robust algorithm so as to decrease the success rate of AR. In the subgraph of Figure 9, it can be seen that the ratio values calculated by the IBRKF algorithm remain larger than the test threshold at the epochs from 800 s to 810 s when 20 m gross error is added, which is obviously superior to the other methods. The same results can be achieved when gross errors of different magnitudes are added. The ratio means are 3.26, 6.11, 9.21, and 12.11, and the success rates are 43.6%, 73.7%, 81.3%, and 94.6% for the four algorithms, respectively.Figure 9
The ratio values of WLS, EKF, IEKF, and IBRKF (Data 2).Aiming at the state predicted abnormities with the maneuvering or airflow interference, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 3. As shown in Figure10, the ratio values decrease significantly at the epochs of the state abnormities emerging except for the IBRCAF algorithm. Furthermore, it can be seen in the subgraph of Figure 10; the IBRCAF algorithm can produce a good adjustment effect at the epochs from 600 s to 610 when there are moderate deviations in the state predicted parameters. The ratio means are 6.98, 7.92, 9.31, and 12.43, and the success rates are 84.0%, 87.5%, 89.2%, and 93.7% for the four algorithms, respectively.Figure 10
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 3).Furthermore, in order to test the full performance of the IBRCAF algorithm for AR, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 4. As shown in Figure11 and its subgraph, the IBRCAF algorithm can keep better performance for AR than other algorithms in the presence of the observation outliers and state predicted abnormities simultaneously. The ratio means are 6.01, 7.31, 8.77, and 12.17, and the success rates are 81.1%, 84.2%, 87.9%, and 93.8% for the four algorithms, respectively.Figure 11
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 4).The ratio means of various schemes for Data 1–4 are shown in Figure12. And the success rates of various algorithms for Data 1–4 are shown in Table 2.Figure 12
The ratio means of various schemes (Data 1–4).Table 2
The success rates of various algorithms for Data 1–4.
SchemesData 1Data 2Data 3Data 4WLS57.543.6——EKF88.173.784.081.1IEKF94.481.387.584.2IBRKF—94.689.287.9IBRCAF——93.793.8“—” means no use of the algorithm for the data.


### 3.2.2. The Performance of Two-Stage Adaptive Filtering for RP

In order to verify the two-stage filtering performance for RP, the following ten algorithms are conducted for RP, respectively. The data are the same with Section3.2.1.Algorithm 1a. Weighted least square for AR and standard Kalman filtering for TC (WLS-SKF).Algorithm 1b. Weighted least square for AR and adaptive Kalman filtering for TC (WLS-AKF).Algorithm 2a. Extend Kalman filtering for AR and standard Kalman filtering for TC (EKF-SKF).Algorithm 2b. Extend Kalman filtering for AR and adaptive Kalman filtering for TC (EKF-AKF).Algorithm 3a. INS-aided extend Kalman filtering for AR and standard Kalman filtering for TC (IEKF-SKF).Algorithm 3b. INS-aided extend Kalman filtering for AR and adaptive Kalman filtering for TC (IEKF-AKF).Algorithm 4a. INS-aided bifactor robust Kalman filtering for AR and standard Kalman filtering for TC (IBRKF-SKF).Algorithm 4b. INS-aided bifactor robust Kalman filtering for AR and adaptive Kalman filtering for TC (IBRKF-AKF).Algorithm 5a. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-SKF).Algorithm 5b. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-AKF).Firstly, Algorithms 1a–3b are adopted for Data 1. As shown in Figures13 and 14, compared with WLS-SKF and EKF-SKF algorithms, the baseline error and velocity error decrease with the IEKF-SKF algorithm, which are owing to the success rate of AR increasing with the IEKF algorithm. Compared with WLS/EKF/IEKF-SKF, the baseline error and velocity error show minor fluctuations with the WLS/EKF/IEKF-AKF algorithms. Particularly, in the presence of maneuvering of UAV in the rendezvous phase, the velocity error increases obviously with the SKF algorithm at epoch of 20 s, 65 s, 180 s, and 300 s due to the changing of flight state parameters. However, the phenomenon is improved greatly with the AKF algorithm, which can adaptively adjust the proportion of the abnormal state estimation by model deviation. The RMS errors of baseline and velocity of various algorithms are shown in Figure 15. Compared with IEKF-SKF, the RMS errors of baseline and velocity with the IEKF-AKF algorithm are improved by 11.11% and 30.76%, respectively.Figure 13
The baseline errors of various algorithms (Data 1).Figure 14
The velocity errors of various algorithms (Data 1).Figure 15
The RMS errors of baseline and velocity of various algorithms (Data 1).Algorithms 1a–4b are utilized for Data 2. As shown in Figures16 and 17, the baseline error and velocity error increase rapidly at the epochs in the presence of gross errors with the WLS/EKF/IEKF algorithms. Among them, the performance of WLS is the worst, which mainly depends on the quality of observations. Furthermore, the errors obviously increase as the magnitude of gross errors increases. However, the IBRKF algorithm has a strong ability to resist the gross errors of different magnitudes. The maximum errors of baseline and velocity of the IBRKF-AKF algorithm are 0.76 m and 0.08 m/s, which maintain good performance in the measurement gross error environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 18.Figure 16
The baseline errors of various algorithms (Data 2).Figure 17
The velocity errors of various algorithms (Data 2).Figure 18
The RMS errors of baseline and velocity of various algorithms (Data 2).Algorithms 2a–5b are used for Data 3. As shown in Figures19 and 20, the baseline error and velocity error increase significantly in the presence of state predicted outliers at epoch of 400~410 s and 600~610 s without adaptive adjustment. However, the IBRCAF-AKF algorithm can effectively adjust the contribution of state outliers and model deviation in the two-stage filter. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.55 m and 0.05 m/s. In addition, it can be found that the IBRKF algorithm is inability for state abnormalities, which is insufficient performance in the high dynamic environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 21.Figure 19
The baseline errors of various algorithms (Data 3).Figure 20
The velocity errors of various algorithms (Data 3).Figure 21
The RMS errors of baseline and velocity of various algorithms for Data 3.Furthermore, the performance of the IBRCAF-AKF algorithm is tested with Data 4. As shown in Figures22 and 23, the baseline error and velocity error of the IBRCAF-AKF algorithm can maintain high precision level in the presence of observation gross errors and state predicted abnormities simultaneously. The smoother error curves are achieved than the other algorithms. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.45 m and 0.06 m/s. The RMS errors of baseline and velocity are 0.056 m and 0.009 m/s. The RMS errors of baseline and velocity of various algorithms are shown in Figure 24. The satisfactory results are owing to two main reasons. For one thing, the high success rate of AR with the IBRCAF algorithm is obtained in complex conditions; for another, the adaptive TC algorithm with correctly fixing ambiguity parameters provides a precision INS-derived position. Hence, the IBRCAF-AKF algorithm is well applicable for relative position in the complex high dynamic environment in rendezvous and following stage in the AAR.Figure 22
The baseline errors of various algorithms (Data 4).Figure 23
The velocity errors of various algorithms (Data 4).Figure 24
The RMS errors of baseline and velocity of various algorithms (Data 4).


## 3.2.1. The Performance of Various Algorithms for AR

In order to evaluate the AR effects with different algorithms, three indicators of ratio value, ADOP value, and empirical success ratePSE are defined, respectively, as follows:
(31)ratio=ΩsecΩmin,where Ωsec and Ωmin are the suboptimal and optimal quadratic form of ambiguity residual determined in LAMBDA.
(32)ADOP=detQX∧a1/m,where QX^a is the covariance matrix of ambiguity floats; m is the number of ambiguity floats.
(33)PSE=numberofcorrectlyfixedepochstotalnumberofepochs.In order to simulate the multipath gross errors caused by the strong reflection effect and state outliers emerged by airflow interference in UAV-tanker proximity, the data is processed as follows to test the various algorithms for AR.Data 1. Original data.Data 2. Gross errors of 5 m, 10 m, 20 m, and 40 m are added to the pseudorange observations randomly to simulate multipath errors at epochs from 200 s to 210 s, 500 s to 510 s, 800 s to 810 s, and 1100s to 1110s, respectively.Data 3. Velocity error of 5 m/s is added to the velocity predicted parameters to simulate kinematic model abnormality at epochs from 400 s to 410 s and 600 s to 610 s.Data 4. Pseudorange observation gross errors of 20 m and velocity errors of 5 m/s are added simultaneously at epochs from 300 s to 310 s and 700 s to 710 s.The following four schemes are performed for AR, respectively.Scheme 1. Weighted least square algorithm (WLS).Scheme 2. Extend Kalman filtering algorithm (EKF).Scheme 3. INS-aided extend Kalman filtering algorithm (IEKF).Scheme 4. INS-aided bifactor robust Kalman filtering algorithm (IBRKF).Scheme 5. INS-aided bifactor robust and classified factors adaptive filtering algorithm (IBRCAF).The test thresholds are taken as the empirical value ofc0=1.5, c1=4.5, and c2=2.0.Firstly, the performances of WLS, EKF, and IEKF algorithms for AR are compared with Data 1. As shown in Figures5 and 6, the efficiency of AR with EKF is better than the WLS method, which is owing to both the system model and observation model established in dynamic conditions. Moreover, as shown in the subgraph of Figure 5, the ratio values of IEKF are greater than the test threshold, whereas they are smaller with WLS and EKF in comparison to IEKF. The ratio means are 4.35, 6.21, and 13.03 for WLS, EKF, and IEKF, respectively. The ratio mean is improved by 109.8% with INS-aided in comparison to without INS-aided. The ADOP value is decreased obviously by INS-aided as well. Those are benefit from the improvement of accuracy of ambiguity floats and reduction of research space of ambiguity. The success rates are 57.5%, 88.1%, and 94.4% of WLS, EKF, and IEKF for AR, respectively, in which the success rate is improved by 7.2% with INS-aided. Hence, the prior INS-derived position provides a useful constraint to enhance the probability of rightly fixing ambiguity. In addition, since the accuracy of ambiguity floats is closely relative to the pseudorange precision and INS-derived position precision, the ADOP value is further analysed with different pseudorange precisions and INS-derived position precisions at epoch 300 s. As shown in Figure 7, the ADOP value increases obviously as the pseudorange precision decreases with the WLS algorithm. However, they increase gently with EKF and IEKF methods, which can obtain higher precision floats in a dynamic environment. As shown in Figure 8, AR performance with IEKF is better than EKF when INS-derived position precision is within 1.5 m. At this moment, the INS-derived position can play a positive role of coordinate constraint. Otherwise, the INS-aided effect becomes worse [27].Figure 5
The ratio values of various algorithms (Data 1).Figure 6
The ADOP values of various algorithms (Data 1).Figure 7
The ADOP values with pseudorange precision (Data 1).Figure 8
The ADOP values with INS-derived position precision (Data 1).Aiming at the multipath outliers in the observations in the UAV-tanker proximity, the WLS, EKF, IEKF, and IBRKF algorithm effects are compared with Data 2. As shown in Figure9, the ratio values reduce greatly at the epochs of the outliers appearing without a robust algorithm so as to decrease the success rate of AR. In the subgraph of Figure 9, it can be seen that the ratio values calculated by the IBRKF algorithm remain larger than the test threshold at the epochs from 800 s to 810 s when 20 m gross error is added, which is obviously superior to the other methods. The same results can be achieved when gross errors of different magnitudes are added. The ratio means are 3.26, 6.11, 9.21, and 12.11, and the success rates are 43.6%, 73.7%, 81.3%, and 94.6% for the four algorithms, respectively.Figure 9
The ratio values of WLS, EKF, IEKF, and IBRKF (Data 2).Aiming at the state predicted abnormities with the maneuvering or airflow interference, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 3. As shown in Figure10, the ratio values decrease significantly at the epochs of the state abnormities emerging except for the IBRCAF algorithm. Furthermore, it can be seen in the subgraph of Figure 10; the IBRCAF algorithm can produce a good adjustment effect at the epochs from 600 s to 610 when there are moderate deviations in the state predicted parameters. The ratio means are 6.98, 7.92, 9.31, and 12.43, and the success rates are 84.0%, 87.5%, 89.2%, and 93.7% for the four algorithms, respectively.Figure 10
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 3).Furthermore, in order to test the full performance of the IBRCAF algorithm for AR, the EKF, IEKF, IBRKF, and IBRCAF algorithm effects are compared with Data 4. As shown in Figure11 and its subgraph, the IBRCAF algorithm can keep better performance for AR than other algorithms in the presence of the observation outliers and state predicted abnormities simultaneously. The ratio means are 6.01, 7.31, 8.77, and 12.17, and the success rates are 81.1%, 84.2%, 87.9%, and 93.8% for the four algorithms, respectively.Figure 11
The ratio values of EKF, IEKF, IBRKF, and IBRCAF (Data 4).The ratio means of various schemes for Data 1–4 are shown in Figure12. And the success rates of various algorithms for Data 1–4 are shown in Table 2.Figure 12
The ratio means of various schemes (Data 1–4).Table 2
The success rates of various algorithms for Data 1–4.
SchemesData 1Data 2Data 3Data 4WLS57.543.6——EKF88.173.784.081.1IEKF94.481.387.584.2IBRKF—94.689.287.9IBRCAF——93.793.8“—” means no use of the algorithm for the data.


## 3.2.2. The Performance of Two-Stage Adaptive Filtering for RP

In order to verify the two-stage filtering performance for RP, the following ten algorithms are conducted for RP, respectively. The data are the same with Section3.2.1.Algorithm 1a. Weighted least square for AR and standard Kalman filtering for TC (WLS-SKF).Algorithm 1b. Weighted least square for AR and adaptive Kalman filtering for TC (WLS-AKF).Algorithm 2a. Extend Kalman filtering for AR and standard Kalman filtering for TC (EKF-SKF).Algorithm 2b. Extend Kalman filtering for AR and adaptive Kalman filtering for TC (EKF-AKF).Algorithm 3a. INS-aided extend Kalman filtering for AR and standard Kalman filtering for TC (IEKF-SKF).Algorithm 3b. INS-aided extend Kalman filtering for AR and adaptive Kalman filtering for TC (IEKF-AKF).Algorithm 4a. INS-aided bifactor robust Kalman filtering for AR and standard Kalman filtering for TC (IBRKF-SKF).Algorithm 4b. INS-aided bifactor robust Kalman filtering for AR and adaptive Kalman filtering for TC (IBRKF-AKF).Algorithm 5a. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-SKF).Algorithm 5b. INS-aided bifactor robust and classified factor adaptive filtering for AR and standard Kalman filtering for TC (IBRCAF-AKF).Firstly, Algorithms 1a–3b are adopted for Data 1. As shown in Figures13 and 14, compared with WLS-SKF and EKF-SKF algorithms, the baseline error and velocity error decrease with the IEKF-SKF algorithm, which are owing to the success rate of AR increasing with the IEKF algorithm. Compared with WLS/EKF/IEKF-SKF, the baseline error and velocity error show minor fluctuations with the WLS/EKF/IEKF-AKF algorithms. Particularly, in the presence of maneuvering of UAV in the rendezvous phase, the velocity error increases obviously with the SKF algorithm at epoch of 20 s, 65 s, 180 s, and 300 s due to the changing of flight state parameters. However, the phenomenon is improved greatly with the AKF algorithm, which can adaptively adjust the proportion of the abnormal state estimation by model deviation. The RMS errors of baseline and velocity of various algorithms are shown in Figure 15. Compared with IEKF-SKF, the RMS errors of baseline and velocity with the IEKF-AKF algorithm are improved by 11.11% and 30.76%, respectively.Figure 13
The baseline errors of various algorithms (Data 1).Figure 14
The velocity errors of various algorithms (Data 1).Figure 15
The RMS errors of baseline and velocity of various algorithms (Data 1).Algorithms 1a–4b are utilized for Data 2. As shown in Figures16 and 17, the baseline error and velocity error increase rapidly at the epochs in the presence of gross errors with the WLS/EKF/IEKF algorithms. Among them, the performance of WLS is the worst, which mainly depends on the quality of observations. Furthermore, the errors obviously increase as the magnitude of gross errors increases. However, the IBRKF algorithm has a strong ability to resist the gross errors of different magnitudes. The maximum errors of baseline and velocity of the IBRKF-AKF algorithm are 0.76 m and 0.08 m/s, which maintain good performance in the measurement gross error environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 18.Figure 16
The baseline errors of various algorithms (Data 2).Figure 17
The velocity errors of various algorithms (Data 2).Figure 18
The RMS errors of baseline and velocity of various algorithms (Data 2).Algorithms 2a–5b are used for Data 3. As shown in Figures19 and 20, the baseline error and velocity error increase significantly in the presence of state predicted outliers at epoch of 400~410 s and 600~610 s without adaptive adjustment. However, the IBRCAF-AKF algorithm can effectively adjust the contribution of state outliers and model deviation in the two-stage filter. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.55 m and 0.05 m/s. In addition, it can be found that the IBRKF algorithm is inability for state abnormalities, which is insufficient performance in the high dynamic environment. The RMS errors of baseline and velocity of various algorithms are shown in Figure 21.Figure 19
The baseline errors of various algorithms (Data 3).Figure 20
The velocity errors of various algorithms (Data 3).Figure 21
The RMS errors of baseline and velocity of various algorithms for Data 3.Furthermore, the performance of the IBRCAF-AKF algorithm is tested with Data 4. As shown in Figures22 and 23, the baseline error and velocity error of the IBRCAF-AKF algorithm can maintain high precision level in the presence of observation gross errors and state predicted abnormities simultaneously. The smoother error curves are achieved than the other algorithms. The maximum errors of baseline and velocity of the IBRCAF-AKF algorithm are 0.45 m and 0.06 m/s. The RMS errors of baseline and velocity are 0.056 m and 0.009 m/s. The RMS errors of baseline and velocity of various algorithms are shown in Figure 24. The satisfactory results are owing to two main reasons. For one thing, the high success rate of AR with the IBRCAF algorithm is obtained in complex conditions; for another, the adaptive TC algorithm with correctly fixing ambiguity parameters provides a precision INS-derived position. Hence, the IBRCAF-AKF algorithm is well applicable for relative position in the complex high dynamic environment in rendezvous and following stage in the AAR.Figure 22
The baseline errors of various algorithms (Data 4).Figure 23
The velocity errors of various algorithms (Data 4).Figure 24
The RMS errors of baseline and velocity of various algorithms (Data 4).


## 4. Conclusion

High precision relative navigation based on the GNSS/INS integrated system is one of the key technologies for AAR mission. A reliable and continuous AR algorithm is the prerequisite for high-precision positioning using carrier phase observations. However, the ambiguity fixing efficiency is greatly affected by code pseudorange multipath observation gross errors and model state abnormal disturbances during following stage, whose accuracy and reliability are critical for docking mission. In addition, short-term high-precision INS-derived position fused by the GNSS/INS TC integrated system can provide favourable prior knowledge for AR. Moreover, the parameter estimation method of the GNSS/INS TC integrated system is important for the precision of INS-derived position as well. Therefore, a novel two-stage adaptive filtering-based GNSS/INS TC precision relative navigation algorithm for AAR is studied in this paper. Particularly, an IBRCAF algorithm for AR is developed. According to the theoretical analysis and numerical simulation results, the following conclusions can be drawn:(1)
Compared with WLS and EKF, the IEKF algorithm can decrease the ambiguity search space and improve the accuracy of ambiguity floats effectively so as to raise the success rate of AR. The success rate of the IEKF algorithm without outliers is improved by 64.2% and 7.2% compared with WLS and EKF, respectively. Hence, the INS-derived position can provide a helpful prior coordinate constraint for AR. However, the INS-aided effect reduces as the INS-derived position precision decreases(2)
Compared with WLS, EKF, and IEKF, the IBRKF algorithm can suppress the influence of observation gross errors, except for state-predicted outliers. However, the IBRCAF algorithm can reduce the influence of observation outliers and state predicted abnormalities simultaneously, which is superior to the other methods in a UAV-tanker proximity environment(3)
Compared with IEKF-SKF, the IEKF-AKF algorithm can obtain smoother error curves with higher precision of baseline and velocity results, which can adaptively adjust the model predicted state deviation. The means of baseline and velocity errors of the IEKF-AKF algorithm are within 10 cm and 2 cm/s under normal conditions(4)
Compared with IEKF-AKF and IBRKF-AKF, the IBRCAF-AKF algorithm can improve the relative positioning accuracy significantly in a complex high dynamic environment in the rendezvous and following stages in AAR. It obtains centimeter-level accuracy, which provides a favourable condition for further docking mission in AARWith the development of the GNSS system and sensors, a multifrequency, multiconstellation, and multisensor fusion method may provide a new idea for AAR; the future work will focus on the further study of the GNSS/INS/MV integrated navigation and high-precision data fusion algorithm for the whole AAR mission.



---

*Source: 8546466-2022-01-31.xml*