Neural network collaborative strong tracking filtering for vehicular tightly-coupled MIMUs/GPS

XIA Lin-lin, MENG Qing-yu, LIU Hui-min, MA Wen-jie, ZHAO Yao

(1. School of Automation Engineering, Northeast Electric Power University, Jilin 132012, China;2. College of Mechanical and Electrical Engineering, Qingdao Agricultural University, Qingdao 266109, China;3. Science College, Northeast Electric Power University, Jilin 132012, China)

Neural network collaborative strong tracking filtering for vehicular tightly-coupled MIMUs/GPS

XIA Lin-lin1, MENG Qing-yu1, LIU Hui-min2, MA Wen-jie3, ZHAO Yao1

(1. School of Automation Engineering, Northeast Electric Power University, Jilin 132012, China;2. College of Mechanical and Electrical Engineering, Qingdao Agricultural University, Qingdao 266109, China;3. Science College, Northeast Electric Power University, Jilin 132012, China)

The MEMS inertial measurement units (IMUs) consist of three orthogonal MEMS gyros, three orthogonal MEMS accelerometers, and three orthogonal micro magnetic sensors. Broadly speaking, an ideal positioning model of integrated navigation can be achieved by combining MIMUs (MEMS IMUs) with a miniature GPS receiver, and with this model, the performance of the tightly-coupled MIMUs/GPS mode in detecting position-velocity-acceleration (PVA) can be greatly improved. Specifically, due to the large drifts of micro inertial sensors, for the sake of a linear self-adaptive parameter model, a fusion filtering strategy is proposed, which fuses the strong tracking filter strategy in state predication, the quadratic EKF algorithm in measurement updates, and the artificial neural network (ANN) aided means in state estimation’s correction.Since the components involved are nonlinear in nature, a collaborative strong tracking filtering design is supposed for this neural network to guarantee that the above information processing model is capable of approaching the real characteristics of MIMUs/GPS and providing higher accuracy estimates of navigation parameters for land vehicular users. Experiments under dynamic environments demonstrate that the novel filtering method can be effectively applied in practical applications of navigation parameter estimation, even if the accuracy of the MEMS inertial sensors is modest.

MEMS technology; MIMUs/GPS; tightly-coupled; strong tracking quadratic EKF; neural network collaborative filtering

Compared with typical INS, MIMUs present prominent properties of small volume, light weight, low cost and low energy consumption[1-2], which are capable of accomplishing the real-time attitude and headingcontrol for vehicles under dynamic conditions. In this framework, an ideal positioning model of integrated navigation is achieved by combining MIMUs with a miniature GPS receiver. Refers to the physical interpretation of filtering, when it comes to the issue of multi- sensor information fusion tools for MIMUs/GPS, the extended Kalman filter (EKF) is a fair candidate with superior and efficient performances on recursive filtering. While,it is important to appreciate that, due to the large drifts of MEMS inertial sensors, once the state models or measurement ones involved are nonlinear, we can hardly prove the estimates error of the navigation parameters by EKF can converge to zero[3]. In practice, however, we are generally concerned with its effectiveness in practical applications.By a method similar to that adopted for typical tightly-coupled INS/GPS, pseudo range-pseudo range rate-heading combined form may be established and idealized as the measurement model, and an adaptive quadratic EKF (QEKF) relative to Hessian Matrix is stressed to compensate the quadratic truncation error extracted from Taylor Expansion. Meanwhile, a method of guaranteeing astringency and robustness of combined MIMUs/GPS consists of first considering how to sense the sudden changes of the states being estimated, just for that reason, a strong tracking filtering strategy which skillfully ignores the prevenient states information is correspondingly introduced. For further optimal calculation precision, by means of nonlinear mapping ability of neural networks, therefore, a RBFNN collaborative strong tracking filtering in terms of fusion information processing is proposed and expected to approach the real characteristics of MIMUs/GPS. Specifically, the collaborative filtering process consists of two parts, they are, respectively, offline training and online correction.As the essential parts of the paper, the elaborate design procedures will be presented in section 1-3, in addition, a simulation experiment is carried out in section 4 to demonstrate the superior performances of the proposed collaborative method.

1 Overall description of tightly-coupled MIMUs/GPS

