INS navigation system

Geodesy and Geodynamics xxx (2017) 1e6

Contents lists available at ScienceDirect

Geodesy and Geodynamics journal homepages: www.keaipublishing.com/en/journals/geog; http://www.jgg09.com/jweb_ddcl_en/EN/volumn/home.shtml

An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system Qing Dai*, Lifen Sui, Lingxuan Wang, Tian Zeng, Yuan Tian Dept. Spatial Information, Information Engineering University, Zhengzhou, 450000, PR China

a r t i c l e i n f o

a b s t r a c t

Article history: Received 18 June 2016 Accepted 18 December 2017 Available online xxx

To further improve the performance of UKF (Unscented Kalman Filter) algorithm used in BDS/SINS (BeiDou Navigation Satellite System/Strap down Inertial Navigation System), an improved GM-UKF (Gaussian Mixture Unscented Kalman Filter) considering non-Gaussian distribution is discussed in this paper. This new algorithm using SVD (Singular Value Decomposition) is proposed to alternative covariance square root calculation in UKF sigma point production. And to end the rapidly increasing number of Gaussian distributions, PDF (Probability Density Function) re-approximation is conducted. In principle this efficiency algorithm proposed here can achieve higher computational speed compared with traditional GM-UKF. And simulation experiment result show that, compared with UKF and GM-UKF algorithm, new algorithm implemented in BDS/SINS tightly integrated navigation system is suitable for handling nonlinear/non-Gaussian integrated navigation position calculation, for its lower computational complexity with high accuracy. © 2018 Institute of Seismology, China Earthquake Administration, etc. Production and hosting by Elsevier B.V. on behalf of KeAi Communications Co., Ltd. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/).

Keywords: Gaussian mixture UKF Singular Value Decomposition Integrated navigation Time complexity

1. Introduction BDS/SINS integrated navigation system with effective performance on system stability is concerned by a considerable number of geodetic scientists [1]. However, when it works in high dynamic and strong electronic interference environment, the navigation results are often confronted by the following problems: (a) the nonlinear navigation system model; (b) the non-Gaussian probability distributions; (c) the computational cost of filter algorithm [2]. EKF (Extended Kalman Filter) is a popular method to tackle nonlinearity problem by Taylor series. But linearization errors may lead to accuracy reduction or divergence [3]. UKF (Unscented Kalman Filter) adopting PDF (Probability Density Function) approximation and weighted statistical linear regression technique

* Corresponding author. E-mail address: [email protected] (Q. Dai). Peer review under responsibility of Institute of Seismology, China Earthquake Administration.

Production and Hosting by Elsevier on behalf of KeAi

attracted attention for its abilities to tackle the defect of EKF [4,5]. Both of these algorithms usually assume that the dynamical and observation errors approximately follow Gaussian distributions. However, this assumption is practically not true in integrated navigation implementation. To this end, some scholars have proposed some effective methods to improve traditional algorithms, such as state amplification method, time varying noise statistical estimator on maximum posteriori estimation as well as weighting Sage-Husa filter [6e8]. Even though the time complexity of calculations is moderated and consistency of filters is satisfied, estimation of covariance calculated by above methods in function model compensation is too conservative. A different type of filter named GMF (Gaussian Mixture Filter) is proposed, which is used to tackle nonlinear/non-Gaussian data assimilation problem [9]. Yang et al. [10] adopted GMF consisted of a set of UKFs (GM-UKF) to reduce linearization errors in non-linear filter model, and solved the problem of priori assumption mismatch with system noise model. However, the number of subfilters increases exponentially over time, which leads to the iterative calculation time increasing exponentially. And the time complexity of the algorithm increases rapidly during navigation calculation [10]. In order to avoid numerical problems, a new efficiency algorithm equipped with SVD (Singular Value Decomposition) and PDF

https://doi.org/10.1016/j.geog.2017.12.001 1674-9847/© 2018 Institute of Seismology, China Earthquake Administration, etc. Production and hosting by Elsevier B.V. on behalf of KeAi Communications Co., Ltd. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/).

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001

