Journal of Global Positioning Systems (2003) Vol. 2, No. 1 : 42-47

Adaptive Kalman Filtering for Vehicle

Congwei Hu1,2, Wu Chen1,Yongqi Chen1 and Dajie Liu2 (1) Department of Land Surveying and Geo-Information, Hong Kong Polytechnic University, Hong Kong,

(2)Tongji University, Shanghai, China

Received: 13 June 2003 / Accepted: 20 November 2003

Abstract. Kalman filters have been widely used for cities. Then the turning points of the trajectory are navigation and system integration. One of the key extracted and compared with the map data at the same problems associated with Kalman filters is how to assign locations to determine the transformation parameters (Hu suitable statistical properties to both the dynamic and the et al, 2003). Currently Kalman filters have been widely observational models. For GPS navigation, the used in different GPS receivers. However, a conventional manoeuvre of the vehicle and the level of measurement is vulnerable for the determination of the are environmental dependent, and hardly to be turning points precisely. predicted. Therefore to assign constant noise levels for The Kalman filtering is an optimal estimation method such applications is not realistic. that has been widely applied in real-time dynamic data processing. A Kalman filter estimates the state of a In this paper, real-time adaptive are applied to dynamic system with two different models namely GPS data processing. Two different adaptive algorithms dynamic and observation models. The dynamic model are discussed in the paper. A number of tests have been describes the behaviour of state vector, while the carried out to compare the performance of the adaptive observation model establishes the relationship between algorithms with a conventional Kalman filter for vehicle measurements and the state vector. Both models are navigation. The test results demonstrate that the new associated with statistical properties to describe the adaptive algorithms are much robust to the sudden accuracy of the models. For many applications, the model changes of vehicle motion and measurement errors. statistic noise levels are given before the filtering process and will maintain unchanged during the whole recursive process. Commonly, this a priori statistical information is Key words: GPS, adaptive filtering, vehicle navigation, determined by test analysis and certain knowledge about real time positioning the observation type beforehand. If such a priori information is inadequate to represent the real statistic noise levels, Kalman estimation is not optimal and may cause to an unreliable results, sometimes even leads to filtering divergence (Mohamed and K.P. Schwarz 1999). For vehicle navigation, sudden acceleration or 1 Introduction deceleration and sudden change of the directions are impossible to predict. Therefore it is difficult to design a For some navigation applications, we need to know the system with constant noise that will satisfy all precise position when a vehicle turns. In map matching situations. One of the common problems with vehicle processing, for example, the turning points of a vehicle navigation using Kalman filter is so called ‘over have to be accurately determined in order to establish shooting’ problem. That is the effect that the dynamic reliable map matching (Yu et al, 2002). In most cities, model keeps position estimation along with previous the local maps are based on local datum and they have to trend while a vehicle actually turns to another direction. be transformed to WGS 84 coordinate system for the use of technology. One of the quick ways Adaptive filtering is trying to determine the statistic to establish the transformation parameters is to drive a car parameters of the dynamic system based on the behaviour equipped with a GPS receiver in the different parts of the of the system during data processing, and it has been paid

Hu etal.: Adaptive Kalman Filtering for Vehicle Navigation 43

much attention in Kalman filtering theory (Jia and Zhu, where k denotes epoch number; X is the state vector at 1984, and Gustafsson, 2000). Different adaptive Kalman k filtering algorithms have been studied for surveying and epoch k; Φk+1,k is the state transition ; Γ k+1,k is navigation applications. Chen (1992) and Mohamed and vector of system disturbance; Ω is vector of dynamic Schwarz (1999) applied adaptive Kalman filters for the k integration of GPS and inertial (INS). model noise; Lk+1 is vector of observation at epoch k+1; Wang et al (1997) applied a simplified adaptive B represent design matrix for observation; V is in kinematic GPS positioning. Chen et al (1999) uses k+1 adaptive filters to estimate the velocity of permanent GPS observation noise; and δkj is the δ -function of stations. Kronecker. In this paper, we investigate the performance of two Kalman filtering estimation can be expressed as: different adaptive Kalman filters for vehicle navigation using GPS, one based on the fading memory and one : based on the estimation. Both algorithms make ˆ ˆ use of the predicted residuals. The fading memory X k+1/k = Φk+1,k X k/k (3) approach tries to estimate a scale factor to increase the predicted variance components of the state vector. The T T QX(k+1/k) = Φk+1,k QX(k/k)Φk+1,k + Γ k+1,k QΩk Γ k+1,k (4) variance estimation method, on the other hand, directly calculates the variance factor for the dynamic model. Updating: Both algorithms closely examine whether there are ˆ ˆ ˆ divergence in the filtering process. If there is no X k+1/k+1 = X k+1/k + K k+1(Lk+1 − Bk+1 X k+1/k ) (5) divergence, the conventional Kalman filtering is used.

