Improved Kalman Filter Algorithm of GPS

注：图和表格在论文的最后。

查了下词典under/in the circumstances 都可以。

Improved Kalman Filter Algorithm of GPS/INSIntegrated Navigation without GPS SignalZHU Lixin1, MENG Yan2(1.Electronic Engineering Institute of PLA, Anhui 230037,China)(2.No.72671 Unit of PLA, Jinan 250022,China)Abstract: To solve the problems that the navigation accuracy of GPS/INS(Global Positioning System/Inertial Navigation System) tightly integrated navigation system is lower under GPS signal invalidation circumstances, and model errors and calculation errors exist in the application of CKF(Cubature Kalman Filter), a strong tracking SRCKF(Square-Root CKF) is proposed. By stressing the function of new data artificially, the algorithm improves the robustness when the model is uncertain, and the square-root strategy ensures the positive definiteness and symmetry of covariance matrix. Simulation results show that the improved algorithm can improve the navigation accuracy and the ability of adapting to the complex environment, and have better efficiency under GPS signal invalidation circumstances.Key words: GPS/INS tightly integrated navigation; CKF; strong tracking filter; square-root strategy; navigation accuracy1. IntroductionThe navigation accuracy of GPS/INS integrated navigation system overcoming the respective defects is higher than the accuracy of two systems working separately[1]. When the receiver is under the great angle motion and the complex circumstances of the city and the valley, satellite signals are disturbed and shielded easily[2]. On the situation, observations provided by GPS/INS tightly integration utilizing less than 4 satellites in the measurement of the pseudorange and the pseudorange rate leads to integrated model working well[3].Considering the nonlinear characteristic of GPS/INS tightly integrated navigation model, a hybrid linear/nonlinear model based on linear state equation and nonlinear measurement equation was proposed[4]. EKF(Extend Kalman filter) and UKF(Unscented Kalman filter) are classical nonlinear filters. EKF expanding the nonlinear equation by the Taylor series up to the first order approximates the original equation. To some extent, although EKF can solve the problem of nonlinear state estimation, it has some disadvantages obviously. First, owing to the lack of high order terms, it can induce truncation error,which has a bad influence on the estimation accuracy. Second, it’s difficult to compute the Jacobian matrix when the nonlinear degree is high. The computation of UKF is similar to EKF’s and the property of UKF is better than that of EKF. Additionally, UKF don’t need to compute the Jacobian matrix. so, it’s expanded continuously in the field of application. However, in the process of recursive filtering, UKF needs to do the matrix decomposition and the inverse matrix, which is difficult to ensure the positive definiteness of covariance matrix.From the above problems, This paper proposes the CKF algorithm based on the s trong tracking filter theory and the square-root strategy under GPS/INS tightly integrated navigation signal invalidation circumstances.2. GPS/INS tightly integrated modelUnder normal circumstances, the characteristic of tightly integration integrated indepth is that the GPS receiver and the inertial navigation system work together. We calculate the pseudorange 1ρ and the pseudorange rate1ρby utilizing the data of satellite ephemeris given by the GPS receiver and the position and velocity given by the inertial navigation system.Compared 1ρand 1ρwith G ρand G ρ given by the GPS receiver, the results are observations.Two systems are adjusted by the error of the GPS receiver and the inertial navigation system estimated by the integrated Kalman filter. Work diagram is shown as Fig.1[5]. 2.1 State equationThe error state variable of GPS is 2-dimensional )(t G X . The error of INS )(t X I consisting of platform error angles(3D), velocity errors(3D), position errors(3D), INS component errors(9D) is 18 dimension. The state equation of GPS/INS integrated navigation system can be written as⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡)()()(00)()()()(00)()()(t t t t t t t t t t G I G I G I G I G I W W G G X X F F X X (1)i.e.)()()()()(t t t t t W G X F X+= (2) whereT ru u rz ry rx bz by bx U N E U N E t t z y x h L v v v t 120],,,,,,,,,,,,,,,,,,,[)(⨯∆∆∆∆∆∆∆∆∆∆∆=εεεεεελϕϕϕX (3) are attitude errors, velocity errors, positionerrors, gyro drift errors(constant offset and random offset ), accelerometer errors, GPS clock correction equivalent distance and equivalent distance rates respectively.W G F ,,are the state transition matrix, the noisecoefficient matrix and the noise vectorrespectively. Specifically, it ’s shown in literature [6].2.2 Measurement equationWe assume that the position given by INS in the geodetic coordinate system denote longitude I λ, latitude I L and altitude I h , thentheactualpositionofcarrierare λλλ∆-=I ,L L L I ∆-=,h h h I ∆-=,which are transformed into ECEF(earth-centeredearth-fixed) coordinate system as follows:⎪⎩⎪⎨⎧∆-∆-+-=∆-∆-∆-+=∆-∆-∆-+=)sin(])1([)sin()cos()()cos()cos()(2L L h h f R z L L h h R y L L h h R x I I N I I I N I I I N λλλλ (4) Thus, the actual distance )4,3,2,1(=j j ρ between the satellite and the carrier is222)()()(z z y y x x j s j s j s j -+-+-=ρ (5)The pseudorange of between the GPS receiver and the satellite j isρρρv t u j jG +∆+= (6)i.e.ρρv t z z y y x x u j s j s j s j G +∆+-+-+-=222)()()( (7)Substituting x,y,z into the above equation yields{}ρλλλλρv t L L h h f R z L L h h R y L L h h R x u I I N j sI I I N j s I I I N j s j G +∆+∆-∆-+--+∆-∆-∆-+-+∆-∆-∆-+-=212222)]sin(])1([[)]sin()cos()([)]cos()cos()([(8)Similarly, the pseudorange isρρρv t ru j j G +∆+= (9) Substituting j ρinto (9) yieldsρρv t z z y y x x z z z z y y y y x xx x ru j s j s j s j s j s j s j s j s j s j G+∆+-+-+---+--+--=222)()()())(())(())(( (10)Substituting x,y,z into the above equation yields the measurement equation of the pseudorange rate, and the pseudorange rate of the receiver as the measurement observation denotesTG G G G G G G G G t Z ][)(43214321ρρρρρρρρ = (11) where 4321,,,G G G G ρρρρ are the pseudoranges, 4321,,,G G G G ρρρρ are the pseudorange rates. From the above equation, )(t G Z is a nonlinear measurement equation.3. CKF algorithm based on strong tracking filter theory and square-root strategy CKF based on the nonlinear filter of deterministic sampling was proposed by the Canadian academic Ienkaran Arasaratnam[7]. By utilizing the numerical integration for the approximation of a target state posterior probability and determining a series of integral points and weights to calculate the posterior probability density, we can reach the compromise of the computation and accuracy. Research show that when the dimension of the nonlinear system is higher than 3-dimension, the optimal filter is CKF because of the filter accuracy of CKF higher than that of UKF[8]. As previously stated, the state dimension of integrated navigation system is higher than 3-dimension, so we select CKF.CKF applied to GPS/INS integrated navigation has two problems. First, the revising function of the present observations for one step prediction is restrained by the past observations, so the filter value don ’t track the change of the true value[9]. Second, owing to the algebraic operation error caused by truncation effect, the positive definiteness and symmetry ofcovariance matrix are lost. The decompositionand inversion of the matrix in the CKF filter process make it worse, which causes the filtering unstability.From the above two problems, the improved algorithm based on the s trong tracking filter theory and the square-root strategy is applied to GPS/INS tightly integrated navigation under GPS signal invalidation circumstances, to validate its efficiency.3.1 Strong tracking filter theoryAssume that the system model and measurement model of the state-space are),(1k k k k f w x x =+ (12)),(1111++++=k k k k h v x z (13)where k x is the system state vector at time k,k z is the measurement variable.k w and 1+k vare the system noise and measurement noise respectively,and),(~k k N Q 0w ,),(~k k N R 0v . f is the state transition function. h is the measurement function.Considering the first problem of CKF applied to integrated navigation, utilizing the fading memory filter can refrain filter divergence caused by model errors, i.e. stressing the function of new data artificially and weakening the function of old data. Based on the theory, the extended Kalman filter with a sub-optimal fading factor was proposed, called STK(strong tracking filter)[10-11].STK induces the fading factor 1+k λ into the state predicted covariance matrix k k |1+P andadjusts the gain matrix 1+k K in real time, which meets the two conditions as follows:min ])ˆ)(ˆ[(=--T k k k k x x xx E (14) ,...1,0,...;1,0,0][===+j k e e E Tk j k (15)When the model matches the actual systemperfectly, two equations are true. Otherwise, the uncertain model causes that the filter state estimation deviates from the system state. At the moment, adjusting the gain matrix online makes the equation (15) true, which forces the strong tracking filter to keep tracking the system state. The equation of the strong tracking filter is:⎪⎪⎪⎪⎩⎪⎪⎪⎪⎨⎧-=+=-+==+==++++-++++++++++++++++++++kk k k k k T k k k k Tk k k k k k k k kk k k k k k k k T k k k k k k kk k k k z z x x x h zx f x|1111111|111|11|111|11|11|1,1,11|1|1)()()ˆ(ˆˆ)ˆ(ˆ)ˆ(ˆP H K I P R H P H H P K K QΦP ΦP λ (16) where 1+k λ is a sub-optimal fading factor, which is computed by the sub-optimal algorithm to improve the real-time performanceof the algorithm. Assume that k k k k z z e |111ˆ+++-= is the output residual sequence, then⎪⎪⎪⎩⎪⎪⎪⎨⎧=--==⎩⎨⎧≤>=+++++++++++++++++Tk T k k k k k k k k T k k k k k k k k k k k k M tr N tr C C C C 1,1,111111111111111][][1,11,H ΦP ΦH M RH Q H V N λ (17) where tr(.) is the matrix trace, 1+k V is the covariance matrix of the output residualsequence. However, it ’s unknown in the actual calculation. It can be estimated by the following equation:⎪⎩⎪⎨⎧≥++==+++1,10,11111k k T k k k Tk ρρe e V e e V (18) where 10≤<ρ is the forgetting factor, and95.0=ρ generally.If the state predicted covariance matrixk k |1+P , the estimation of the autocorrelation covariance matrix k k zz |1,+P and the estimation of the cross-correlation covariance matrixk k xz |1,+P are known, then 1+k N and 1+k M are equivalent to1|1,1|11|1|1,11][])[(][++-+-++++-⨯-=k k k xz k k k T k k T k k xz k k R P P Q P P V N (19) 11|1,1++++-+=k k k k zz k V N P M (20)3.2 Square-root filter strategyConsidering the second problem of CKF applied to integrated navigation,the square-root strategy has a great effect on weakening matrix pathological influence. In the update steps of SRCKF, the square-root factor of the prediction and posterior error covariance matrix takes part in iteration instead of the covariance matrix. Moreover, we compute the gain by utilizing the least square, which avoids the operation of the inverse matrix. The triangle decomposition factor of the covariance matrix is obtained by utilizing triangle or QR decomposition instead of the square-root factor. They all enhance the stability of filters.Generally, QR decomposition algorithm denotes that )(A S Tria =, where S is a lower triangular matrix, the relationship between matrix A and S is that if R obtained by QR decomposition of T A is a upper triangularmatrix, then T R S =. 3.3 Strong tracking SRCKFCombining the strong tracking filter theory and the square-root strategy with the CKF algorithm, we present a strong tracking SRCKF. Algorithm steps are as follows:3.3.1 Time update and evaluate the fading factor1)Assume at time k that the posterior density function ),ˆ()(||k k k k k N p p xx = is known.FactorizeT k k k k k k |||S S P = (21)2)Evaluate the cubature points i ξi i n]1[22=α, i ]1[ denotes the i th of the set of [1]. Here, 2]1[R ∈ can be expressed as⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡-⎥⎦⎤⎢⎣⎡-⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡=10011001]1[3)Evaluate the propagated cubature points),...,2,1(ˆ,m i k k k k i =+=xS αξ (22) ),(*|1,k f i k k i ξξ=+ (23)4)Evaluate the predicted state at time k+1∑=++=m i k k i k k m 1*|1,|11ˆξx(24) 5)Evaluate the predicted state covariance at time k+1T kk k k kk |1|1*|1+++=SS P(25)where k k |1+S is the square-root factor of covariance.])([,*|1|1k Q k k k k Tria S X S ++= (26)where k Q ,S denotes a square-root factor of k Q , and the weighted, centered matrix is]ˆ...ˆ,ˆ[1|1*|1,|1*|1,2|1*|1,1*|1k k k k m k k kk k k k k k k x x x m+++++++---=ξξξX (27) Evaluate the fading factor 1+k λ:6)Evaluate the propagated cubature points),...,2,1(ˆ|1|1|1,m i k k i k k k k i =+=+++xS αξ(28) )(|1,1|1,k k i k k k i h +++=ξχ (29)7)Evaluate the estimation of measurement attime k+1∑=++=m i k k i k k m 1*|1,|11ˆχz(30) 8)Evaluate the estimation of the autocorrelation covariance matrix k k zz |1,+P at time k+1T k k zz k k zz k k zz |1,|1,|1,+++=S S P (31)where k k zz |1,+S is the square-root factor of covariance.])([1,|1|1,+++=k R k k k k zz Tria S Z S (32)where 1,+k R S denotes a square-root factor of1+k R , and the weighted, centered matrix is]ˆ...ˆ,ˆ[1|1|1,|1|1,2|1|1,1|1k k k k m k k k k k k k k k k m+++++++---=z χz χz χZ (33) 9)Evaluate the estimation of thecross-correlation covariance matrixT k k k k k k xz |1|1|1,+++=Z X P (34)where]ˆ...ˆ,ˆ[1|1|1,|1|1,2|1|1,1|1k k k k m k k k k k k k k k k x x xm+++++++---=ξξξX (35) According to the algorithm of section 3.1, we can evaluate the fading factor 1+k λ and the predicted state covariance of the fading factork *|11|1)(Q Q P P +-=+++k k k k k k λ (36)3.3.2 Measurement update1)Factorize the predicted state covarianceT k k k k k k |1|1|1+++=S S P (37)2)According to the step 6),8),9) of section 3.3.1, one evaluates the estimation of measurement, the cross-correlation covariance matrix and the autocorrelation covariance matrix. 3)Evaluate the filter gain matrix1|1,|1,1-+++=k k zz k k xz k P P W (38)4)State update)ˆ(ˆˆ|111|11k k k k k k k +++++-+=z z W x x(39) 5)Evaluate the correlation covariance matrixand the square-root factorT k k T k k k zz k k k k k 111|1,1|11|1++++++++=-=S S W P W P P (40) ])([1,1|11|11++++++-=k R k k k k k k k Tria S W Z W X S (41) 4. Simulation verification and result analysis To validate the efficiency of the improved algorithm, we make simulation experiments on the accuracy of GPS/INS integrated navigation under GPS signal invalidation circumstances.The simulation time of the flying process consisting of constant speed, acceleration, deceleration, wheel, climb, dive, level flight, etc. is 500s. The specific flight path is shown in Fig.2.Simulation parameters are as follows: the random drift of SINS gyro is 0.01(°)/h; therandom noise of the first-order markov processgyro is 0.01(°)/h; the drift error of accelerometer is 10-4g; the random pseudorange error and the random pseudorange rate measurement error of GPS are 10m and 0.05m/s respectively; the sampling period of the simulated aircraft is 0.01s, the update frequency of the strap-down inertial navigation is 50Hz, the update frequency of both the GPS receiver filter data fusion is 1Hz. According to the simulation of the traces and parameters, at the period of 200-400s, it has the large-angle maneuver of acceleration, wheel, dive and so on, inducing GPS signal invalidation easily. In the simulation, we set 3 satellites to validate the efficiency of the improved algorithm under the normal and abnormal situation. Due to space reasons, the accuracy comparison of the strong tracking SRCKF and UKF under normal circumstances is shown in Fig.3 and Fig.4.It can be seen from the above simulation that in the integrated navigation, due to the same question of the algorithm, we make some improvement on CKF and UKF. As a result, the filter accuracy of CKF is higher than that of UKF, validating the conclusion of literature [8].under GPS signal invalidation circumstances, the error of simulation results is shown in Fig.5-Fig.10. The comparison of the navigation information square-root errors in different cases is shown in Tab.1 and Tab.2.From the simulation results of Fig.5-Fig.10 and Tab.1- Tab.2, GPS/INS tightly integrated navigation system can provide an accurate range of the navigation information under GPS signal invalidation circumstances.From the strong tracking filter theory and the square-root strategy, the improved algorithm of CKF improving the function of the present observations effectively reduces the navigation errors induced by the uncertainty of model and computational problems in the filtering process. Under normal circumstances, compared withCKF, the strong tracking SRCKF provides higher accurate navigation information and improves the navigation accuracy of 20%-30%. Under signal invalidation circumstances, the improved CKF provides higher accurate navigation information and improves the navigation accuracy of 30%-40%, and develops its advantages.The integrated navigation has a high require for the real-time performance of the filtering algorithm. Compared with the single filter iteration time of two algorithms in Fig.11, the single filter iteration time of the strong tracking SRCKF is a little higher than that of CKF. Because it needs to compute the fading factor1+kλat update time, which leads to increase thecomputation, but it still can meet the requirements of the real-time performance. Thus, the strong tracking improves the filtering accuracy at the cost of the computation.5. ConclusionThis paper has proposed the CKF algorithm based on the s trong tracking filter theory and square-root strategy. By stressing the function of new data, this algorithm improves the robustness when the model is uncertain, and the square-root strategy ensures the positive definiteness and symmetry of covariance matrix. The simulation results show that, compared with the algorithm of CKF, the strong tracking SRCKF can provide the higher accurate navigation information and develop its advantages under GPS signal invalidation circumstances. Although the single filter iteration time increases 40%, because of utilizing the nonlinear filter algorithm of the strong tracking strategy, the real-time performance of the algorithm still meets the requirements .References[1] Quan W.Inertia/Astronomy/Satellite integratednavigation technology[M]. Beijing:National Defense Industry Press, 2011. (in Chinese) [2] Grewal M S, Weill L R, Andrews A P. GlobalPositioning Systems, Inertial Navigation and Integration[M]. New York: Wiley Interscience, 2007.[3] Xu G, Huang G R, Peng X Z, et al. Performancecomparison of SINS/GPS different combinations Under signal invalidation circumstances[J].Application Research of Computers, 2012(10): 3888-3896.(in Chinese)[4] Yin J J, Zhang J Q, Lin Q. Unscented Kalmanfilter-Kalman filter algorithm[J]. System Engineering and Electronics. 2008, 30(4): 617-620.(in Chinese)[5] Liu J Y, Zeng Q H, Zhao W, et al. Theory andapplication of navigation system[M]. Xi’an: Northwestern Polytechnical University Press, 2010.(in Chinese)[6] Wang H N. Theory and application of GPSnavigation[M]. Beijing: Science Press, 2003.(in Chinese)[7] Ienkaran Arasaratnam, Simon Haykin. CubatureKalman filters[J]. IEEE Trans on Automatic Control, 2009, 54(6):1254-1269.[8] Sun F, Lang L J. Estimation precision comparisonof Cubature Kalman filter and Unscented Kalman filter[J]. Control and Decision, 2013, 28(2): 303-308. (in Chinese)[9] Li L. Fading memory extended Kalman filteralgorithm for target tracking application[J].Electronic Measurement Technology, 2011, 34(2): 36-38, 65.(in Chinese)[10] Zhou D H, Xi Y G, Zhang Z J. A suboptimalmultiple fading extended Kalman filter[J]. Acta Automatica Sinica, 1991, 17(6):689-695.(in Chinese)[11] Jin T, Wang Y B, Cong L, et al. Adaptive strongtracking UKF algorithm based on f uzzy logic[J].High Technology Letters, 2012, 22(4): 348-354.(in Chinese)。