# A New Triple Filtering Algorithm and Its Application for Aerial GNSS/INS-Integrated Direct Georeferencing System

Authors: Qusen Chen; Leilei Li; Keyi Xu; Xiangdong An; Yu Wu
Journal: Journal of Sensors
 (2021)

---

## Abstract

A global navigation satellite system and inertial navigation system- (GNSS/INS-) integrated system is employed to provide direct georeferencing (DG) in aerial photogrammetry. However, GNSS/INS suffers from stochastic error, strong nonlinearity, and weak observability problems in high dynamic or less maneuver scenarios. In this paper, we proposed a new triple filtering algorithm for aerial GNSS/INS integration. The new algorithm implements filtering in the sequence of forward, backward, and forward directions. Each filter is initialized by a previous filter to get a quick convergence, and the final result is combination of the last two filtering to smooth error. The proposed triple filtering strategy avoids inaccuracy in the 1st forward filtering when the system has not reached convergence. Moreover, it facilitates engineering implementation because backward filtering can employ the same equations with forward filtering. To assess stochastic error of the inertial measurement unit, the Allan variance method is used and abbreviated stochastic model is built. A real aerial testing is conducted, and the result indicates that DG can achieve horizontal accuracy of 5 cm by the proposed algorithm, which has 63% improvement compared to standard extended Kalman filter.


---

## Body

## 1. Introduction

Aerial photogrammetry performs surveying and mapping by taking images of ground from an elevated position. The images could be collected by sensors like digital cameras, Lidar, and multispectral or hyperspectral scanners, to make digital orthophoto map (DOM), digital elevation model (DEM), digital line graphic (DLG), and other mapping products. Before mapping, the exterior orientation elements (positions and attitude) of every image need to be known when taking images. Normally, these elements are resolved by ground control points and aerial triangulation (AT) algorithm. With the aid of a GNSS/INS-integrated system, which works as direct georeferencing (DG), the exterior orientation elements could be directly measured [1, 2]. Thus, the ground control points can be considerably reduced or even entirely eliminated.Generally, differential GNSS is used to get 3-dimensional coordinates with centimeter-level precision [3, 4], while inertial measurement unit (IMU) in aerial photogrammetry application is commonly of tactical grade, consisting of three close loop fiber optic gyros (FOG) with 0.1 ~ 10°/h bias and three quartz accelerometers with 0.1 ~ 1 mg bias. The primary error sources of GNSS/INS integration in aerial application are sensor stochastic error, strong nonlinearity, and weak observability, among which sensors’ stochastic errors are very intractable as many types of stochastic processes are involved. They cause accumulative positioning and orientation error with time. Thus, the implementation of GNSS/INS-integrated system requires careful and accurate error modeling. On the other hand, it is unreasonable to model all the stochastic processes since their observability will significantly decrease in GNSS denied environment and deficient maneuver scenarios, which consequently results in system instability. In most cases, abbreviated stochastic processes are accounted to approximate real situation in the integration model, while others are ignored or merged to existing error [5]. These approximate models would make the system performance worse, especially in high kinematic scenarios.Inertial navigation is a typical nonlinear system, and the nonlinearity of INS will be strengthened when vehicle makes rapid maneuver such as turning, acceleration, and deceleration. Classical extended Kalman filter (EKF) takes linearization to the nonlinear model and implements the linear KF algorithm for estimation of the state vector; thus, it has been extensively used in many engineering applications. However, it suffers from strong nonlinearity, such as large maneuver or environment influence. For these cases, some other nonlinear filters are carried out to solve the nonlinearity problem, such as iterated EKF [6, 7], unscented Kalman filter (UKF) [8, 9], particle filter (PF) [10–12], and neural network method [13, 14], among which iterated EKF and UKF are effective for a nonlinear model, but they are on the assumption of Gaussian stochastic process. PF and neural network can deal well with both nonlinearity and inaccurate model problem. However, these nonlinear filters are not valid to improve observability.Weak observability is another problem in aerial GNSS/INS integration, which often happens when GNSS signal is challenged or the vehicle is lack of maneuver. The former is mainly caused by signal interference, ionosphere scintillation, or signal blockage by inclined airframe when making U-turn, while the latter is due to the rarely changed flight state in aerial photogrammetry. The plane requires to fly with straight and parallel air routes to capture images of ground and make a U-turn while switching to the adjacent air route. Long straight and steady flying lead to decrease of heading observability, so that heading accuracy is degraded in each straight air route. The theoretical scheme to solve this heading observability reduction is to shorten the straight routes and make more turning, but it will clearly decrease the mapping efficiency. In practical processing, a back propagation model is always applied to estimate system states since aerial photography does not require real-time processing in most cases. Different from the standard forward model, the back propagation model has good observability at the end of straight route or GNSS outage, but weak observability at the beginning. The combination of the forward and backward propagation model leads to the smoothing EKF. Several smoothers can be found in the literature [15], in which the most used are forward-backward (FB) smoother [16, 17], Rauch–Tung–Striebel (RTS) smoother [18, 19], and their modifications. FB smoother needs to define new variable to avoid matrix inversion and provide a valid boundary initialization. It employs the information form of EKF instead of the standard EKF for backward filtering. By comparison, RTS does not require full backward EKF that results in good computational efficiency. However, the computation is not of first priority in posttask. Meanwhile, RTS also requires a different form of EKF for backward filtering. Moreover, the attitude is not accurate at the beginning of forward filtering for both FB smoother and RTS, because the initial attitude is obtained by coarse alignment, which will deteriorate the combined result in spite of being averaged by backward filtering.In this research, we present a new triple filtering algorithm for aerial GNSS/INS-integrated DG. Firstly, stochastic processes are investigated by the Allan variance method. The state equation of EKF is built including the navigation error propagation model and abbreviated stochastic model of IMU. Secondly, a forward-backward-forward EKF (FBF-EKF) is proposed to address the inaccurate model, strong nonlinearity, and weak observability problems. Specifically, this algorithm implements triple filtering in the sequence of forward, backward, and forward filtering. The 1st forward EKF mainly works for convergence of the filter, and the last two EKFs are initialized by foregoing EKF. Our final result is combination of the backward EKF and the 2nd forward EKF. The scheme of FBF-EKF leads to two principal benefits compared with commonly used smoothers: (1) eliminations of inaccuracy before EKF convergence and (2) easy implementation on the base of standard EKF.The content of this paper is organized as follows. Section2 introduces the identification and modeling of IMU stochastic process. Section 3 describes the error model of Kalman Filter, including the navigation propagation model and stochastic processing model. Section 4 presents the FBF-EKF algorithm, and the experiment is outlined in Section 5. Conclusion is presented in Section 6.