Otherwise, the adaptive algorithms are applied. Two QX(k+1/k+1 ) = (I − K k+1Bk+1 )QX(k+1/k) (6) examples of vehicle navigation with DGPS are given in the paper. It demonstrates that the positioning accuracy K = Q BT (B Q BT + Q )−1 with the adaptive approaches is significantly better than k+1 X (k+1/ k ) k+1 k+1 X (k+1/ k ) k +! L(k+1) the conventional Kalman filtering, especially when the (7) vehicle turns around the corners. ˆ where X k+1/ k is the predicted state vector; QX (k+1/ k ) is the variance matrix for Xˆ ; K is the gain matrix; 2 The Adaptive Kalman Filtering Algorithms k+1/ k k+1 ˆ X k+1/ k+1 is the estimation of filtering; and QX(k+1/k+1 ) is Considering a general linear dynamic system: its variance matrix.

X k+1 = Φk+1,k X k + Γ k+1,k Ωk (1) The adaptive filtering with fading memory algorithm Lk+1 = Bk+1 X k+1 + Vk+1 (2) with The Kalman filtering estimation at epoch k can be considered as ‘weighted’ adjustment between the new E(Ωk ) = 0 , measurements (observation model) and the predicted state vector based on the dynamic model and all previous

cov(Ωk ,Ω j ) = QΩk δkj , measurements. If too much ‘weight’ were put to the dynamic model, the estimation would ignore the E(V ) = 0 , information from measurements and causes the k divergence of the filtering process. The idea of fading cov(V ,V ) = Q δ , memory is simple. By applying a factor S>1 to the k j Lk kj predicted matrix to deliberately increase the variance of the predicted state vector (Eq. 8), more cov(Ωk ,V j ) = 0 , ‘weight’ will be given to the measurements.

' T T ˆ Q X(k+1/k) = S(Φ Q Φ + Γ Q Γ ) E(X 0 ) = X 0/ 0 , k+1,k X(k/k) k+1,k k+1,k Ωk k+1,k (8) Q = var(X ) . X 0 / 0 0 The main difference between different fading memory algorithms is on how to calculate the scale factor S. One approach is to assign the scale factor as a constant, S =1

44 Journal of Global Positioning Systems

〜1.4. When S =1, it becomes the conventional Kalman 1 (V T V ) filtering. Obviously there are some drawbacks with a m k+1/k k+1/k constant factor. For example, as the filtering proceeds, the S ≥ k+1 precision of the filtering will decrease because the effects 1 N −1 1 (V T V ) of old data will become less and less. The best way is to N ∑ m k−N + j/k−N k−N + j/k−N use a variant scale factor that will be determined base on j=0 k−N + j the dynamic and observation model accuracy. In this (16) paper, an algorithm is derived based on the size of the predicted residuals, which represent the difference where mi is the number of measurements at epoch i. between the measurements and the predicted state vector. When S≤1，it indicates that the filtering is in a steady The predicted residual vector is expressed as: state processing. When S>1, it indicates that the filtering ˆ may be in an unstable state or in a state of divergence. In Vk+1/k = Lk+1 − Bk+1 (X k+1/k ) (9) practice, in order to avoid false alarm, a threshold S0>1 is For a linear dynamic system, we have (Jia and Zhu, 1984) selected. When S>S0, the adaptive algorithm is applied. Otherwise S =1 and the conventional Kalman filtering T E(Vk+1/kVk+1/k ) algorithm is used. T (10) = Bk+1QX(k+1/k)Bk+1 + QL(k+1 ) The with variance component Let us introduce a scale factor S≥1 to the predicted estimation