When a miniature GPS receiver is installed in the platform of MIMUs, this combined system, in fact, can be seen as a low-cost solid state INS/GPS, which can generally provide time, attitude, velocity and position to moving bodies. Differing from the typical INS, the estimate of heading angle is subtly obtained from magnetic sensors[4-5]. In this set of equipment, it’s worth noting that, GPS realizes the position and velocity measurements, and the accelerometers, however, are not used as a direct attitude reference detecting meters (eg.,just as they acted in Ref. [6], being used to measure the gravitational component), they merely contribute to fulfill the updates of the velocity vector[7]. The block diagram of MIMUs/GPS and the corresponding diagrammmatic representation are shown in Fig.1.

Fig.1 Block diagram of MIMUs/GPS

1.1 System state model

Denote the navigation coordinate frame by E- N- U,where E, N, U express the east, north and up respectively.LetX(t)denote the system state model, since our application objects are land vehicles there is no need to take into account the velocity with respect to the vertical direction, thenX(t)can be expressed as

whereX(t)∈R15, δL,δexpress the latitude error and longitude error in geographic coordinate frame respecttively; δVE,δVNexpress the east velocity error and north velocity error respectively; φE,φN,φUexpress the attitude angle error of navigation platform;εband∇brespectively express the drift vector of MEMS gyros and the bias vector of MEMS accelerometers in body coordinate frame; δtris the receiver clock bias in meters,δtfis the receiver clock frequency bias in meters per second, and they are defined by

where, τεand τ∇denote time constants relating to the gyros’ drifts and accelerometers’ bias respectively; is the anti-correlated time parameter relating to the receiver clock bias; Wε,W∇andare all defined as white noises.

1.2 System measurement model

Typically, modernized GPS can realize accurate measurements of three original parameters, including pseudorange, delta range and Doppler shift[8]. As a consequence of universal tightly-coupled INS/GPS design, our measurement model is established according to pseudo range and pseudo range rate (measured via Doppler) equations.Note that, these equations are originally nonlinear in nature, so when we concern the information processing solutions, generally, nonlinear Gaussian EKF associated with Taylor Expansion Theory is a certainly reminiscent.

Take pseudo-range of GPS as an example, and note also that the position measurements of GPS are generally described in ECEF (Earth-Centered Earth-Fixed) coordinate frame. Let (x, y, z)Tandrespectively denote the true position vector of moving vehicle (being measured)and the satellite position vector via the j th satellite, the pseudo range measurements of GPS can be written as

Note that Eq. (4) is the actual range between satellites and moving vehicle. Where, n is the number of visible satellites, andis the composite of measurement errors with respect to j th satellite, known as white noise.

Denote the estimated position vector of moving vehicle (in ECEF coordinate frame) obtained from MIMUs by, and denote the pseudorange between moving vehicle and the jth visible satellite obtained from MIMUs by, we have

Let δr=rI-r denote the position error vector, and linearize (6) according to Taylor Expansion Theory, we have

Review the transformation of position error vector between ECEF coordinate frame and geodetic (g-)coordinate frame with the following form,

Now, the observation equation of the standard EKF can be expressed as:

Broadly speaking, the method chosen to solve the information processing of integrated navigation system should be that which approaches the real characteristics of nonlinear models as far as possible. For the system represented by the above Eq. (1) and Eq. (10), when the accuracy of the MEMS inertial sensors (so-called tactical grade sensors) is modest, typical EKF would suffer from large high-order truncation error that local linearization causes, which, definitely, is in conflict with strong nonlinearrity of MIMUs/GPS and influence the principal stability of filtering as well. The QEKF, in contrast, addersses this problem by compensating more nonlinear error via computing Hessian matrix, and QEKF itself follows the basic Taylor Expansion Theory, which is expected to acquire the optimal state estimates of MIMUs/GPS to relatively large extent. Moreover, concerning the demands of nonlinear smooth estimates, a type of adaptive strong tracking (ST) filter is introduced, with no more computational complexity or burden added[9], ST shows its distinguished sensitivity to states’ sudden changes.Together with QEKF, it constitutes the improved EKF model by which MIMUs/GPS exhibits its strong navigation applicability. The following section will concentrate on the detailed design procedures.

2 Design of strong tracking QEKF