## 2. Identification for IMU Stochastic Process

The error of IMU sensor consists of deterministic and stochastic errors. According to IEEE Standard 647 and Standard 528 [20, 21], stochastic errors include white noise, rate random walk, Markov process, quantization noise, flicker noise, sinusoidal noise, and rate ramp. The bias is different every time when the IMU sensor is powered on, and it is not correlated to the angle rate or acceleration; thus, the bias can be modeled as a random constant. The rate white noise, also called angle random walk, is a result of wide band noise being added on the rate data. Integrating rate white noise on angular acceleration and jerk results in rate random walk. Flicker noise is the low-frequency bias fluctuations on the measured rate data, which represents the best instability of sensor. Flicker noise can be modeled by the combination of several Markov processes, while rate ramp can be modeled by a second-order differential equation driven by white noise. Sinusoidal noise can be usually characterized by a number of distinct frequencies, and quantization noise is caused by sampling a continuous signal with finite byte length. Models of each stochastic error are shown in Equation (1), taking a gyro as an example.
(1)ḃω=0,sḟω=0,wω=N⋅v,δω̇K=K⋅v,δω̇B=−βBδωB+βBB⋅v,δω¨R=R⋅v,δω̇M=−βMδωM+βMM⋅v,where bω is the gyro’s bias, sfωdenotes the scale factor error, wω is the white noise, δωKrepresents the rate random walk, δωB denotes the flicker noise, δωR shows the rate ramp, and δωM is Markov process. N, K, B, R, and M are the power spectral density (PSD) coefficients of rate white noise, rate random walk, flicker noise, rate ramp, and Markov process, respectively, while v is a unit Gaussian white noise.Assuming all of the above stochastic processes are independent, the total stochastic error is the sum of each noise type, which is shown in Equation (2), including both gyro and accelerometer.
(2)δa=sfa⋅a+ba+δaK+δaB+δaR+δaM+wa,δω=sfω⋅ω+bω+δωK+δωB+δωR+δωM+wω,where ba,sfa, wa, δaK, δaB, δaR, and δaM are the accelerometer’s bias, scale factor error, white noise, rate random walk, flicker noise, rate ramp, and Markov process, respectively. ωand a are the angle rate and specific force.Allan variance is a standard approach to characterize stochastic errors of inertial sensors. To identify and evaluate the stochastic process of IMU, the Allan variance method was used due to its recognized simplicity and efficiency in our previous work [5]. The relationship between Allan variance and PSD is expressed as follows:
(3)σ2τ=4∫0∞Sωfsin4πfτπfτ2df,where τ is the correlation time, σ2τ denotes the Allan variance, f presents the frequency, and Sωf is the PSD of inertial sensor’s output.Equation (3) can be considered a band-pass filter, and its bandwidth depends on correlation time τ. The resulted Allan variance is proportional to the energy of signal within the bandwidth. In general, different stochastic processes have different correlation times. They present different piecewise linearity with associated slope and intercept after passing though the filter, which can be used for identification of stochastic processes. A detailed relationship between the slope, stochastic process, and coefficient can be found in reference [21].Assuming that the angular rateω is sampled with rate fs for a collection of N data points, The data can be divided into K=N/M clusters, where M is the number of points in each cluster. The corresponding correlation time is τ=M/fs. Then, the average of each cluster is
(4)ω¯=1M∑i=1Mωk−1M+i,k=1,⋯,K.Allan Variance can be computed by the following averaging operation:(5)σA2τM=12K−1∑k=1K−1ω¯k+1M−ω¯kM2.In this work, eight hours’ static IMU data was sampled with a rate of 200 Hz. The Allan variance results are shown in Figures1 and 2. The dominant stochastic processes are rate white noise and rate random walk for both FOG and accelerometers. The PSD coefficients of these two stochastic processes are listed in Table 1, in which the largest values among three axes are used for the following Kalman filter. Flicker noise is also found in Figures 1 and 2. However, it is highly unlikely to model all of the stochastic processes and to make estimation because of poor observability. This effort may even lead to the instability of the filter. Therefore, only selected and principal stochastic processes are considered in practical applications. Here, four stochastic components are considered for the aerial referencing system, which are bias, white noise, rate random walk, and scale factor error. Both bias and scale factor error are modeled as random constant. Then, Equation (2) can be rewritten as follows:
(6)δω=sfω⋅ω+bω+δωK+wω,δa=sfa⋅a+ba+δaK+wa,Figure 1
Allan variance of gyros.Figure 2
Allan variance of accelerometers.Table 1
Stochastic process coefficient.
N (rad/s1/2)K (m/s3/2)Gyro2.53×10−39.82×10−5Accelerometer4.30×10−48.81×10−5