T This approach tries to estimate the variance factor of the Q& X (k+1/ k ) = S(Φk+1,k QX (k / k )Qk+1,k (11) dynamic model directly using the predicted residuals. + Γ Q Γ T ) Considering the prediction vector as a pseudo- k+1 Ωk k+1,k observation, the Kalman filter equation can be expressed Then as a Gauss-Markov model: T T V = Axˆ − L (17) Vk+1/kVk+1/k ≤ Tr(SBk+1QX(k+1/k)Bk+1 + QL(k+1) ) k/k ( s≥1) (12) with weight matrix P As s ≥ 1, we can further obtain that where

T T v ˆ  V V ≤ STr(B Q B + Q ) (13) xk/k −1 k+1/k k+1/k k+1 X(k+1/k) k+1 L(k+1) V =   T v  Lk  When a filter is stable, we can estimate E(Vk+1/kVk+1/k ) with the previous N (some time it is called the filtering  I  window) epoch data: A =   B 1 N −1 T T (14) E(Vk+1/kVk+1/k ) ≈ ∑(Vk+1/kVk+1/k ) −1 N j=0 xˆk/k−1 QX(k/k−1 ) 0  L =   , P =  −1  By combining Eqs 13 and 14 finally we can have  Lk   0 QL(k)  T The predicted residual vector can be stated: Vk+1/kVk+1/k S ≥ (15) ~ 1 T V = B Xˆ − L (18) (V V ) k k k / k −1 k N k−N + j/k−N k−N + j/k−N Because In practice, the number of measurements may vary from 2 −1 epoch to epoch, such as the GPS signal is blocked off by cov (Lk ) = σ L 0 Pk obstacles or the newly rising and down of satellite. Eq 2 −1 2 (15) can be modified as cov ( X k/k −1 ) = σ X 0 PX ( k/k −1) = σ X 0 Q X ( k/k −1) The covariance matrix of the predicted residuals can be expressed as:

~ T cov (V ) = σ 2 B P −1 B + σ 2 P −1 (19) k X 0 k X ( k/k −1) k L 0 Lk

Hu etal.: Adaptive Kalman Filtering for Vehicle Navigation 45

Therefore accelerations (X&&,Y&&,Z&&) are considered as the dynamic ~ T ~ ~ model noise. The double difference pseudoranges were E (V k PL V k ) = tr (PL D(V k )) = k k formed as the observation, and therefore, the receiver tr (P (σ 2 B P −1 B T + σ 2 P −1 )) Lk X 0 k X ( k/k −1) k L 0 Lk clock bias errors were not modeled in data processing. The conventional Kalman filtering method was firstly = σ 2 tr (P B P −1 B T ) X 0 Lk k X(k/k −1 ) k (20) used to process the DGPS data. In our test, two different 2 2 2 + σ L 0 m dynamic noise levels, σ X&& =0.1m/s and σ X&& =0.05m/s , had been chosen to analyze the dependence of the model where m is the number of measurements at the epoch. noise on the positioning performance. Fig.s 2 and 3 show In our approach, we only try to estimate the variance the positioning errors of the conventional Kalman filter factor of the dynamic model, as GPS measurement noise with these two dynamic noise levels. It is clearly shown can be assigned to a reasonable level based on the type of that the positioning errors are significantly different when GPS receiver used. Then the variance factor of the different dynamic noise levels were selected. The peaks 2 −1 in Fig. 3 are larger than those in Fig. 2. The RMS errors observation cov (Lk ) = σ L 0 PL is assumed to be k are 2.18 m (Easting) and 1.85 m (Northing) and 4.02 m known. From Eq. 20, the estimation of the variance factor 2 (Easting) and 2.80 m (Northing) for the noise levels of can be formed as 2 2 σ X 0 0.1m/s and 0.05m/s respectively.

~ ~ 2482100 σˆ 2 = []E (V T P V ) − σ 2 m / X 0 k Lk k L 0 (21) 2482050 −1 T tr (P B P B ) 2482000 Lk k X(k/k −1 ) k

2481950

Similarly we can derive the variance factor for the 2481900 Northing (m) dynamic model noise: 2481850

