An Improved Real-Time Transfer Alignment Algorithm Based on Adaptive Noise Estimation for Distributed POS

Distributed position and orientation system (POS) plays an important role in the fields of aerial remote sensing, which serves the sensors by precise motion information. For distributed POS, the slave systems consist of low accuracy inertial sensors, which must depend on the high accuracy motion information of the master to proceed transfer alignment (TA) to improve accuracy. Generally, the TA filtering algorithms perform superior performance when the noise statistical characteristic is known and accurate, however like gust, engine vibration and other external disturbances both will cause the inertial sensors output with unknown, varying noises and performance decline. Aiming at this, a variational Bayesian central difference Kalman filtering algorithm for distributed POS real-time TA is developed to suppress the effect of external noise, incorporating the central difference Kalman filtering (CDKF) algorithm and variational Bayesian (VB) adaption theory. In detail, the algorithm is to apply the noise estimation by VB adaption to the CDKF algorithm to achieve real-time TA accuracy enhancement. Distributed POS flight test is devoted for the algorithm validation, by the comparison, evident progress in TA accuracy has been presented.


I. INTRODUCTION
Acoording to the researches of the past few years, multitask remote sensing loads have gradually become the main trends of aerial remote sensing imaging, such as airborne distributed array antenna SAR and flexible baseline interferometric SAR (InSAR), etc. [1], [2]. And in order to achieve high performance of aerial remote sensing system, it is necessary to obtain high accuracy multi-node motion parameters information of each load's location. Distributed position and orientation system (POS) is an effective means to present multi-node motion information with high accuracy, which has become one of the key equipment of aerial remote sensing system [3], [4].
Due to the restriction of space and weight, the distributed POS mainly contains a master POS and several low accuracy slave inertial measurement units (IMUs), where the master POS (master system) combines inertial navigation system The associate editor coordinating the review of this manuscript and approving it for publication was Chunsheng Zhu .
(INS)/global positioning system (GPS) [5], and the slave systems are only IMUs. As the typical application mentioned above, the master POS is installed on the middle of the aircraft, the slave IMUs are installed rigidly with the SAR antennas and depend on TA to obtain high accuracy motion parameters of sub-nodes by using accurate motion information of master POS, including position, velocity and attitude, etc. [6].
In essential, the distributed POS is a nonlinear system, then nonlinear filtering is used to proceed the TA estimation. For nonlinear filtering algorithms, developed from the fundamental Kalman filter which is originally proposed to obtain the recursive minimum variance estimation for linear state-space system with Gaussian white noise [7], the extended Kalman filter is formed by utilizing Taylor series approximations to the non-linearities and conducts the Kalman filtering algorithm to obtain the state estimation [8]. However, the extended Kalman filter algorithm applies the first-order Taylor-series expansion to linearize the nonlinear problems, which cannot satisfy the accuracy requirements in strong nonlinear VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ environments [9]. Further, several deterministic sampling methods based nonlinear filters like unscented Kalman filter [10], central difference Kalman filter [11], which both belong to the category of Bayesian filter and Gaussian filter, have been proposed to use a series of deterministic sampling points to deduce the propagation of system nonlinearity, and then obtain the posterior distribution characteristic, which has been widely used in nonlinear filtering problems. Because less parameter is used to proceed the algorithm and the same accuracy level with unscented Kalman filter, central difference Kalman filter is focused in this work. Since state estimation implies an integration of a noisy signal and the above nonlinear filtering algorithms both achieve superior performance under the restriction that noise statistical characteristic is known and accurate, but in the real circumstances, external disturbances like gust, engine vibration both will result that the inertial sensors outputs with unknown and varying noise, which can not be described accurately, and inappropriate noise parameters will cause that filtering performance decline. Aiming at this problem, noise estimation methods have been discussed, like scaling factor adjustment method [12], innovation-based adaptive estimation [13], [14] and so on. However, these methods both use the vectors of the last m epoch to get the next estimation, which sets high requirements for storage and calculation [15].
Developed on the basis of VB learning, VB adaption method is realized by calculating the joint probability density of the state variable and parameter to be estimated using the variational method [16]. Specially, the process is implemented by two new distributions to conduct approximation and minimize the Kullback-Leibler (KL) divergence [18]. By these operations, the noise statistical characteristic can be given recursively, which has been used in many related fields like tracking a moving target or an unknown signal [17], [18] and so on. And compared with the above methods, it has the following advantages: 1) For VB adaption, the estimation results of the last moment is to deduce the next step estimation, which has less computational burden, also decreases compute quantity.
2) VB adaption is immune to the storage of vectors used for the next step and search for proper windows width, which possesses higher algorithm efficiency.
For nonlinear filtering algorithm, the state estimation process proceeds in performance under that measurement noise is known and accurate, but for the real case, the disturbances will result that measurement noise is unknown and varying and then filtering performance decline. For this sake, the VB adaption method is introduced for measurement noise adaptive estimation to achieve performance improvement and a VB based postprocessing algorithm has been used in [19]. Up to now, combination of the VB method and nonlinear Gaussian filtering algorithm has been studied by many researchers, such as [20]- [24], which has achieved better performance. Also, some noise adaptive estimation by variational Bayesian or expectation-maximization method together with filtering algorithm has been concerned by researchers [25]- [27].
In this work, a variational Bayesian central difference Kalman filtering (VB-CDKF) algorithm for distributed POS realtime TA is developed to use the adaptive measurement noise covariance estimation to conduct the CDKF to achieve performance improvement, the contributions of this paper are as follows: 1) VB adaption method is used to substitute the traditional adaptive method to estimate the measurement noise for distributed POS TA, which only uses the estimation of last moment to estimate the measurement noise covariance adaptively, and is free of vectors storage and searching proper windows width.
2) CDKF algorithm is employed to conduct the state estimation of distributed POS TA, which uses a series of deterministic sampling points to obtain the state estimation and do not require the calculation of Jacobian matrix, also possesses less parameter to proceed the estimation but same estimation accuracy with unscented Kalman filter.
3) By using the adaptive measurement noise estimation to replace the constant value to proceed the CDKF algorithm, performance enhancement can be realized. After the experiment validation based on flight data, the result shows that the proposed algorithm can estimate the measurement noise effectively and noticeable motion parameter accuracy improvement has achieved, and further will enhance the performance of aerial remote sensing system.
For the reminder, Section II displays the VB-CDKF algorithm flow by applying the VB adaption method to CDKF algorithm; In section III, distributed POS flight test and algorithm validation are given respectively; The last section concludes the whole work.