## 3. Error Models in Kalman Filter

The data fusion schemes for GNSS/INS are generally divided into loosely coupling and tightly coupling modes based on combination information, among which the Kalman filter in loosely coupling has a lower order and is more reliable compared to tightly coupling. The main reason is that GNSS and INS work independently in the loosely coupling and integrate on navigation result. However, loosely coupling has to depend on pure INS when three or less GNSS satellites are observed. In free inertial mode, the accuracy will be quickly degraded because of sensor stochastic errors. By comparison, tightly coupling does not have limitation on the satellite number. The integration can work even when the GNSS satellite number is inadequate for positioning. Therefore, tightly coupling has a higher accuracy than the loosely coupling in GNSS-challenged scenario. Aerial application has an open view of satellites, and the satellite number normally can be more than 20 with gradual establishment of multiconstellation GNSS. Therefore, loosely coupling is used in the GNSS/INS-integrated DG with consideration of system reliability and GNSS quality control.The INS equation must be linearized before filtering because of its strong nonlinearity [18]. The linearized state equation of continuous system and discrete observation equation are expressed as follows:
(7)ẋt=Ftxt+GtWt,yk=Hkxk+Vk,where xk is the error states vector of the Kalman filter, yk denotes the observation vector, and Ft and Gt are the dynamics matrix and coefficient matrix of process noise, respectively.Hk is the observation coefficient matrix, while Wt and Vk are process noise and observation noise, respectively. Please note that the continuous state equation needs to be discretized before use.According to the sensor stochastic error models in Equation (6) with considering navigation errors, the state equation of the Kalman filter can be concluded as
(8)ẋrvεẋaẋω=F11F12F13F21F22F23F31F32F33xrvεxaxω+G11G12G21G22G31G32WaWω,where xrvε is the 9-order navigation error including position, velocity, and attitude. xa and xω are the stochastic error of gyro and accelerometer, respectively. As depicted in Equation (6), they are composed by bias, rate random walk, and scale factor stochastic errors, which are shown in Equations (9) and (10), respectively.
(9)xa=baδaKsfaT,(10)xω=bωδωKsfωT.The white noise in Equation (6) is not included in xa and xω, as it is the component of the system-driven noises Waand Wω:
(11)Wa=NaKav,(12)Wω=NωKωv,where Na and Ka are accelerometer’s PSD coefficients of rate white noise and rate random walk, respectively. Nω and Kω are gyro’s PSD coefficients of rate white noise and rate random walk, respectively.For dynamics matrix,F11 is the navigation coefficient matrix and can be found in many literatures, e.g., [22, 23]. F21, F22, F23, F31, F32, and F33 are 9-order zero square matrices. F12 and F13 are presented as follows:
(13)F12=03×303×303×3CbeCbeCbediaga03×303×303×3,(14)F13=03×303×303×303×303×303×3CbeCbeCbediagω,where Cbe is the rotation matrix from body frame to reference frame, and ECEF is used in this paper. diag is a diagonal matrix of the elements.According to the definition of system-driven noisesWa and Wω in Equations (11) and (12), the coefficient matrix of process noise Gtis easily derived as
(15)G11=03×303×3Cbe03×303×303×3,(16)G12=03×303×303×303×3Cbe03×3,(17)G21=03×303×303×3I3×303×303×3,(18)G32=03×303×303×303×303×3I3×3,where I3×3 is a 3-order identity matrix. G22 and G31 are 9 by 6 zero matrices.In loosely coupling, the observation is the difference between INS and GPS:(19)y=rINS−rGPSvINS−vGPS,among which rINS and vINS are the 3-dimension position and velocity vectors by INS, rGPS and vGPS are the 3-dimension position and velocity vectors by GPS. The observation coefficient matrix can be written as
(20)H=I606×21.


