# Adaptive extended Kalman filter for aided inertial navigation system/Isplestinis Kalmano filtras automatinei inertinei navigacijai.

IntroductionGPS and micro-electro-mechanical (MEMS) inertial systems have complementary qualities that make integrated navigation systems more robust. The effective integration of GPS and MEMS sensors is still challenging task for low cost navigation system. Kalman filters are widely used for sensor data fusion and navigation in mobile robotics [1]. Taking in account GPS and inertial data processing problem nonlinearity, the Extended Kalman Filter (EKF) is used [2, 3].

One of the most important tasks in integration of GPS/INS is to choose the realistic dynamic model covariance matrix Q and measurement noise covariance matrix R for use in the Kalman filter [2, 4]. Adaptive algorithms automatically adjust Kalman filter system and measurement noise covariance matrix parameters taking in account navigation process performance i.e. position error [5]. In this paper innovation based adaptive EKF for adapting R and Q was used in order to improve navigation system performance during GPS signal outages.

EKF algorithm for one dimensional GPS/IMU system

In an aided inertial navigation system, the system process model comprises the INS mechanization, a description of the evolution of the navigation parameters (position, velocity and attitude), and the inertial sensor error models. Regardless of the choice of the INS error model, the linearized system model can be described as [5]

[delta]x = F([??],u, t)[delta]x + w , (1)

here F([??],u, t) = [partial derivative]f/[[partial derivative]x.sub.x=[??]]; F--dynamic coefficient matrix of system; w--process noise and x = [??] + [delta]x; x--estimated state of navigation system, computed using mechanization equation; [delta]x--error state, estimated by EKF; x--corrected state estimate.

In similar way the measurement equation can be presented as

z = y - [??] = H([??],t)[delta]x + r, (2)

here H([??], t) = [partial derivative]h/[[partial derivative]x.sub.x=x]; H--measurement matrix; z--residual measurement; r--measurement noise.

Let consider system one dimensional navigation system with accelerometer and GPS receiver. The accelerometer output signal is modeled as

u = a - [alpha]a--[b.sub.u] - [[zeta].sub.u], (3)

here a--true acceleration value; [alpha]--accelerometer scale factor; [b.sub.a]--accelerometer bias; [[zeta].sub.u]--white Gaussian noise with PSD equal to [[sigma].sub.2].sub.u]. Here nonlinearity in system is caused by the scale factor error.

The position calculated from GPS measurements has the following model

y = s + [b.sub.y] + [q.sub.s], (4)

here s--true position value; [b.sub.y]--sensor bias; [q.sub.s]--white Gaussian measurement noise with PSD equal to [[sigma].sup.2.sub.s]. Bias is modeled as random constant.

The system equation for navigation system is [6]

[??](t) = f (x(t), u(t), t), (5)

with x defined as

x = [[v a -[[lambda].sub.u][b.sub.u] + [w.sub.u] [w.sub.y] 0].sup.T], (6)

here x=[s v [b.sub.u] [b.sub.y] a]; s--estimate of position of vehicle; v--estimate of vehicle velocity; [b.sub.u]--estimate of accelerometer bias; [b.sub.y]--estimate of position bias calculation using GPS receiver; [alpha]--estimate of accelerometer scale factor; [w.sub.u] and [W.sub.y] are white Gaussian noise with corresponding PSD equal to [[sigma].sup.2.sub.u] and [[sigma].sup.2.sub.y]; [[lambda].sub.u]--is an inverse of correlation time of Gauss-Markov random process for accelerometer bias. The scale factor is modeled as random constant.

The navigation mechanization equation is

[??](t) = f ([??](t), u(t), t), (7)

here [??] = [[??][??] -[[lambda].sub.u] [[??].sub.u] 0 0].sup.T].

The estimation of [??] can be done using (3)

[??] = u + [[??].sub.u]/1-[??]. (8)

The dynamic coefficient matrix F can be derived using (1). And the error state vector for such system will be