2

Q. Dai et al. / Geodesy and Geodynamics xxx (2017) 1e6

where H r and H r_ denote the observation matrix of Pseudo-range and Pseudo-range rate respectively, which are described specifically in Ref. [1]. V r and V r_ are measurement noise parts.

re-approximation is proposed in this paper. SVD is introduced before filtering step and PDF re-approximation is conducted after filtering step, which used to reduce square root computation cost and the number of Gaussian mixture distribution respectively. The computational speed of new algorithm discussed in this paper and algorithm equipped with reduced rank UKF could be same, if GMF is implemented in parallel. In simulation of tightly coupled BDS/ SINS integrated navigation, tests results verify the efficiency and effectiveness of the algorithm.

3. Gaussian Mixture Unscented Kalman Filter The basic idea of GM-UKF algorithm is to approximate state estimate conditional PDF with Gaussian Mixture model instead of a single Gaussian model. Consider the following discrete-time nonlinear stochastic system:

2. BDS/INS integrated navigation system model Tightly coupled BDS/SINS integrated navigation system is a complex combination. As is shown in Fig. 1, rI and r_ I are pseudorange and pseudo-range rates respectively, they are provided by ephemeris date using BDS receivers and position (velocity) data from INS equipment. rD and r_ D are pseudo-range and pseudo-range rates from BDS receiver respectively. In closed-loop feedback mode, filter is used to estimate systematic errors. Then the estimated parameters amend SINS system parameters.

X_ I X_ D


¼

FI 0

0 FD




XI XD




þ

GI 0

0 GD




WI WD

(1)

i¼0

The PDF of the dynamical noise pðuk1 Þ is given by Eqs. (7) and (8) described the divided j ¼ nuk1 sub-systems of the original dynamical system. u

pðuk1 Þz

nk1 X

  auk1;i N uk1 : 0; Q k1;i

(7)

i¼1