## 4. Forward-Backward-Forward EKF

The advantage of postprocessing is that all observations can be used for the state estimation at any epoch. Smoothing Kalman filter is very suitable for processing errors caused by an inaccurate system model and environment influence, such as inaccuracy of stochastic process model, linearization error of the extended Kalman filter, and GNSS interruption.In this paper, a forward-backward-forward EKF (FBF-EKF) is proposed to smooth the navigation result. After filtering, for all epochs by the 1st forward, EKF runs in a reversed direction, which means the state equation evolves in backward time sequence. Following that, forward filtering will be run for the second time. The final result is a combination of backward and the 2nd forward estimation. The main task of the 1st forward EKF is to provide initializations for the backward and 2nd forward EKFs, such as navigation states, sensor’s error, and their covariance. These initializations are more accurate compared to the initial alignment. This triple filtering strategy avoids inaccuracy in the 1st forward EKF when the filter has not reached convergence. Another bonus is the backward filtering can employ standard EKF equations like forward EKF as it starts with a finite covariance. These characteristics are very attractive in the view of engineering implementation.The algorithm of FBF-EKF is presented in Figure3:
(i)
Perform forward EKF with Equation (7) as the system model(ii)
Initialize the backward EKF with(21)x^N,b−=x^N,f+,PN,b−=PN,f+,where x^N,b− and PN,b− are the prior estimation and covariance of backward EKF at the last epoch N, respectively. x^N,f+ and PN,f+ are posterior estimation and covariance of 1st forward EKF at the last epoch N, respectively(iii)
Perform backward EKF with the system model as(22)ẋ−t=F−tx−t+G−tW−t,yk=Hkxk+Vk(iv)
Initialize the 2nd forward EKF with Equation (23),
(23)x^1,f−=x^1,b+,P1,f−=P1,b+,where x^1,f− and P1,f− are the prior estimation and covariance of 2nd forward EKF at the first epoch, respectively. x^1,b+ and P1,b+ are posterior estimation and covariance of backward EKF at the first epoch, respectively.(v)
Perform 2nd forward EKF with Equation (7) as the system model(vi)
Combine the 2nd forward estimation and backward estimation as(24)x^k=Pk,bx^k,f+Pk,fx^k,bPk,f+Pk,b−1,Pk=Pk,f−1+Pk,b−1−1,where x^k and Pk are the combination estimation and covariance, respectively. x^k,f and Pk,fdenote the 2nd forward estimation and covariance, respectively, while x^k,b and Pk,b present the backward estimation and covariance.Figure 3
FBF-EKF Algorithm.It should be noticed that the backward state propagation in Equation (22) is in the negative of time interval. It avoids the matrix inversion like other EKF smoothers. Similarly, the backward INS equations are also implemented in the negative of time interval. Therefore, only the time interval is changed for both backward EKF and backward INS compared to the standard algorithm. This makes the forward and backward algorithm possible to share the same coding function and facilitates the FBF-EKF being extended from the EKF algorithm.