II. VB-CDKF ALGORITHM
In this section, we will firstly introduce the algorithm flows of CDKF and VB adaption, then combine the basic framework of CDKF algorithm and the measurement noise covariance adaptive estimation by VB adaption, the VB-CDKF algorithm is proposed to achieve real-time TA performance improvement. Given nonlinear system with state x k and measurement z k as where f (·) is the state function, G k is the process noise coefficient matrix, H k is the measurement matrix, w k , v k are mutually independent noise vectors with zero mean and Broadly speaking, general approaches to optimal filtering for state estimation of a stochastic dynamical system can be formulated by the way of recursive Bayesian approach, or Bayesian filtering [28]. In most cases, to obtain the analytical solutions of Bayesian filtering, the Gaussian distribution assumption is used to simply the recursive process as Gaussian filtering (GF). Among of this, central difference Kalman filtering [11] is formulated based on the Stirling's interpolation formula to numerically solve the GF equation.
For n-dimensional stochastic variable x with meanx and variance P and nonlinear function y = f (x), where mean and variance of y areȳ and P y , covariance P xy . Define first-order and second-order operator Df and D 2 f as and e i are the unit vectors, υ = √ 3 is the scalar central difference step size. Then second order approximation of function f can be formulated as then one can obtain that where S i denotes the ith column of S, andf (z ± υe i ) = f (x ± υS i ). Based on the above description, the CDKF algorithm process will be presented.
As an effective way to estimate noise characteristics adaptively, VB adaption [16] is implemented by deducing the recursive solution of the state and noise statistical distribution, can also be used for state and measurement noise as Apply the VB adaption to nonlinear filtering, detailed algorithm flow for system (1) is depicted as: Step 1: At time k − 1, the density p(x k−1 |Z k−1 ) = N(x k−1 , P k−1 ) holds for the state estimation, wherex represents Gaussian distribution with meanx and covariance matrix P. Also, given measurement Z k−1 , the following equa- [16], A, B are the parameters of the Inverse-Wishart distribution.
Step 2: Generate the sigma-points χ i,k−1 at time k − 1 according to where . . , n, S k−1 is the Cholesky decomposition of P k−1 . Then calculate using the state function where − 1 4υ 2 , which can be obtained from (8) and (9), also (14) can be deduced from (9). And p(x k |Z k−1 ) is given by N(x − k , P − k ). Also, according to VB adaption, the distribution p(x k , k |Z k−1 ) can be formulated as the following equality where Step 4: Regenerate sigma-points i,k at time k as where [·||·] kl measures the difference between two probability distributions. By calculating the variations, it has that [16] q( Step 5: Use measurement y k to the update process and obtain p(x k |Z k ) as N(x k , P k ). Utilize the measurement model x k , P k can be deduced by the iteration process (j = 0, 1, 2, . . . , N ): k are obtained for the next recursion.
Here we select the univariate non-stationary growth model (UNGM) which has been used in many existing literatures to design the simulation to verify the effectiveness of the proposed method. Given the UNGM model as where w k−1 is the process noise with zero mean and covariance 1, v k is the measurement noise with zero mean and covariance R k . And initial state x 0 ∼ N (x; 1, 1), design the true measurement noise covariance R k as follows: k < 30, R k = 0.1; 30 < k < 80, R k = 0.2; k > 80, R k = 0.1(k : 0 ∼ 100). Root mean square error (RMSE) of the two filtering result (50 runs) is shown in FIGURE 1. It can be seen from the figure that the proposed method performs better than traditional CDKF, which uses the adaptive measurement noise estimation to achieve accuracy improvement.

III. DISTRIBUTED POS EXPERIMENT VALIDATION
In order to validate the effectiveness of the proposed algorithm, distributed POS flight test with SAR imaging is also conducted. Here we firstly present the distributed POS TA description, then the flight test and results discussion is followed.

A. DISTRIBUTED POS TA DESCRIPTION
Before the distributed POS real-time TA model establishment, some frames are presented as: the east-north-up frame is selected as navigation frame (n frame), i, p, s, e denote the inertial frame, platform frame, slave system body frame, the earth-fixed frame, respectively. For inertial navigation system, large misalignment angles will result that attitude error equation and velocity error equation contains the nonlinear terms, then the state equation is nonlinear, but differences of velocity and attitude between the master POS and slave IMU are used to establish the measurement, which not contains the nonlinear terms, then the measurement model is linear.
Firstly, state equation can be depicted asẋ T is zero mean white noise with covariance Q, where w s ε and w s ∇ are the random noises of inertial sensors, ξ i (i = x, y, z) denotes the Gaussian white noise. Detailed formula of process noise matrix G is omitted here.
The nonlinear function f is decided by the models as follows [30]: where the symbols ω n in , ω n ie , ω n en can be found in related literatures, and δω n in , δω n ie , δω n en are the errors, respectively. f s is the accelerometers data output. C p n is the direction cosine matrices between n frame p frame, where C n p = (C p n ) T . Let µ i and ζ i denote the misalignment angles and flexure angles, then the Markov model is to characterize the flexure lever arm effect between the master and slave system, then the misalignment angle and flexure angle between the master and slave system along the axes x, y, z of slave system can be estimated [31]: where parameters η i = 2.146/τ i , τ i is the correlation time, ξ i is noise vector with variance Q ξ i = 4η 3 i ν 2 i . Based on the model, formulate the flexible lever arm r as the sum of initial value and variations r = r 0 + r, where r =   r z ζ y − r y ζ z r x ζ z − r z ζ x r y ζ x − r x ζ y   and r x , r y , r z are the components of r 0 .
By velocity and attitude matching, the measurement is z = [δφ δθ δγ δV E δV N δV U ] T , which is composed by the attitude and velocity differences of the master and slave system, then z = Hx +v, where v is the noise vector with zero mean, covariance and H is and the elements of H can be found in related literatures.

B. EXPERIMENT VALIDATION 1) EXPERIMENT EQUIPMENT
The flight experiment equipment includes the flight plane, SAR, distributed POS developed by Beihang University, and the distributed POS is composed of a master POS as the master system and two slave IMUs as the slave systems, a distributed navigation processing computer system (DPCS). Specially, the master POS consists of a fiber optic gyroscope based IMU, Novatel GPS receiver (mobile station and base  station), and the slave systems consist of the same IMUs. For the IMU, the gyroscope and accelerometer bias are 0.01 • /h and 20µg, the random noises of gyroscope and accelerometer are also 0.01 • /h and 20µg, respectively. The master POS is installed in the cabin of plane abdomen and the two slave systems are symmetrically distributed on both sides of the plane and mounted rigidly near the SAR antenna, which can provide angular velocity and linear acceleration measurement of their location. Also, the DPCS is placed in the cabin and used to record and solve the motion information of the master and slave systems. In addition, the mobile antenna of GPS is fixed on the top of the aircraft and the base station equipment of GPS is fixed on a relatively open place. Detailed experiment platform and the sensors are shown in FIGURE 2, where (a) and (b) denote the master IMU and slave IMU, respectively, (c) is the GPS base station and RTK transmitting antenna, (d) is the flight airplane. After installation, the plane experiences the stages of initial alignment, taking off, imaging task and landing on the airdrome in 4h, which can be shown in FIGURE 3, where the red part denotes the imaging area and the arrows denote the direction. VOLUME 8, 2020 2) VALIDATION ARRANGEMENT The left system is used to validate the algorithm due to the similarity of the two slave systems. By calibration using the Leica laser total station, the lever arm between the left slave IMU and master POS is r 0 = [1.5214 3.4452 − 0.0851] T (m) in master POS body frame after the sensors are fixed. Considering that the flexible deformation occurs mainly around the y-axis (the plane body direction), the parameters to describe the flexible deformation are selected as τ x = τ z = 2, τ y = 10 and ν x = ν z = 0.01, ν y = 0.1. By utilizing the parameters, the simulation of flexure angles ζ i and angles derivativesζ i are obtained to calculate the lever arm. In order to adapt the development of distributed POS which consists of low accuracy slave IMUs for SAR imaging, process the slave system data by simulated flexure angles and angles derivatives, gyroscope drift and accelerometers bias are added to decrease the accuracy to 0.1 • /h and 100µg, then the data is used to the validation of next section.