2.1 QEKF

The main idea of QEKF is to reserve the quadratic term of Taylor Expansion Expression, therefore, for the formula (7) represented above, the problem-generated Hessian Matrix is employed to compensate the extracted truncation error that standard EKF raises. Suppose rI=r is the given value, and rewrite (7), we get

Considering the limited length of this paper, more detailed on Hessian Matrix calculations are available in Ref. [10] and Ref. [11], here, we simply present the concise results as follows:

● Pseudo_range Measurements

When the data is from the pseudo range measurements, the aim function derived can be expressed as

Let H1′(t) denote the observation matrix. The pseudo_range sub-system model can be described as

● Pseudo_range rate measurements

In like manner, when the data is from the pseudo_range rate measurements, the aim function is given by the transfer matrix between ECEF coordinate and navigation coordinate (E-N-U).

Now, the measurement equation of pseudo range rate sub-system can be similarly described as● Heading Measurement

Since the complete system reveals its great dependence on the magnetic field vector, the heading angle difference between the magnetic sensors’ output and MIMUs’ heading angle (termed as Euler angle together with pitch and roll) estimate can be expressed as

where, ψ1represents the magnetic heading in geodetic coordinate, Dis the local magnetic declination between geodetic north and magnetic north, and ψˆis the derived heading angle from MIMUs, then we have

Now, the measurement equation of unembellished heading sub-system can be described as

After composing (13), (15) and (18), the complete measurement equation can be given by

2.2 Principle of strong tracking filter

Consider the dynamic model of nonlinear discretetime system described below[12]

where, x∈Rn,u∈Rp, andy∈Rmbeing, respectively,state vector, input vector and output vector of the system at time k, the nonlinear functions f:Rp×Rn→Rnand h:Rn→Rmare all assumed to be continuously differentiable with respect tox, Γ(k) is a known matrix, ν(k)∈Rqand e(k)∈Rmrespectively represent the system noise and measurement noise, and they are independent with each other. Moreover, these two serials are all the Gaussian white noises with zero mean,and the relevant formula can be given as[13]

where, δkjis Kroneckerδfunction.

Refer to the Discrete-time updating equations of Kalman filter, the algorithm expressions of ST with one suboptimal parameter λ(k) can be written as follows:

For the previous variance estimate, it follows that

where,tr(·)denotes the tracing operator,ρ∈(0,1]being forgetting factor, typicallyρ=0.95[16-17].

In strong tracking model, the real-time gain matrix,predicted state estimate matrix and variance estimate matrix can be respectively given by

As described above, it should be noticed that ST is desired to implement the predictions of filtering,unlike that, however, QEKF plays a part in the updates of the filtering. Fig.2 shows the schematic of strong tracking QEKF.

Fig.2 Schematic of strong tracking QEKF

3 Further design of RBFNN collaborative filtering

Quite simply, the nonlinear estimates error of filtering is solved by reference to the strong tracking QEKF if high accuracy INS/GPS is taken into account,however, it is usually - though not unextended - desired to minimize the inferior influences coming from some unknown model error, like a variety of system noises,sensor bias and drifts etc[18]. Inspired by Ref. [19] and Ref. [20], a General Regression RBFNN collaborative filtering scheme is proposed, apart from the strong nonlinearity mapping ability, the desired RBFNN also possesses the following features:

● It shows superiority when the training samples are extremely large (1 state per sample period).

● It allows the enormous input & output neurons(both determined by the number of state parameters)and numbers of them greatly exceeds those available in hidden layer.

● It manifests less computational complexity and high computation rate with forward-back structure.

The so-called radial basis activation function with good analytical properties is applied to express the nonlinear mapping function of hidden neurons[21], it follows that

where, gi（x）is designated as the Gauss function,ci∈RIbeing the center vector, σiis the basis width vector,Iis the number of Gauss functions,denotes the Euclidean distance in norm form. The output of RBFNN can be described as (for the k-th neuron):

where, mrepresents the neuron number of output layer,wikis the weight vector between hidden layer and output layer. The topology architecture of the RBFNN for MIMUs/GPS navigation is given in Fig.3.

Fig.3 Topology of RBF neural network

