Adaptive Extended Kalman Filter for Aided Inertial Navigation System

GPS 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.


Introduction
GPS 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]  ; H -measurement matrix; zresidual measurement; r -measurement noise.
Let consider system one dimensional navigation system with accelerometer and GPS receiver.The accelerometer output signal is modeled as here a -true acceleration value; α -accelerometer scale factor; b a -accelerometer bias; ζ u -white Gaussian noise with PSD equal to 2 u  .Here nonlinearity in system is caused by the scale factor error.The position calculated from GPS measurements has the following model here s -true position value; b y -sensor bias; q s -white Gaussian measurement noise with PSD equal to 2 s  .Bias is modeled as random constant.
The system equation for navigation system is [6] ) ), ( ), ( ( with x  defined as here . The estimation of a ˆ can be done using (3) . 1 The dynamic coefficient matrix F can be derived using (1).And the error state vector for such system will be 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 Thus the measurement matrix H has following form 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.

Fig. 1. Closed loop implementation of EKF
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 k and Q k themselves are adapted as measurements evolve with time.
The innovation sequence e k at epoch k in the considered EKF algorithm is the difference between the residual measurement z k and predicted value of residual measurement: here k y ˆ -position calculated using IMU measurements; k y ~ -position calculated using GPS measurements.
The value of the innovation e k at the current epoch k cannot be predicted from previous values of it, and therefore each observation e 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:

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.

Fig. 2. Estimates of scale factor of MEMS accelerometer
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.

Fig. 3. Estimate of accelerometer bias
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.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.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.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.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.

;
P k(predicted) -predicted covariance of state matrix; K k -Kalman filter gain matrix.Knowing the innovation sequence, one can compute the innovation matrix k e C ˆ, 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 k e C ˆ and both Q and R is needed.

Fig. 6 .
Fig. 6.Vehicle velocity estimation during 80 s of GPS signal outage at time from 200 s till 280 s

Fig. 7 .
Fig. 7. Estimation of R value by adaptive EKFAgain not perfect results of Test 3 for position estimation with adaptive EKF were due imperfection of GPS and IMU data synchronization.

Table 1 .
EKF algorithm performance analyze during GPS signal outage 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