[delta]x = [[delta]s [delta]v [delta][b.sub.u] [delta][b.sub.y] [delta][alpha]. (9)

There's no need to make linearization in order to obtain measurement matrix H, because the residual position measurement can be represented as linear combination of state vector components

z = [delta]s + [delta][b.sub.y] + [q.sub.s]. (10)

Thus the measurement matrix H has following form

H = [1 0 0 1 0]. (11)

Then determined above system and measurement equations should be transformed to discrete form and further processed using EKF algorithm. The implementation of algorithm is shown in the Fig.1.

[FIGURE 1 OMITTED]

Here, the errors estimated by the EKF are fed back every iteration, to correct the system itself, zeroing Kalman filter states in process. This feedback process keeps the Kalman filter states small, minimizing the effect of neglecting high order products of the states in the system model [3].

Innovation-based adaptive estimation of system and measurement noise matrices

In the innovation-based adaptive estimation (IAE) approach [4] the covariance matrices [R.sub.k] and [Q.sub.k] themselves are adapted as measurements evolve with time.

The innovation sequence [e.sub.k] at epoch k in the considered EKF algorithm is the difference between the residual measurement [z.sub.k] and predicted value of residual measurement:

[e.sub.k] = [z.sub.k] - [H.sub.k] [delta][x.sub.k], (12)

[z.sub.k] = [[??].sub.k] - [[??].sub.k], (13)

here [[??].sub.k]--position calculated using IMU measurements; [[??].sub.k]--position calculated using GPS measurements.

The value of the innovation [e.sub.k] at the current epoch k cannot be predicted from previous values of it, and therefore each observation [e.sub.k] brings new information. Hence, the innovation sequence represents the information content in the new observation and is considered as the most relevant source of information for the filter adaptation [4]. Based on the whiteness of the filter innovation sequence, the filter statistical information matrices are adapted as follows:

[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII] (14)

[MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], (15)

here [MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII]--predicted covariance of state matrix; [K.sub.k]--Kalman filter gain matrix. Knowing the innovation sequence, one can compute the innovation matrix [MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII], at epoch k, through averaging inside a moving estimation window of size N. In order to account for such an adaptive approach in the EKF algorithm, an additional block for computing the innovation matrix [MATHEMATICAL EXPRESSION NOT REPRODUCIBLE IN ASCII] and both Q and R is needed.

Tests and results

The navigation equipment was installed inside car. The navigation equipment consists of low cost BU-353 GPS receiver with USB interface connection port, MEMS IMU MotionNode. The data rate of GPS receiver is 1 Hz. The data rate of IMU is 50 Hz. The measurement data recorded onto HDD of notebook for further synchronization and processing by EKF algorithm. Three kinematics tests were conducted on the flat asphalt road. The vehicle accelerates till certain velocity, after that the velocity of vehicle remains the same during 1-2 minutes and after vehicle slowed down. Two tests (Test 1 and 2) were conducted at velocity 60 km/h, and third (Test 3) with velocity 70 km/h.

Accelerometer scale factor is not possible to estimate using linear KF. The estimated scale factors and bias of accelerometer using convenient EKF algorithm are shown in Fig. 2 and Fig. 3.

[FIGURE 2 OMITTED]

The step jumps of the estimation of scale factor are due to the switch over from the stage when system state (scale factor) is not observable due to the almost zero value of vehicle acceleration to the stage, when this state is observable in case of vehicle acceleration. The worst estimation is found for test #3. The reason of this is less exact synchronisation of GPS and IMU data achieved for this test.

[FIGURE 3 OMITTED]

The estimate of accelerometer bias for Test 1 is shown in the Fig. 3. The estimations of accelerometer bias during Test 2 and 3 are not shown here, as these ones are very similar to result of Test 1. As we can see from Fig. 3 the bias has considerable changes during vehicle movement at time from 200 s till 320 s.

For EKF algorithm is very important to set correctly initial values of predicted covariance of state matrix and initial values of estimated system state. If these values are set incorrectly, there's high risk of filter divergence. If the system dynamics is not known well, it's recommended to increase values of system noise covariance matrix.

Knowing noise specification of IMU MotionNode and GPS receiver, it was quite easy to adjust properly parameters for EKF except values for Q. The adaptive algorithm can be used for Q (and also R) values determining using equation (14)-(15). In order to check performance of this adaptive EKF some metrics were built.

[FIGURE 4 OMITTED]

The analysis of filter innovation sequence can indicate whether the filter processing is optimal or not. The innovation sequence of considered adaptive EKF and innovation sequence histogram are shown in Fig. 4-5. It's obvious that innovation sequence has Gaussian noise with zero mean sequences properties that means filter is in optimal mode. The mean value of innovation sequence is 0.0089 m with standard deviation 0.6599 m.

[FIGURE 5 OMITTED]

The more serious problem of proper EKF adjusting and navigation system performance increasing appears during simulating of GPS signal outages. In order to fix problem innovation based adaptive estimation was used. The vehicle velocity estimation (Test 1) during simulating GPS outages is shown in Fig. 6.

The velocity error increases with time, when simple EKF used, whereas velocity estimation with adaptive EKF is very near to reference value. The velocity reference value corresponds to estimated velocity, when GPS measurements are available.

[FIGURE 6 OMITTED]

The adaptation process of position measurement noise value for Test 1 is shown in Fig. 7. During period of GPS signal outage, we see abrupt increase of R value, because uncertainty of position measurements increase, as GPS signal is blocked.

The results of compare for conventional and adaptive EKF during GPS signal outage simulating are presented in Table 1-2. The position and velocity estimation error are much smaller for adaptive EKF except Test 3, where difference is not so big. The reason of this is that parameters of conventional EKF for Test 3 were carefully adjusted by numerous data processing cycles and even with this huge effort, the performance didn't reach one of adaptive EKF.

[FIGURE 7 OMITTED]

Again not perfect results of Test 3 for position estimation with adaptive EKF were due imperfection of GPS and IMU data synchronization.

It's necessary to precise inertial sensor noise (bias, scale factor) models in order to make further improvement of navigation algorithm performance during GPS signal outages. This can be done for example using Allan variance analysis of the accelerometer signal [3]. And after identifying sensor noise model, it's possible to include model parameters in adaptive algorithm for navigation parameter estimation.

Conclusions

The considered adaptive EKF has smaller estimation error, more robust to GPS signal outages. There's no need for solving problem of setting values for Q and R matrices, as these are adapted themselves using IAE approach. The drawback of the adaptive Kalman filter is a more complex algorithm which leads to an additional estimation block in the Kalman filter algorithm.

Received 2012 03 19

Accepted after revision 2012 05 13

References

[1.] Berntorp K., Arzen K., Anders Robertsson A. Sensor Fusion for Motion Estimation of Mobile Robots with Compensation for Out-of-Sequence Measurements // 11th International Conference on Control, Automation and Systems.--KINTEX, Gyeonggi-do, Korea, 2011.

[2.] Bistrovs V. Analyse of Kalman Algorithm for Different Movement Modes of Land Mobile Object // Electronics and Electrical Engineering.--Kaunas: Technologija, 2008.--No. 6(86).--P. 89-92.

[3.] Groves D. Paul. Principles of GNSS, Inertial, and Multisensor integrated Navigation Systems.--London: Artech House, 2008.--518 p.

[4.] Mohamed A. H., Schwarz K. P. Adaptive Kalman filtering for INS/GPS // Journal of Geodesy.--Heidelberg: Springer-Verlag GmbH, 1999.--No. 73.--P. 193-203.

[5.] Kluga A., Kluga J. Dynamic Data Processing with Kalman Filter // Electronics and Electrical Engineering.--Kaunas: Technologija, 2011.--No. 5(111).--P. 33-36. DOI: 10.5755/j01.eee.111.5.351

[6.] Farrell Jay A. GPS with High Rate Sensors.--USA: McGrawHill, 2007.--530 p.

V. Bistrovs, A. Kluga

Department of Transport Electronics and Telematics, Riga Technical University, Lomonosova iela 1, V korpuss, LV-1019, Riga, Latvia, e-mails: bistrov@inbox.lv, ansis.kluga@rtu.lv

http://dx.doi.org/10.5755/j01.eee.122.6.1818

Table 1. EKF algorithm performance analyze during GPS signal outage RMSE ** of Period of RMSE ** of velocity, Velocity *, GPS signal velocity, adaptive Test # km/h outage, s EKF, km/h EKF, km/h 1 60 200-280 6.70 0.54 2 60 80-160 6.11 1.74 3 70 60-160 4.79 1.72 Note: * typical profile of velocity during test is shown in Fig. 6 ** RMSE value during period of GPS signal outage Table 2. EKF algorithm performance analyze during GPS signal outage EKF Adaptive EKF RMSE * of [DELTA]S RMSE * of [DELTA]S Test # position, m **, m position, m **, m 1 69 140 2.96 4 2 53 120 2.64 5 3 13 21 6.70 12 * RMSE value during period of GPS signal outage ** Absolute maximal position error during GPS signal outage

Printer friendly Cite/link Email Feedback | |

Title Annotation: | AUTOMATION, ROBOTICS/AUTOMATIZAVIMAS, ROBOTECHNIKA |
---|---|

Author: | Bistrovs, V.; Kluga, A. |

Publication: | Elektronika ir Elektrotechnika |

Article Type: | Report |

Geographic Code: | 4EXLA |

Date: | Jun 1, 2012 |

Words: | 2178 |

Previous Article: | The approach to processing of biomedical experimental results/Biomedicininiu eksperimentu rezultatu apdorojimo budas. |

Next Article: | Study of frequency modulated boost converter operating in discontinuous conduction mode/Dazniu moduliuoto aukstinanciojo keitiklio, veikiancio... |

Topics: |