2 ~ T ~ 2481800 σ Ω 0 = [ ⋅ (V k PL V k ) k 2481750 24900 24950 25000 25050 25100 25150 25200 Easting (m) − σ 2 tr (P B Φ Q Φ T B T ) X 0 Lk k k,k −1 X ( k −1/k ) k,k −1 k Fig. 1 Trajectory of the Car − σ 2 m) ] / tr (P B Γ Q Γ T B T ) L 0 Lk k k,k −1 Ω ( k −1) k,k −1 k (22) 8

3

E 3 Examples N

Position Error (m) -2

In order to evaluate the performance of the adaptive -7 algorithms on GPS positioning, a number of tests were -12 1 21 41 61 81 101 121 141 161 181 carried out. In the first example, we used two TOPCON Epoch Java Legacy-E dual frequency GPS receivers. One receiver was set on the roof of the Tang Ping Yuan Fig.2 Positioning Error with conventional Kalman Filter ( =0.1 σ X&& Building in the Hong Kong Polytechnic University as a m/s2) reference station. Another GPS receiver was installed on roof of a car that was driven along the road, which is 12 about 15 km from the university. Fig. 1 shows the 8 trajectory of the car. Both dual frequency pseudorange 4 E and carrier phase measurements were collected during the 0 N test. The data collected were then post-processed using (m) Error position -4 in-house GPS software developed by Hong Kong -8 Polytechnic University. Firstly the reference trajectory -12 was obtained with kinematic GPS positioning using 1 21 41 61 81 101 121 141 161 181 Epoch carrier phase measurements. Then the L1 C/A code data were processed using different Kalman filters. A constant Fig. 3 Positioning Error with conventional Kalman Filter ( =0.05 velocity model is adopted as the dynamic model for the σ X&& Kalman filters. The state vector consists of geocentric m/s2) coordinate (X ,Y,Z) and velocity (X& ,Y&,Z&) . The It is much clearer to compare the estimated trajectories with differential pseudorange with the ‘true’ one

46 Journal of Global Positioning Systems

estimated by carrier phase measurements. Fig. 4 and 5 has better performance than the fading memory filter and show the ‘true’ trajectory estimated with carrier phase it is not affected by the selection of the initial dynamic measurements and the estimated trajectory using C/A noise level. A number of other tests have also been code pseudorange, using conventional Kalman filter. On carried out and the results confirm the above conclusions. the straight lines, the positioning errors are similar with different noise levels. However, with tight constraint on the dynamic noise level (0.05 m/s2), the positioning errors 8 are significantly larger when the car turns. 3 E N 2482100 -2 Position Error (m)

2482050 -7 2482000

2481950 -12 1 21 41 61 81 101 121 141 161 181 2481900 Northing (m) Epoch 2481850

2481800 Fig. 6 Positioning Error with Fading Memory Filter

2481750 24950 25000 25050 25100 25150 25200 Easting (m)

true conventional filter 12 10 Fig. 4 Estimated trajectory with conventional Kalman filter (σ =0.1 8 X&& 6 2 4 m/s ) 2 E 0 N 2482100 -2 -4 2482050 Position Error (m) -6 -8 2482000 -10

2481950 -12 1 21 41 61 81 101 121 141 161 181 2481900

Northing (m) Epoch

2481850

2481800 Fig.7 Positioning Error with Variance Estimation Filter

2481750 24950 25000 25050 25100 25150 25200 Easting (m) 230 ture conventional filter 210 True position Conventional filtering 190 Fig. 5 Estimated trajectory with conventional Kalman filter ( =0.05 σ X&& m/s2) 170 150 Then the same data set was processed using the adaptive Northing (m) filters discussed in section 2. Fig. 6 and 7 show the 130 positioning errors with the two adaptive Kalman filters 110 respectively. Comparing Fig. 5 and 6 with Fig. 1 and 2, it 90 is clearly shown that the positioning errors with the 20 40 60 80 100 120 Easting (m) adaptive filters are significantly smaller than the conventional filter. For fading memory filter, the (a)Conventional Kalman filter positioning errors are reduced to 0.91 m and 1.34 m RMS for easting and northing respectively. Better results can 230 be achieved with the variance estimation method, with 210 True position the RMS error of 0.72 m and 1.21 m for easting and Adaptive filtering northing respectively. Also, the errors in Fig. 6 and 7 are 190 uniformly distributed and this means the positioning 170 errors are not associated with the sudden manoeuvre 150 Northing (m) changes of the car. Tab. 1 summarizes the RMS error of 130 positioning error using different filtering algorithms. For the adaptive filters, the initial noise levels are chosen as 110 the same as the conventional Kalman filter. The dynamic 90 noise level strongly affects the performance of the 20 40 60 80 100 120 Easting (m) conventional Kalman filter. For fading memory filter, the positioning errors are significantly reduced. However, the (b)Adaptive Kalman filter positioning accuracy is still affected by the selection of Fig.8 Estimated trajectories with different filtering types for example 2 initial dynamic noise level. The variance estimation filter