where,being the input vector, which represents the predict value of the state,being the output vector, which represents the expected state estimates, m=15. Based on the principle research and many simulation experiments, three Gauss functions are chosen to summarize the rule from acquired data (input layer and output layer) and obtain inherent laws, and they are g1(x)=(0,0.1),g2(x)=(1.5,0.1), g3(x)=(-2,0.1). Besides, the initial weights associated withgi( ci,σi)(i=3) are respectively set asw1k=1, w2k=1, w3k=0.5.Inspired by Ref. [22] and Ref. [23], the learning method utilized includes the use of offline training and online correction.

3.1 Offline training

where, ΔE∈R15being the error between state estimatesobtained from complicated mapping process of RBFNN, and state estimatescoming from the state equation. To realize the online correction, these error data would be simultaneously stored in a certain database as the simulation experiment proceeds.

3.2 Online correction

The predicated state is accordingly being corrected based on the trained RBFNN. It is noting that the previous state vectorin strong tracking filtering would be substituted by vector,we may then rewrite the covariance matrix, gain matrix and update estimates of states. The graphic interpretation of this RBFNN collaborative strong tracking QEKF is shown in Fig.4.

Fig.4 Block diagram of RBFNN collaborative STQEKF

As illustrated above, two correlative steps are involved.

● After implementing the typical strong tracking QEKF, we may then get state estimates.

● On basis of obtaining the modified stateaided by online correction, we may then employ vectorinstead of vectoras the state estimates vector in the following training process.

4 Simulation experiment

4.1 Simulation environment

The numerical simulation is carried out to verify the performance of RBFNN collaborative strong tracking QEKF.Suppose that SV02, SV06 and SV11 (poor GDOP solution) are visible satellites (collected GPS data are obtained from National Geodetic Survey), and assume that the average acceleration of the moving vehicle is a=0.1g, the initial velocity is VE0=VN0=10 m/s, the initial position isL0=45.78°,λ0=126.67°, and the body plat base is in motion of lateral and vertical swing. The initial conditions of filtering are designated as:

● MIMUs

The initial longitude and latitude error are all 0.001°, the initial velocity errors are all 0.05m/s, and the initial attitude errors are 0.5°,0.5°, and 3° respectively. The initial attitude quaternion is q(1,0,0,0)Tand the application method adopted in quaternion propagation is 4-order Runge-kutta algorithm. Let the instantneous acceleration vector & magnetic vector bem/s2and m0=1.5×10-6·[1 0 1]TT (obtained from IGRF model, related to real-time longitude, latitude and altitude) respectively.

● GPS

The receiver clock bias is set to be 30m (1σ), receiver clock bias is set to be 0.05m/s (1σ), and time parameter relating to receiver clock bias is set to be α=0.001.

● MIMUs/GPS

The GPS outage is tied to 1pps (one pulse per second), but as with the sample period for MIMUs being 0.01s in this case, care must be taken to ensure an accurate overlapping status, therefore, the filter period is sampled as Tf=1.0s. The corresponding initial estimates of applied MIMUs are listed in Tab.1.

Tab.1 Initial estimates of MIMUs

4.2 Results and analyses

Fig.5 presents the graphs of g1(x)=(0, 0.1), g2(x)=(1.5, 0.1), g3(x)=(-2, 0.1) and weight sum of these three Gauss functions.

The graphs of estimated state parameters by RBFNN collaborative strong tracking filtering are shown in Fig.6-Fig.8, which contains the estimated attitude errors (in g-coordinate), estimated velocity errors (in g-coordinate)and estimated position errors (in ECEF coordinate).

In Fig.6 - Fig.8, the blue thick lines denote the estimated state errors by typical strong tracking QEKF,and the red slim lines denote the estimated state errors by RBFNN collaborative strong tracking QEKF. The performance comparisons of these two schemes are given in terms of mean & RMS data, as shown in Tab. 2.

Fig.5 Weight sum of Gauss activation functions

Fig.6 Estimated attitude errors of MIMUs/GPS

Fig.7 Estimated velocity errors of MIMUs/GPS

Fig.8 Estimated position errors of MIMUs/GPS

Tab.2 Mean and RMS of estimated state parameters

On basis of the above-mentioned results, we point out the following conclusions:

● RBFNN exhibits faster convergence speed and superior predictive capability, and it may then demonstrate the online correction has no impact on the real time requirements of practical navigation applications.

● The tightly-coupled MIMUs/GPS is not sensitive to

the drifts of MEMS gyros, and the filtering strategy is proved to be feasible for state estimates in cases where a class of MEMS inertial sensors is present.● RBFNN collaborative strong tracking QEKF shows its improved performances of compensating various noise errors, bias, etc., and the estimation accuracy of navigation state parameters by this is highly enhanced.

5 Conclusions

Multi-sensor information of MIMUs/GPS consists of the acceleration measured from MEMS accelerators,angular rate measured from MEMS gyros, magnetic intensity measured from micro magnetic sensors, and velocity and position measured from miniature GPS. By means of the optimized nonlinear approximation properties of proposed RBFNN collaborative strong tracking QEKF, the low cost tightly-coupled model is expected to exhibit superior dynamic behavior and be qualified to estimate optimal state parameters for moving vehicles. Furthermore, the simulation experiment results are presented to indicate that the high accuracy state estimates can be achieved on the considered occasion,the proposed filtering scheme may then be termed as a tool reference for a class of low accuracy integrated navigation systems preceded by strong nonlinearity hypothesis.

[1] Wang Ding-jie, Lv Han-feng, Wu Jie. Augmented Cubature Kalman filter for nonlinear RTK/MIMU integrated navigation with non-additive noise[J]. Measurement, 2017,97: 111-125.

[2] Yin Juan, Zhu Xin-hua, Wu Zhi-qiang. Design of automated batch calibrating system for MIMU[C]//2016 Tenth International Conference on Sensing Technology,Nanjing, China, 11-13 Nov. 2016: 1-10.

[3] Silvère Bonnabel, Erwan Salaün. Design and prototyping of a low-cost vehicle location system with guaranteed convergence properties[J]. Control Engineering Practice,2011, 19: 591-601.

[4] Doostdar P, Keighobadi J. Design and implementation of a SMO for a nonlinear MIMO AHRS[J]. Mechanical System and Signal Processing, 2012, 32: 94-115.

[5] Ligorio G, Sabatini A M. A linear Kalman filtering-based approach for 3D orientation estimation from magnetic/inertial sensors[C]//2015 IEEE International Symposium on Multi- sensor Fusion and Integration for Intelligent System, San Diego, USA, 14-16 Sept. 2015: 77-82.

[6] Batista P, Silvestre C, Oliveira P. Sensor-based long baseline navigation: observability analysis and filter design[J].Asian Journal of Control, 2014, 16(3): 1-21.

[7] Jose A R, Elecia W. Fusion filter algorithm enhancements for a MEMS GPS/IMU[C]//Proceedings of the Institute of Navigation National Technical Meeting (ION NTM),San Diego, USA, 28-30, Jan. 2002: 1-12.

[8] Tsai Yi-Hsueh, Chang Fan-Ren, Yang Wen-Chieh. Using delta range on GPS positioning, fault detection and fault exclusion[J]. Asian Journal of Control, 2011, 14(4): 936-946.

[9] Zhou D H. Introduction to adaptive control of non-linear systems[M]. Beijing: Publishing House of Tsinghua University, 2002.

[10] Zhao Lin, Xia Lin-lin, Cheng Jian-hua. The quadratic EKF approach for AHRS/GPS integration based on pseudo range-pseudo range rate-heading measurements model[J]. Journal of Harbin Engineering University, 2008,29(10): 1054-1059.

[11] Xia Lin-lin, Wang Jian-guo, Liu Yin-dong. Quadratic EKF algorithm enhancements for low cost tightly-coupled AHRS/GPS[C]//The 2nd International Symposium on Systems and Control in Aeronautics and Astronautics,Shenzhen, China, 10-12 Dec. 2008: 559-564.

[12] Mao Yang, Huang Bin-yang, Jiang Bo, et al. Real-time prediction for wind power based on Kalman filter and support vector machines[J]. Journal of Northeast Electric Power University, 2017, 37(2): 45-51.

[13] Hajiyev C. GNSS signal processing via linear and extended Kalman filters[J]. Asian Journal of Control,2011, 13(2): 273-282.