## 5. Aerial Test and Performance Assessment

To evaluate the performance of the proposed FBF-EKF algorithm, an aerial surveying task was performed in Huanghua, Hebei Province, China. Dual-frequency GPS/GLOANSS/BDS data of both base and rover receivers were collected with a sampling rate of 1 Hz. The flying time in surveying area was about 1 hour, and the longest baseline was about 50 km. An FOG based IMU with 200 Hz sampling rate was rigidly mounted with camera sensor to provide its exterior orientation elements. Figure4 shows the assembly of all sensors, and the trajectory in surveying area is shown in Figure 5. The surveying area was 22 km long and 5.3 km wide, covered by 7 straight routes.Figure 4
The assembly of sensors.Figure 5
Aerial trajectory on Google Earth.The position standard deviation (Std) of FBF-EKF on surveying routes was displayed in Figure6, including the 1st forward, backward, 2nd forward, and the smoothing EKF. We observe that the Std of three single-directional EKF have similar values. Both horizontal and elevation Std are less than 5 cm for most epochs, while the smoothing EKF has smaller Std compared to single-directional EKF. All the four processing have a peak at the time of 45 min due to 10 seconds GNSS interruption.Figure 6
Position Std of FBF-EKF.To investigate the performance of FBF-EKF, partial details around GNSS interruption are presented in Figure7. We find that the error presents periodicity according to GNSS observation. In each GNSS interval, the error of forward EKF grows up with increased time and reaches the largest value at end of the interval. However, the error of backward EKF rises with decreasing time and reaches the largest value at beginning of the interval. For the smoothing EKF, the error exhibits archy appearance. It rises and drops in each GNSS interval. The maximum error happens at middle of the GNSS observation. The maximum Stds of 2nd forward EKF in GNSS interruption are 21.6 cm, 21.5 cm, and 13.3 cm in the east, north, and elevation directions, respectively. By contrast, their values are 7.9 cm, 8.0 cm, and 6.5 cm, respectively, by smoothing EKF. In this way, the positioning error was reduced to approximate 37% in horizontal and 49% in elevation after utilizing smoothing EKF. On the other hand, it is reasonable to assume that positioning error grows quadratically with time, since the specific force is integrated twice to calculate position. After a combination of forward and backward EKF, the largest error happens at the middle time of each GNSS interval, where the error is approximately a quarter of the largest in theory, which is close to the performance obtained by processing results.Figure 7
Partial details of position Std.Figure8 presents the attitude Std by single-directional EKF and smoothing EKF. It has the same characteristics with position Std when single-directional EKF and smoothing EKF are considered. The pitch and roll standard deviation are less than 0.01°, and the yaw is within 0.07° for single-directional EKF. By comparison, the smoothing EKF has better accuracy. The pitch and roll standard deviation are less than 0.008°, and the yaw is within 0.04°. However, the periodicity of attitude Std are different from position Std. Yaw Std presents obvious periodicity according to flight route, and it is contrary to position Std that possesses periodicity coincident with GNSS observation. Specifically, yaw Std grows up with increasing time by forward EKF and rises with decreasing time by backward EKF. For smoothed EKF, the yaw Std rises in the first half of a straight route and decreases in the second half. This is because yaw has weak observability when the plane is in a straight movement. When the plane makes maneuvering like a U-turn, it can help calibrate the yaw angle. Another reason is the smoothing algorithm. Forward filtering yields a good estimation at beginning of the straight route but deteriorates at the end, while backward filtering has the opposite function. As a consequence, smoothing EKF presents an archy error in one straight route, compared to monotone increasing error by single-directional EKF. The value of attitude error is also reduced by the smoother.Figure 8
Attitude Std of FBF-EKF.Figure9 demonstrates the yaw angle of the 1st and 2nd forward filtering at beginning time of the test. We observe that there is about a 10.2-degree difference between two forward EKF at the beginning, and the difference decreases after the airplane starts at 200 s. After 5-minute maneuvering, the yaw angles between two forward EKF are basically identical. This is because the attitude is not convergent after coarse and fine alignment in the 1st forward EKF. It will deteriorate the smoother if 1st forward EKF and backward EKF are combined. In the FBF-EKF, the 2nd forward EKF is initialized by backward EKF, which makes better estimation of attitude at the beginning time. Therefore, the 2nd forward EKF is essential to situations with deficient maneuver, during when the system cannot get convergent before entering surveying route.Figure 9
Initial yaw angle of the 1st and 2nd forward EKF.Figure10 presents horizontal positioning innovation of the 2nd forward EKF, while Figure 11 displays the cumulative distribution of positioning innovation. Elevation innovation of the 2nd forward EKF and its cumulative distribution are shown in Figures 12 and 13, respectively. Due to similar results as the 2nd forward EKF, innovation of the 1st forward EKF and backward EKF is not displayed here. Considering differential GNSS result as a reference, innovation, that is, the observation in Equation (19), is used as evaluation of the INS error. We find that the forward EKF has horizontal positioning accuracy of 15.2 cm (1σ) and elevation positioning accuracy of 49.9 cm (1σ). According to Figures 6 and 7, the horizontal and elevation error can be reduced to 37% and 49% of single-directional EKF after the smoothing algorithm has been implemented. Therefore, the horizontal and elevation positioning accuracy of FBF-EKF are about 5.6 cm (1σ) and 24.4 cm (1σ), respectively. The largest error also happens at the middle time of each GNSS observation interval.Figure 10
Horizontal innovation of the 2nd forward EKF.Figure 11
Cumulative distribution of horizontal Innovation in the 2nd forward EKF.Figure 12
Elevation innovation of the 2nd forward EKF.Figure 13
Cumulative distribution of elevation innovation in the 2nd forward EKF.


## 6. Conclusions

This paper studies the stochastic process modelling and smoothing method of GNSS/INS-integrated direct georeferencing system. To identify and evaluate the error types and values, Allan variance is applied for IMU static data of eight hours. Four types of stochastic errors are considered, that is, bias, rate random walk, scale factor, and white noise. A 27-order error state vector is established, including navigation errors and stochastic errors of IMU. In order to solve the problem of inaccuracy model, strong nonlinearity, and weak observability, which are commonly occurred in aerial photogrammetry applications, a new FBF-EKF is proposed to make optimal estimation. It carries out triple EKF, in which the 1st EKF converges the system and the last two smooth the error. This new scheme avoids inaccuracy of the 1st forward EKF and makes it possible to implement the same EKF equations and codes for all the three filtering process. Our testing result indicates that the horizontal and elevation accuracy can be improved by 63% and 51%, respectively, by utilizing the proposed algorithm. Finally, it leads to positioning accuracy of centimeter level (5.6 cm) in horizontal and decimeter level (24.4 cm) in elevation.



---

*Source: 6527356-2021-10-18.xml*