(

xk ¼ M k;k1 ðxk1 Þ þ uk1;j     p uk1;j ¼ N uk1 : 0; Q k1;j

(8)

o2lk1 n Each set of sigma points xsk1;i produced in UKF algorithm i¼0

Pseudo-range and Pseudo-range rate outputted by BDS together with SINS predictions constitute the measurement input of observation model. Measurement equation is considered in the following formula [1]:

Hr Vr Xt þ H r_ V r_

(6)

3.1. Propagation step

(2)

2.2. Measurement equation of BDS/INS tightly coupled model

Z¼

  bk1;s N xk1 : Z k1;s ; Fk1

i¼0

where X t ¼ ½dL; dl; dh; dvx ; dvy ; dvz ; 4x ; 4y ; 4z ; εbx ; εby ; εbz ; Vbx ; Vby ; Vbz ; bclk; dclk  are state parameters constituted of position errors, velocity errors, platform angle errors, INS element errors, distance errors bclk equivalent to clock error, as well as distance errors dclk equivalent to the clock frequency error. BDS receiver state transition function and system error matrix are in terms of F t and Gt respectively. W t is non-Gaussian dynamical noise.




2q X

where Nðxk1 : Z k1;s ; Fk1 Þ are Gaussian mixture distributions, and every Gaussian distribution is assembled by sigma points set o2lk1 o2lk1 n n and corresponding weights wsk1;i . xsk1;i



X_ t ¼ F t X t þ Gt W t



(5)

s¼0

And it is often desirable to re-write as




yk ¼ H k ðxk1 Þ þ vk

~ ðxk1 jY k1 Þ ¼ p

State equation of tightly coupled navigation system is considered in the following formula [1]:



(4)

where M k;k1 is state transition matrix, H k is observation matrix, uk and vk are non-Gaussian distribution of statement and observation respectively. Based on the approximation of Gaussian mixture model, at instant k  1 the posterior PDF of pðxk1 jY k1 Þ is approximated by:

2.1. State equation of BDS/INS tightly coupled model




xk ¼ M k;k1 ðxk1 Þ þ uk

is evolved forward through each sub-system. The prior PDF is formed



xb

pðxk jY k1 Þz



nk1 X

b

b gk;s N xk : bx k;s ; P k;s b

(9)

s¼1

(3)

Correction (Position, Velocity, Attitude )

IMU

SINS Caculation

Pseudorange, Pseudorange rate

+ GM-UKF

Ephemeris

GPS Receriver

Satellite Signal Preprocessing

Position, Velocity, Attitude

Pseudorange, pseudorange rate

Fig. 1. Tightly coupled integrated navigation system.

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001

Q. Dai et al. / Geodesy and Geodynamics xxx (2017) 1e6

3

where

4. GM-UKF algorithm improvement

gk;s ¼ auk1;j bk1;i

4.1. Reduced rank scaled UKF b a was calculated by SVD algorithm. Let The square root of P k1 and ð2lk1 þ 1Þ sigma S xa k1 ¼ ½sk1;i ek1;i ; /; sk1;lk1 ek1;lk1 , points can be generated as follows:

u nxb k1 ¼ ð2lk1 þ 1Þ  j ¼ nk1

s ¼ i þ ð2lk1 þ 1Þ  ðj  1Þ

(

3.2. Filtering step Eq. (10) gives the PDF of observation noise pðvk Þ, and Eq. (11) describes the divided nvk sub-systems of the original observation system. v

pðvk Þz

nk X

  avk;i N vk : 0; Rk;i

(10)

i¼1

(

yk ¼ H k ðxk Þ þ vk;j     p vk;j ¼ N vk;j : 0; Rk;j

(11)

where j ¼ nvk , the Kalman gain K S ðs ¼ 1; /; nxa ; nxa ¼ nxb  nvk Þ k k k evaluated

for

each

b bb Gaussian distribution Nðxk : b x k;j ; P k;j Þ a a b b incoming observation yk , x k;s and P k;s ðs ¼

ðj ¼ 1; /; nxb Þ. With the k Þ were updated 1; /; nxa k b bb xb Nðxk : b x k;j ; P k;j Þðj ¼ 1; /; nk Þ.

for

each

Gaussian

distribution

   8 a b b
bk;s

(12)

to

posterior

 pr  b b þR x k;i ; P yk : H k b k;i k;i ¼ P xb P v   pr   nk nk b b þR g av N yk : H k bx k;i ; P k;i k;i i¼1 j¼1 k;i k;j

gk;i avk;j N



weights



a xak1;0 ¼ b x k1

a xak1;i ¼ b x k1 ±aðlk1 þ lÞ1=2 sk1;i ek1;i

(15)

where sk1;i and ek1;i are the leading lk1 eigenvalues and correb a respectively. For calculation, the first sponding eigenvectors of P k1 lk1 leading pairs of eigenvalues and eigenvectors instead of whole spectrum need to be calculated. So in high dimensional system this improved strategy could reduce computation cost. Table 1 shows the computational complexity of different filter schemes. In this table variable refers to the average number of iterations in finding a pair of eigenvalue and corresponding eigenvector. 4.2. GM-UKF PDF re-approximation As is discussed in chapter 3.2, a potential problem of GM-UKF is that the number of Gaussian distributions in Gaussian mixture model may grow rapidly, which affect the efficiency of algorithm. To this end, PDF re-approximate is proposed on a criterion of PDF re-approximation, which makes the mean and covariance of new Gaussian mixture model match the original one. pðxk jY k Þ was ~ ðxk jY k Þ using approximated by another Gaussian mixture model p m ¼ 2q þ 1 Gaussian distributions

~ ðxk jY k Þ ¼ pðxk jY k Þzp

2q X

  bk;s N xk : Z k;s ; Fk

(16)

s¼0 a ~ak ¼ b x k zx

2q X

bs Z s

(17)

s¼0

(13)

~a ¼ b a zP P k1 k1

2q X

  ~ÞðZ s  x ~ ÞT bs Fs þ ðZ s  x

(18)

s¼0

3.3. Filters combination pðxk jY k Þ was re-approximated by a set of Gaussian distributions, a b a were evaluated: and the mean b x and covariance P k

xa

pðxk jY k Þz

nk X

k



bk;s N xk :

a b x k;s ;

ba P k;s



(14)

s¼1

3.4. Performance analysis of GM-UKF algorithm Gaussian Mixture filter used a set of UKFs to approximate PDF in nonlinear system, which makes the accuracy of nonlinear model approximation near second or third order of Taylor series. GM-UKF algorithm has robustness capability, because weights of Gaussian component values can be adaptively adjusted by measuring vectors yk . However, the number of Gaussian component weight bk;s is much more than the number of Gaussian component weight bk1;s . And in such circumstances, the Gaussian component size of GMUKF could grow very rapidly with time. So it can be shown that GM-UKF is unfavorable for navigation position calculation.

xa b Sk

b a factorization was performed by SVD Covariance matrix P k

¼ ½sk;i ek;i ; /; sk;lk ek;lk  and two more square root matrices S~k;1 and S~ were constructed as follows: k;2

i h S~k;1 ¼ c sk;1 ek;1 ; /sk;q ek;q

(19)

i h S~k;2 ¼ dsk;1 ek;1 ; /dsk;q ek;q ; /sk;lk ek;lk

(20)

where c2½0; 1; d ¼ ð1  c2 Þ1=2 . According to above information, Eq. (18) can be rewritten as 2q X

 T ~ÞðZ s  x ~ÞT ¼ S~k;1 S~k;1 bs ðZ s  x

(21)

 T bs Fs ¼ S~k;2 S~k;2

(22)

s¼0 2q X s¼0

Compared with UKF algorithm, Eqs. (17) and (21) can be solved by scaled unscented transform (SUT). Where Zs and bs can be

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001

4

Q. Dai et al. / Geodesy and Geodynamics xxx (2017) 1e6

Table 1 Comparison of time complexity. Scheme

One sigma point

2lk1 þ 1 sigma points

UKF

Oðm2 Þ

Oðm3 Þ

RUKF

Oðm2 Þ

lk1  it  Oðm2 Þ

treated as a set of sigma points and associated weights respectively. Meanwhile Fs were set to be the same for all i ¼ 0; /2q components, then F ¼ F ¼ S~ ðS~ ÞT can be calculated by Eq. (21). i

k;2

k;2

In this work PDF re-approximation is proposed to reduce traditional GM-UKF computational cost. And in every filter step of this improved algorithm (RGM-UKF), SVD performed once can be used for generating sigma points for individual reduced rank scaled UKF and conducting PDF re-approximation, by letting the covariance of all Gaussian distributions in re-approximated GM-UKF to be the same. The benefit of adopting this method is that, if algorithm is implemented in parallel, the computational speed of RGM-UKF will almost the same as that of reduced rank scaled UKF. Fig. 3. Measurements of pitch.

5. Simulation and analysis Tightly coupled BDS/SINS simulation experiments are performed to verify the feasibility of proposed algorithm in this paper. Experiment parameters were set: BDS output set period of 1 s, pseudo range measurement error of 15 m, random error rate of 0.05 m/s. SINS output set cycle of 0.01 s, filtering cycle of 1 s, gyroscope relevant time of 100.0 s, accelerometer relevant time of 60.0 s, gyro constant drift of 2 ( )/h, gyro white noise variance of 0.02 ( )/h, and accelerometer bias of 103 g. Accelerometer shave white noise with variance 106 g. This simulation has been performed on Lenovo I5-4460 (I5 1.9 GHz/RAM 8 GB/win10 OS) computing platform for 2000 s. Figs. 2e4 show measurements of roll, pitch and yaw respectively. In order to discuss the performance of different integrated navigation system filters affected by non-Gaussian noise factor in simulation test, model 0:9Nð0; 0:52 Þ þ ð1  0:1ÞNð0; 0:82 Þ is introduced to set non-Gaussian noise during the period from 500 s to 2000 s. Comparing PRN19 (low satellite elevation angle) with PRN5 (higher satellite elevation angle), responding to the previous ‘comparing’ pseudo-range observation noise exhibits super Gaussian distribution and its mean is nonzero. The kurtos is of PRN19 and PRN5 are calculated to be 0.09 and 0.65, respectively. Quantile normal distribution is shown in Figs. 5 and 6.

Fig. 2. Measurements of roll.

Fig. 4. Measurements of yaw.

Fig. 5. QeQ plot of PRN19 measurement noise.

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001

Q. Dai et al. / Geodesy and Geodynamics xxx (2017) 1e6

Fig. 6. QeQ plot of PRN5 measurement noise.

Three different state estimation schemes are performed here: Scheme 1 e Standard UKF, Scheme 2 - GM-UKF, Scheme 3 e RGMUKF. The experimental results are analyzed. As is shown in Fig. 7, all three different schemes can converge in range segmentation from 0 s to 500 s, because both the system noise model and measurement noise model match with Gaussian white noise sequence. After the 500th second, system model and measurement model are influenced by non-Gaussian noise sequence, and the

5

stability as well as the accuracy of three different filter schemes is reduced significantly. Compared with Scheme 1, Scheme 2 and Scheme 3 provide more accurate position results significantly. Because in GM-UKF framework Gaussian component weights were adjusted with the observation of new information adaptively, which enhance the own robustness of the algorithm. In other words, GM-UKF algorithm could be much better for non-Gaussian noise distribution. In order to comparing positioning performance of RGM-UKF and GM-UKF clearly, Fig. 8 shows position errors in different directions during the epoch 500 se2000 s. It can be seen that The performance of both RGM-UKF discussed in this paper and GM-UKF are rather stability, and the accuracy of the navigation results can reach standard requirement. Table 2 shows RMSE (root mean square error) of attitude, velocity and position by three different filter schemes during simulation period 500 se2000 s. Compared with scheme 1, RMSE calculated by Scheme 2 and Scheme 3 is significantly smaller. It indicates that GMUKF can effectively improve filter accuracy and reduce errors in nonlinear/non-Gaussian BDS/SINS tightly coupled navigation system. However, in general there is no guarantee that re-approximate step also preserves the higher order moments of original one. So filtering accuracy calculated by RGM-UKF algorithm is slightly lower than GM-UKF algorithm. Comparison of computation time in three different schemes. Calculation time for Scheme 3 is shorter than scheme 2, due to the relatively stable number of Gaussian components in RGM-UKF algorithm, compared to GM-UKF algorithm. Based on above information, the new proposed algorithm reduces computation cost in an improvement form. Even though the accuracy of the proposed algorithm is not as high as GM-UKF

Fig. 7. Position error of three Schemes.

Fig. 8. Position error based on three different filters (East, North, Up).

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001

6

Q. Dai et al. / Geodesy and Geodynamics xxx (2017) 1e6

Foundation (41674016 and 41274016). We gratefully acknowledge members of our laboratory in preparing this application note.

Table 2 Estimation errors (RMS) and simulation time.

Attitude error ( ) (roll/pitch) Speed error (m/s) (E/N/U)

Position error (m) (E/N/U)

Calculating time (s)

Scheme 1

Scheme 2

Scheme 3

Standard UKF

GM-UKF

RGM-UKF

2.322 3.422 3.223 3.411 4.202 6.873 6.793 7.993 8.3

1.992 2.221 2.021 1.993 2.402 3.493 3.901 4.203 15.6

2.002 2.231 2.021 2.221 2.452 3.982 4.173 4.439 9.8

algorithm, calculation time is reduced significantly. And the new filter introduced here is suitable for state estimation of nonlinear/non-Gaussian BDS/SINS navigation position calculation system. 6. Conclusion In this paper a new filter named RGM-UKF is introduced to assimilate nonlinear/non-Gaussian BDS/SINS integrated navigation system. This new filter is based on the framework of the Gauss mixture model, and it is the combination of reduced rank scaled UKF and Gaussian Mixture model. It essentially consists of a set of parallel reduced rank scaled UKF. To reduce the computational cost of GM-UKF algorithm, an auxiliary algorithm conducted PDF re-approximation is proposed, which speed up the computational time when GM-UKF is implemented in parallel. In the simulation, tightly coupled BDS/SINS integrated navigation model is used to illustrate the performance of RGM-UKF algorithm. Numerical results of experiments showed that navigation error of new algorithm is 5% larger than GM-UKF algorithm. And the calculation time cost of RGM-UKF reduced 37% compared to the GM-UKF algorithm.

References [1] Xinlong Wang, SINS/GPS Integrated Navigation Technology, first ed., Beihang University Press, Beijing, 2015 (in Chinese). [2] Lin Zhao, Xiaoxun Wang, Nonlinear System Filtering theory, first ed., Beijing National Defence Industry Press, 2012 (in Chinese). [3] Gao Yi, Gao Shesheng, Robust adaptively sage filtering integrated navigation system, J. Meas. Control Technol. 04 (2015) 135e138 (in Chinese). [4] Yue Guo, Xinxue Liu, Caihong Wang, A study on IMM-UPF of tracking ballistic missile at free-flight phase in mixed coordinate system, J. Ballistics 01 (2015) 12e17 (in Chinese). [5] Jian Xiong, Linsheng Wei, Hang Guo, Rong Liu, Research on SINS nonlinear fast initial alignment for large azimuth misalignment angle, J. Bull. Surv. Mapp. 08 (2013) 6e9 (in Chinese). [6] Zhaihe Zhou, Jianye Liu, Jizhou Lai, Jian Xiong, Computational complexity of Mixture Gaussian particle filter about application in integrated navigation, J. Chin. Iner. Technol. 05 (2010) 595e599 (in Chinese). [7] Shao Jie Qiao, Kun Jin, Nan Han, Chang Jie Tang, Gesangduoji, Louis Alberto Gutierrz, Trajectory prediction algorithm based on Gaussian mixture model, J. Softw. 05 (2015) 1048e1063 (in Chinese). [8] Qing hui Hu, Lixin Ding, Yujing Lu, Rapid robust clustering algorithm for Gaussian finite mixture model, J. Comput. Sci. 08 (2013) 191e195 (in Chinese). [9] G. Terejanu, P. Singla, T. Singh, Adaptive Gaussian sum filter for nonlinear Bayesian estimation, J. IEEE Trans. Autom. Control 56 (2011) 2151e2156. [10] Miin-Shen Yang, Chien-Yo Lai, Chih-Ying Lin, A robust EM clustering algorithm for Gaussian mixture models, J. Pattern Recognit. 11 (2012) 245e253.

Qing Dai was born in Luoyang, Henan, on April 18, 1985. He received the B.S in remote sense from Information Engineering University, Zhengzhou, and M.S in Geomatics from Technological University of Malaysia, Johor Bahru. From 2014 to now he is a Ph.D Candidates research on nonlinear/non-Gaussian integrated navigation system. E-mail address: [email protected]. Full postal address: Unit 2, Block 32, Ju Yu Jiang Nan Residential, Yong Chuan District, ChongQing City, PR China. Telephone: þ86-15023198853

Acknowledgment This work was carried out by department of spatial information, Information Engineering University, Zhengzhou, China, and this research was supported by Chinese National Natural Science

Please cite this article in press as: Q. Dai, et al., An efficiency algorithm on Gaussian mixture UKF for BDS/INS navigation system, Geodesy and Geodynamics (2017), https://doi.org/10.1016/j.geog.2017.12.001