[14] Huang Wei, Xie Hong-sheng, Shen Chen, et al. A robust strong tracking cubature Kalman filter for spacecraft attitude estimation with quaternion constraint[J]. Acta Astronautica, 2016, 121: 153-163.

[15] Yao Yi, Cheng Kai, Zhou Zhi-jie, et al. A novel method for estimating the track-soil parameters based on Kalman and improved strong tracking filters[J]. ISA Transactions,2015, 59: 450-456.

[16] Zhang Long, Cui Nai-gang, Wang Xiao-gang, etc. Strong tracking-cubature Kalman filter for tracking ballistic reentry target[J]. Journal of Chinese Inertial Technology,2015, 23(2): 211-218.

[17] Guo Ze, Miao Ling-juan, Zhao Hong-song. An improved strong tracking UKF algorithm and its application in SINS initial alignment under large azimuth misalignment angles[J]. Acta Aeronautica et Astronautica Sinica, 2014,35(1): 203-214.

[18] Bhatt D, Aggarwal P, Devabhaktuni V, et al. A novel hybrid fusion algorithm to bridge the period of GPS outages using low-cost INS[J]. Expert Systems with Applications, 2014, 41(5): 2166-2173.

[19] Yin Jian-chuan, Zou Zao-jian, Xu Feng. On-line predicttion of ship roll motion during maneuvering using sequential learning RBF neural networks[J]. Ocean Engineering, 2013, 61: 139-147.

[20] Jwo Dah-Jing, Chuang Chih-Hsun, Yang Jing-Yu, et al.Neural network assisted Ultra-tightly coupled GPS/INS integration for seamless navigation[C]//12thInternational Conference on ITS Telecommunications. Taipei, Taiwan,5-8, Nov. 2012: 385-390.

[21] Dai Wu-chang, Wang Jian-guo, Xu Tian-xi. Estimation of battery state of charge based on neural network[J]. Journal of Northeast Dianli University, 2016, 36(5): 1-6.

[22] Tan Xing-long, Wang Jian, Han Hou-zeng, et al. Improved neural network aided GPS/INS integrated navigation algorithm[J]. Journal of China University of Mining &Technology, 2014, 43(3): 526-532.

[23] Xu Xiao-su, Zhou Feng, Zhang Tao, et al. Application of neural network by genetic algorithm optimization in SINS/GPS[J]. Journal of Chinese Inertial Technology,2015, 23(3): 322-327.

车载紧耦合MIMUs/GPS的神经网络辅助强跟踪滤波方法

夏琳琳1，孟庆瑜1，刘惠敏2，马文杰3，赵 耀1

（1. 东北电力大学 自动化工程学院，吉林 132012；2. 青岛农业大学 机电工程学院，青岛 266109；3. 东北电力大学 理学院，吉林 132012）

微惯性测量单元由三轴正交的微机械陀螺、加速度计和微型地磁传感器组成。将上述装置与GPS接收机组合，可构成最佳导航定位模型，其中紧耦合MIMUs/GPS对全导航参数（位置、速度及姿态）的测量精度可大幅提高。由于微惯性传感器具有大漂移特性，为获得具有自适应的线性参数模型，提出了融合滤波的信息处理方法，利用强跟踪滤波实现状态预测，二阶EKF实现测量更新，并借用神经网络技术完成对状态预测的修正。由于系统组件具有非线性，该神经网络辅助的强跟踪滤波方法旨在逼近MIMUs/GPS的真实特性，并为车载用户提供更为精准的导航参数信息。动态环境下的仿真试验表明，尽管MEMS惯性传感器的精度有限，所提出的方法能够有效用于实际的导航参数解算。

MEMS技术；MIMUs/GPS；紧耦合；强跟踪二阶EKF；神经网络辅助滤波

U666.1

：A

1005-6734(2017)03-0320-08

2017-03-27；

：2017-05-22

国家自然科学基金项目（61503073）；吉林省科技厅自然科学基金项目（20170101125JC）；吉林省教育厅科学技术研究项目（JJKH20170103KJ）；吉林市科技局杰出青年项目（20166005）

夏琳琳（1980—），女，博士，教授，从事组合导航研究。E-mail: prettylin521@aliyun.com

10.13695/j.cnki.12-1222/o3.2017.03.008