3) EXPERIMENT DISCUSSION
By conducting transformation to the INS/GPS results of the master node, including misalignment transformation and above flexure model, the validation reference is obtained, and a 600s section of the flight process is selected for validation. Since the different spatial positions of master POS and slave IMU, we use high accuracy calibration and the information of the master system to obtain the attitude reference of slave IMU. Firstly, the relative attitude between master POS and slave IMU can be calibrated by Leica laser station; then use the second order Markov model to describe the flexure deformation; finally, transform the high accuracy attitude information by Markov model and the calibration result to obtain the accurate attitude reference to slave IMU.
Results of CDKF algorithm, method in [20], method in [23] and the proposed algorithm are both displayed, where the method in [20] views the measurement noise as t distribution (heavy-tailed measurement noise) to utilize the variational Bayesian method (VB filter for the non-linear model with Student t distributed measurement noise), method in [23] uses the inverse Gamma distribution to deal with measurement noise except the inverse Wishart distribution of our work, as shown in FIGURE 4-FIGURE 6. FIGURE 4 presents the heading angle, pitch angle, roll angle error comparison of the several algorithms, for heading angle estimation, the proposed VB-CDKF algorithm is obviously higher than CDKF algorithm, also performs better than method in [20] and [23], meanwhile the pitch angle and roll angle estimation are both enhanced. Also, the heading angle error has a convergence process which can be found in related literatures, here lasts around 100s. One can see from figure that the heading angle convergence rate estimated by VB-CDKF algorithm is faster than the CDKF algorithm and method in [20] and [23]. FIGURE 5 shows the velocity error comparison estimated by the CDKF, method in [20], [23] and proposed VB-CDKF algorithms, where east velocity,  north velocity and up velocity estimations have achieved noticeable improvement by using the proposed estimation algorithm. Accordingly, the latitude error, longitude error and height error given by the several algorithms is displayed in FIGURE 6, which also achieves improvement compared with the CDKF algorithm and the method in [20], [23]. Then one can see that the VB adaption can relieve the influence of measurement noise mismatch on state estimation effectively compared with the CDKF algorithm which proceeds under that measurement noise covariance is fixed, also performs better than the method in [20] which deals with the measurement noise as t distribution, and method in [23] as the inverse Gamma distribution.
Take the mean square error root as the accuracy criterion, error statistics are listed in TABLE 1, and it can be learned that compared with the traditional CDKF algorithm, the attitude, velocity, position accuracy are both improved by applying the proposed method, also the proposed method outperforms than the method in [20] and [23]. One can see from the table that compared with the traditional CDKF algorithm, heading angle, pitch angle and roll angle estimation  [20], [23] and Proposed algorithms. errors have reduced from 0.1356 • , 0.0188 • and 0.0107 • to 0.1136 • , 0.0174 • and 0.0102 • , which improvements are 16.2%, 7.4%, 4.7%, respectively. The east, north and up velocity errors have reduced from 0.0072m/s, 0.0075m/s and 0.0176m/s to 0.0031m/s 0.0042m/s and 0.0152m/s, which achieve 56.9%, 44.0%, 13.6% enhancement, respectively. Meanwhile, the latitude, longitude and height errors have reduced from 0.1028m, 0.1357m and 0.1898m to 0.0648m, 0.0619m, 0.1823m and improvement are 37.0%, 54.3%, 4.0%, respectively. In summary, the distributed POS motion parameter errors are both reduced by the application of proposed method.
Different from [24], the whole flight experiment includes the taking off, climbing section, imaging section (contains several laps) and landing, the algorithm developed in the manuscript is devoted for aerial remote sensing imaging (imaging section), and in the experiment, the plane is flying around the imaging zone for several laps which are same for distributed POS and each lap lasts 600s. In this work, we use one lap data to verify the effectiveness of the proposed algorithm, which can show the achievement of accuracy improvement. And in the future, some more complex maneuvers in imaging section will be taken into consideration to verify the proposed method.
Here comparison between the proposed method with some related existing work such as ''A novel adaptive Kalman filter with inaccurate process and measurement noise covariance matrices'', ''A new adaptive extended Kalman filter for cooperative localization'', ''A novel robust Student's t based Kalman filter'' has been given. For the first, variational Bayesian adaptive Kalman filter with inaccurate process noise covariance matrix and measurement noise covariance matrix is proposed and applied to the problem of target tracking, the manuscript discusses the variational Bayesian central difference Kalman filter to deal with inaccurate measurement noise covariance matrix and applies to transfer alignment of distributed POS. For the second, a new adaptive extended Kalman filter used for cooperative localization is proposed by estimating the predicted error covariance matrix and measurement noise covariance matrix adaptively based on an online expectation-maximization approach, a different method to estimate the measurement noise covariance matrix compared with the manuscript. For the last, a robust Student's t based Kalman filter with both heavy-tailed process and measurement noises is proposed by using the variational Bayesian approach, where the one-step pdf and posterior pdf are viewed as Student's t distribution, different from the manuscript. Compared with the proposed method, the first and last are both linear model with variational Bayesian adaption method, the other uses expectation-maximization to deal with measurement noise, then we select the method of [20] to conduct experiment comparison with the proposed method. On the other hand, for some latest related works, such as [22]- [24], discussion between these methods and the proposed is also presented. In [22], a noise adaptive variational Bayesian cubature information filter (based on Wishart distribution or inverse Gamma distribution) to deal with measurement noise covariance estimation is proposed and applied in target tracking scenario; for [23], a class of nonlinear filtering algorithms based on variational Bayesian theory is studied to settle the unknown measurement noise problem in target tracking system, including the VB-EKF (Algorithm 3, based on inverse Gamma distribution) and VB-UKF (Algorithm 5, based on inverse Wishart distribution) algorithms; and [24] proposes an adaptive cubature Kalman filter based on variational Bayesian method based on inverse Wishart distribution to improve the accuracy of initial alignment. In view of that the variational Bayesian adaption based on inverse Wishart distribution has been considered in this work and consider the algorithms of [22]- [24], the nonlinear filtering algorithm based on inverse Gamma distribution (which is shown as method in [23]) is also selected to give comparison with the proposed method, which the results are added and discussed in our work.
In this work, the proposed algorithm is processed online and devoted to distributed POS real-time application. For CDKF algorithm, the computational cost is cubic with the state dimensionality of the system and the dimensionality of measurement [32]; and the computational cost of VB-CDKF algorithm is also cubic with the state dimensionality of the system and the dimensionality of measurement. On the other hand, for time performance, the VB-CDKF takes more time to conduct estimation, but achieves higher accuracy by using estimated measurement noise parameters. In the current implementation, the CDKF algorithm is faster than VB-CDKF, where the later takes more time to estimate the measurement noise adaptively on MATLAB 2015b. In the future, algorithm speed will be optimized in the way of C++ which can be used in real-time achievement, which can meet the requirement of attitude information to the aerial remote sensing system.

IV. CONCLUSION
For distributed POS real-time TA, a noise estimation aided CDKF algorithm has been proposed, integrating the CDKF algorithm and VB adaption to improve the parameters accuracy of distributed POS. It has been shown that how the VB adaption is used to adapt the unknown and varying noise to improve the performance of nonlinear CDKF algorithm. Based on distributed POS flight data, a semi-physical test is used to demonstrate the effectiveness of the proposed algorithm. Results show that the proposed algorithm can deal with unknown and varying measurement noise effectively, and higher accuracy motion information can be obtained compared with the CDKF algorithm, which can promote the distributed POS real-time TA performance.
In this work, three high accuracy IMU are used in the flight experiment, and by many reasons, the experiment with low accuracy IMU has not been conducted up to now. In the next researches, the accuracy of SINS will be better designed, the experiment with low accuracy slave system will be implemented to show the effectiveness of the proposed method, and more accurate lever arm deformation description (such as external means to achieve high accuracy measurement by direct measurement) will be established to improve the accuracy of distributed POS real-time TA, which can be used to enhance the development of aerial remote sensing technology. Also, we will apply the developed approach to solve various engineering issues such as sensor networks mentioned in [33] to achieve performance improvement in the future.