Hu etal.: Adaptive Kalman Filtering for Vehicle Navigation 47

Tab. 1 Positioning Errors with Different Filters (Example 1) than the fading memory filter. However, the positioning Filter type Conventional Fading Variance accuracy with variance estimation filter is better. KF Memory Estimation Noise Level 0.1 0.05 0.1 0.05 0.1 0.05 Acknowledgements (m/s2) Easting (m) 2.18 4.02 0.91 1.28 0.72 0.72 Northing 1.85 2.80 1.34 1.55 1.21 1.21 This research is supported by Hong Kong RGC research grants: (m) PolyU 5062/02E and PolyU 5075/01E.

The second example comes from cross section surveying References of highway. Leica SR229 GPS receivers were used to survey the cross section profile (as shown in Fig. 8). The position of the profile is determined by GPS RTK Chen W. (1992) Integration of GPS/INS for Surveying technique with the accuracy of centimeter level. Then the Applications”, Ph.D thesis, University of Newcastle Upon conventional Kalman filters and the two adaptive filters Tyne. discussed in this paper are used to process differential psuedorange data. The positioning errors with these three Chen W, T. Cahit, R. Bingley, V. Ashkenazi (1999) Velocity methods are plotted in Fig. 8. It is clear to see in Fig. 8 Field Estimation of Geodetic Network Using Modified that the position estimation accuracy with the Kalman Filters, IUGG General Assembly, July 1999. conventional Kalman filter is much worse than that with Cui X.Z., Z.C. Yu, B.Z. Tao, D.J. Liu and Z.L. Yu (1992), the adaptive filters. The peak errors in Fig. 8 associated Generized surveying adjustment, Surveying and Mapping with the turns of the trajectory. Tab. 2 shows the RMS Publishing Company, China. error of the three filters. Gustafsson F. (2000) Adaptive filtering and change detection, Tab. 2 Positioning Errors with Different Filters (m) (Example 2) John Wiley&Sons Ltd. Filter type Convent- Fading Variance Estimation Hu C.W, W.Chen, Z.H.Zhao and Z.L.Li(2003) Map ional KF Memory Transformation and Rectification for Navigation by Easting 1.25 0.92 0.88 Global Positioning System Positioning, Journal of Tongji Northing 0.93 0.53 0.41 University, Jan. 2003,Vol.31 No.1 pp69-72. Jia P.Z. and Z.T. Zhu (1984), Optimal estimate and its 4. Conclusion application, Science publishing company, China Mohamed H. and K.P. Schwarz (1999), Adaptive Kalman Conventional Kalman filter is very sensitive to the Filtering for INS/GPS, Journal of Geodesy, vol 73, selection of the dynamic model noise level. One of the pp193-203. approaches tries to use adaptive filters to adjust the Wang J., M. Stewart and M. Tsakiri (1997), Kinematic GPS dynamic model noise based on the divergence of the positioning with Adaptive filtering techniques, IAG dynamic and the observation models. In this paper, two Scientific Assembly, Rio de Janeiro, Brazil, Sept 3-9, adaptive Kalman filtering algorithms have been derived, 1997, pp389-394 one is simply applying a scale factor to the predicted Yu M., W. Chen, Z. Li, Y. Chen and J. Chao (2002), A covariance matrix (fading memory) and another to Simplified Map Matching Algorithms for In Vehicle estimate the variance factor directly. The test Navigation Unit, Geographiic Inofrmation Sciences, Vol 8 demonstrates that both adaptive algorithms are better than No.1 the conventional Kalman filter, especially when the car changes its manoeuvre along the road. The computation for the variance estimation filter is slightly more complex