Mathematical Problems in Engineering

Volume 2014 (2014), Article ID 904062, 10 pages

http://dx.doi.org/10.1155/2014/904062

Research Article

Study of Robust Filtering Application in Loosely Coupled INS/GPS System

Lin Zhao,¹ Haiyang Qiu,¹ and Yanming Feng²

¹School of Automation, Harbin Engineering University, 145 Nantong Street, Harbin 150001, China

²School of Electrical Engineering and Computer Science, Queensland University of Technology, 2 George Street, Brisbane, QLD 4001, Australia

Received 15 April 2014; Accepted 15 May 2014; Published 4 June 2014

Academic Editor: Ligang Wu

Copyright © 2014 Lin Zhao et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract.

Since a celebrate linear minimum mean square (MMS) Kalman filter in integration GPS/INS system cannot guarantee the robustness performance, a filtering with respect to polytopic uncertainty is designed. The purpose of this paper is to give an illustration of this application and a contrast with traditional Kalman filter. A game theory filter is first reviewed; next we utilize linear matrix inequalities (LMI) approach to design the robust filter. For the special INS/GPS model, unstable model case is considered. We give an explanation for Kalman filter divergence under uncertain dynamic system and simultaneously investigate the relationship between filter and Kalman filter. A loosely coupled INS/GPS simulation system is given here to verify this application. Result shows that the robust filter has a better performance when system suffers uncertainty; also it is more robust compared to the conventional Kalman filter.

1. Introduction

Inertial navigation system (INS) is a self-contained system that had been widely used in military, mining, and tunnel construction in the recent decades. However, the INS parameter update functions are nonlinear, the process biases are coupled, and twice integration procedure definitely enlarges the noise influence, the dynamic error of INS keeps increasing with the time going on. Due to hardware and sensor limitation, low cost INS device can only provide an accurate navigation solution for a short period. To conquer this hardship, INS/GPS integrate system is proposed. The GPS is a time bias-free and precise system under less interference circumstance. Measurement of GPS can contribute a calibration to INS, which maintains the accuracy constant.

Estimation theory is widely used in integrate navigation field that blends and fuses information to get an optimal solution. For a typical INS/GPS system, the filter dynamic equation is normally chosen by the INS error state for a linear propagation property. It calls up various investigations based on the modification of a Kalman filter to improve the accuracy or to reduce the computation load of the system [1–3].

However, Kalman filter is the optimal estimation of minimum mean square error (MSE) for a linear time invariant (LTI) system; it has a strict requirement for the process noise covariance and measurement noise covariance . If the noise is Gaussian, Kalman filter is the linear minimum variance estimator and if the noise is not Gaussian, it is linear minimum variance estimator. In practical engineering problem, we cannot absolutely assure either the statistical character of the noise or of the dynamic model. And if the objective is to achieve a minimum worst-case estimator rather than minimum variance estimator, Kalman filter will not meet the requirement. In other words, Kalman filter cannot guarantee the robustness if the system contains structure or model uncertainty and lack of noise statistical character.

Motivated by this problem, precursors tried to improve the robustness performance although at the sacrifice of somewhat accuracy such as an EKF approach for parameter identification system [4]. filter is undoubtedly the best choice since it calls for no prior knowledge of the noise distribution. Also we see application in INS/GPS area [5–7], including bounded filter theory [8], and we will review the game theory which is also called the minimax theory used in this area. It is well known that no models can all the way precisely describe a system; huge efforts have been put into the research of Robust filer. This theory assumes that there exists some uncertainty in the system and tries to solve this problem by a quadratic stabilized constrain. Riccati equation and linear matrix inequality (LMI) are two main approaches to deal with robust problems. Compared to Riccati process, LMI does not make any assumptions about the parameters and can be solved efficiently with various MATLAB toolboxes [9, 10]. theory was first applied in the control area [11]; state space realization is a bond connecting it with a estimation theory. In fact, most of research of robust filter is based on the “conservatism,” an important index in robust control. To the authors’ knowledge, there is no investigation of the robust filter considered unstable factor in INS/GPS model; here we merely discuss this application and how it can be implemented.

In this paper, we firstly analyse the relationship between filter and Kalman filter and then modify the LMI design approach, making it possible to apply the robust filter for INS/GPS model; a classical 15-element (incorporating errors of attitude, velocity, position, and gyro and accelerator biases) state vector is used to check filter performance. The purpose of this paper is to make a summary and an easy access to filtering implementation in INS/GPS system.

The rest of the paper is organized as follows. Derivation of recursive filter formulation based on “game theory” [12] is introduced in Section 2; in Section 3 gives designing procedure for robust filter based on LMI; Section 4 discusses the Kalman divergence issue with uncertain system and method in dealing with unstable model when using LMI; in Section 5, we compare the robust filter and Kalman filter performance in different situation for loosely coupled INS/GPS model.

The notation used here is fairly standard: is a norm; denotes the largest singular value of ; denotes a transform operator from to ; and is th dimensional Euclidian state space. stands for the trace of matrix , symbol represents the symmetry term in a symmetric matrix, means the supremum, and represents the transposed matrix .

2. Game Theory Recursive Filter

Since many previous authors had utilized filter in INS/GPS model, we discuss the game theory approach in this section as a fundamental illustration of the robust filter. In order to give a definition of norm. We begin with the norm. For a continuous infinite systems during the period , norm is defined as

where to a discrete system, suppose the sampling number is in the period ; then

reflects the meaning of “energy” of the variable. Then the norm of a linear, time-invariant operator is defined as

where denotes the max singular value of the square matrix and is the frequency parameter in the range of . In frequency domain, norm means a peak value of all the frequency ranges, while in time domain, it means the largest energy “transfer” form variable to variable ; in other words, has the largest influence on . So what is the filter problem? Consider the following state-space system:

where is the dynamic state, is the measurement, is the output to be estimated, is linear combination of , and and are the process and measurement noise, respectively. is controllable and is detectable. Note that it is detectable but not “observable”; in an INS/GPS model, the measurement is often “detectable” not “observable.” Now, consider such a cost function:

where , , , and are symmetric, positive definite weighting matrices, and formulation . Note that these weighting matrix are not noise covariance matrix in a kalman filter, they can be set arbitrary by the filter designer, of cause, can set to be equal with Kalman filter’s , , .

The filter problem is to find an estimation strategy which minimizes . The famous game theory depicted the estimation of cost function as a dynamic, two-person game: one is the engineering and the other say the nature. The goal of engineering is to find a proper estimation that minimizes , while his opponent, the nature tries to maximize from the disturbances and initial state adversely. As a contrast to Kalman filter (KF), what KF pursuits is to minimize , but it cannot guarantee the under some fixed extent, meaning that there may be a huge noise influence on the estimation result when achieves the minimum MSE.

Substituting, ; this problem can turn to seeking a saddle point , satisfying

In fact, this saddle point, that is, the smallest , is not tractable in normal case. In general, people look for a closed form solution to the suboptimal estimation such that in Figure 1: the sole saddle point, which lies in the crossing of the upside and bottom surfaces, is difficult to achieve, while suboptimal question guarantees the solution in the range of the two planes that contains the saddle point.

Figure 1: Game theory filter model.

A summary for the game theory filter approach is: fix a prescribed parameter satisfying , ; try to achieve minimum subject to (5). This derivation process can be implemented by using the Lagrange multiplier [13, 14]. The steady-state filter formulations are given by

For an intuitive contrast with recursive Kalman filter, some transformations are carried out here. A typical Kalman gain can be written as

Premultiplying outside by and postmultiplying each term inside by the inverse of , we obtain

Postmultiplying outside the parentheses by the inverse of and premultiplying each term inside parentheses by the inverse of , we have

So the reformed Kalman filter is given as

Note that there is in (10) while in (11). For a LTI system, it is easy to prove that the and converse to the same stable constant when , proof procedure is omitted here for brief. Now suppose that ; the filter reduces to Kalman filter fantastically. Again, analogous to Figure 1, if the up half-surface represents the performance of the Kalman filter, this filter will pursuit the minimum MMS point, that is, ellipsoid endpoint without concerning the noise transform level. We treat the as a filter criterion, and minimax game theory provides a guarantee about this, and the noise attenuation parameter is also the key index in robust filter.

3. Robust Filter

Consider a system

Note that the difference with system (1) lies in the description of noise. Setting , , and we can get the above formula. In this term, the disturbance process and measurement noise are seen as a united element.

For a system, it is impossible to obtain an exact system model due to the presence of inherent parameter uncertainty instinct. The matrices , , , are not exactly known or depend on some time-varying parameter that vary in a fixed bound set. This is called “polytopic model” with the character

The vector time-varying parameter belongs to the unit simplex as

Any uncertain matrices set belongs to a polytopic model with convex bound uncertain domain ; hence any , can be written as a combination of vertices of the convex polytope. And the model is supposed to be entirely known if the vertices number .

Another important descriptor is the norm-bounded uncertainty. The mode subject to norm-bounded parameter uncertainty is written in the form

for the uncertain matrix parameter such that , , and are known matrices.

The purpose of the filter problem is to design an estimation which is given by , where is an operator filter set with the form

for all the admissible uncertainty such that . The estimation error can easily be yielded as . Construct state variable , the closed loop state-space form is

where

Defining the transfer function from the noise input to estimation error is ; hence . Robust filtering problem: determining , , , for all the admissible uncertainty , satisfy the following:

(i)model (17) is asymptotic stabilization;(ii)transfer function infinity norm holds that .

According to the bounded real lemma, the closed loop system (17) with the upper bounded is asymptotically stable if and only if there exists a matrix to such the algebraic Riccati inequality:

Implement Schur complement lemma, this inequality is equivalent with the LMI:

3.1. LMI Approach to Robust Filtering

The LMI can be solved efficiently by the Matlab LMI Toolbox; however, two problems have to be considered.

(i)Solutions to LMI (20) are , , , so the relationship between inequality and variables, , must be founded.(ii)The inequality should be transformed into linear convex optimisation problem, so it can be addressed by LMI approach.

Considering the formulation (20), partition together with its inverse as

substituting the partitioned matrix in (17) with , , , ; the inequality changes into

where denotes the symmetric block. The next step is converting this joint nonlinear matrix into LMI form [15]. Making an equivalent transform by permuting third and fourth columns and rows, we obtain

Define the nonsingular matrix

Applying the congruence transformation, pre- and postmultiplying and , the inequality can be obtained as

where , , , are matrix variables defined as

The objective is to determine the feasible solutions to the LMI; hence the filter matrices can be obtained:

and matrices , can be set arbitrarily to be nonsingular as

Here the filtering problem can be solved by a convex LMI optimisation approach. Give a summary to this filter problem. LMI filtering: minimise subject to inequality (26) hold. Based on the inherent properties of polytopic system, this approach can be extended to a robust filter problem. Substitute , , , by the vertex matrices , , , of , and guaranteeing that all the vertex LMIs hold, the robust problem can be solved.

It should be highlighted that this formulation can reduced to standard Kalman filter when restricting some special case. Suppose that and ; a standard Kalman state description (4) can recovered from (12) when replace this strict LMI (21) by a non strict one through choosing that , , and ; then the remaining variable should hold the LMI as

Define the new variable and . Applying the Schur complement to (29), we obtain

If the parameter (30) is reduced to

the optimal solution is arbitrarily close to the solution of this algebraic Riccati equation when replacing the inequality by a same equality. And this Riccati equation solution is our familiar estimation error covariance , when , also called stable state of the Kalman filter for a LTI system. Defining and and holding (28) we have

4. Robust to INS/GPS Loosely Coupled Model

4.1. INS Error Model

The estimation objective in GPS/INS system is the INS error. Filter for this INS dynamic error model consists of 15 elements such as

where , , are the pitch, roll, and yaw errors, , , are the east, north, and up velocity errors, , , are the latitude, longitude, and height errors, , , , and , , are the gyroscope and accelerator bias errors in subscript orientation. Assume that the gyroscope and accelerator have a Gaussian white noise. The transition matrix can be written as

is the 9 fundamental parameter transfer matrix of which element detailed definition can be found in [16], is identity matrix, and is the transfer matrix between fundamental parameter and INS sensors bias, written as

is the rotation matrix to transform the states from body frame to navigation frame. It is obviously that is a time-varying matrix determined by the real attitude , velocity , and position , although can be seen as a piecewise approximate constant matrix when implementing filtering for each epoch. estimation calls for no prestatistical character of noise, but accurate system matrices like , , , . If the INS output is corrupt, then cannot be guaranteed, neither is the estimation state accuracy . And that is why a robust filter is designed for such a case.

4.2. Alternative LMI Approach to Unstable Model

It can be easy found that in a LMI approach to solve the filter context, a precondition that transfer matrix be stable must be satisfied. The reason for this is that since the state-space model cannot be completely cancelled in the estimation error, they must be stable to guarantee the boundness of estimation error. And to apply LMI (21), it must guarantee diagonal element strictly less than 0. If is unstable, , the first element of (21) is infeasible. To deal with this tough situation, an alternative LMI approach is proposed for coping with unstable model [17, 18]. Consider system (12), its state vector can be partitioned as

and the system matrices partitions are

where the subscript “” denotes the stable component and “” denotes the unstable component.

Assumption. Consider the following.

(i)The subsystem is quadratically stable.(ii)The matrix is unstable.(iii) is detectable and is controllable.

Next, partition the filter matrices, and we obtain

Change the variable with a augmented state, will become

Equation (17) is reformed as

where

So the partial state is bounded while is unbounded. If and are bounded, there must be assured that and , also meaning that

Now, we have solved the unstable problems and the remaining processor can use a typical LMI approach to the robust filtering. Since we cannot remove the unstable part from an entire system, the only choice is to try a decomposition of the stable and unstable parts; the key factor lies in eliminating the unbounded estimation influence rendering that the final result is not corrupted.

4.3. Uncertainty to Kalman Filter Divergency Issue

Another drawback when applying Kalman filter to an uncertain and unstable system is divergency issue. The steady state of (4) is

The estimation error dynamic function is

If is stable, then error dynamic function is stable, meaning KF is stable. Now, suppose that is uncertain, and the real matrix is , and is the nominal matrix in filtering. We obtain

The above formulation can be written as

If and are both unstable, when , can be arbitrarily large, so even is small; hence can be arbitrarily large, and the filer is divergent.

5. Simulation Result

The state model is as (34), observations are the difference between INS and GPS position measurement. This simulation chooses a static condition, and its period lasts for 500 s. Gyros constant drift and random white noise 1 sigma are and ; accelerator constant drift and random white noise 1 sigma are 100 μg and  μg. GPS position measurement white noise 1 sigma is 1 m/s. Corresponding correlation noise matrices are set as

Initial estimation error correlation matrix can be given as

This simulation software details can be found in [19], since compared to INS, GPS measurement is more easily interfered; two scenarios are considered.

(i)Normal situation means that there is an accurate prior information for the noise character.(ii)Interfered situation means that the GPS measurement noise is 5 times compared to nominal noise character during 100–200 s.

Results are shown in Figures 2 and 3; pink curve represents the real value. Subpictures show the error attitude, error velocity, error position, gyro bias, and accelerator bias in turn. Because the up direction is divergent, only north and east results are shown here.

Figure 2: Normal situation Kalman filter and robust filter results.

Figure 3: Interfered situation Kalman filter and robust filter results.

Triple INS compensate parameters , , statistical index STD is shown in Tables 1 and 2 for these two situations.

Table 1: Normal situation STD index.

STD
Kalman
Robust

Pitch
Roll
Yaw
Pitch
Roll
Yaw

0.1130
0.1386
2.4796
0.0129
0.0149
0.035

North
East
Up
North
East
Up

m/s
0.0296
0.0266
Nan
0.0366
0.0519
Nan

m
0.2806
0.3487
Nan
0.6351
0.8284
Nan

Table 2: Interfered situation STD index.

STD
Kalman
Robust

Pitch
Roll
Yaw
Pitch
Roll
Yaw

0.1430
0.1786
2.9196
0.0159
0.0199
0.055

North
East
Up
North
East
Up

m/s
0.0473
0.0565
Nan
0.0372
0.0568
Nan

m
0.4206
0.5787
Nan
0.8321
0.9674
Nan

In normal situation, KF shows a more “stable” and “accurate” performance than filter; the filter exhibits a jitter character in the tracking process of real value, whereas in interfered situation, filter is more robust, takes account of the north , filter STD varies from 0.0366 to 0.0372 while KF from 0.0296 to 0.0473. Note that filter contributes a better performance to in both situations because filter has a “weak” effect on estimation error matrix which means that the initial value has a weak influence. In the follow scenario, we extend the simulation time to 1500 s without calibration to INS using the filter output, result is shown in Figure 4. The Kalman filter suffers a divergence. Although there is a large trend of the estimation error, is stable during this period.

Figure 4: Normal situation Kalman filter and robust filter results for 1500 s.

6. Conclusion

In this paper, we design a robust filter for a loosely coupled INS/GPS model. Firstly, a game theory gives a approach in a Kalman form; it can be easily transformed by adjusting the noise attenuation parameter . Next, a robust filter based LMI is discussed; the unstable case is considered. We also illustrate that the robust filter based on LMI approach can convert into standard KF in special conditions. For a direct performance contrast, simulation result shows the INS error estimate state that is not compensated for INS. This proposed approach may be applied in a system under uncertainty such as low-cost IMU and interfered GPS environment.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgments

This research was funded by the State Scholarship Fund of the China Scholarships Council (CSC). The authors would like to thank all the editors and anonymous reviewers for improving this paper.

References

J. Ali and F. Jiancheng, “SINS/ANS/GPS integration using federated kalman filter based on optimized information-sharing coefficients,” in Proceedings of the AIAA Guidance, Navigation, and Control Conference, pp. 1–13, August 2005.

E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman filtering for spacecraft attitude estimation,” Journal of Guidance, Control, and Dynamics, vol. 5, no. 5, pp. 417–429, 1982.

A. H. Mohamed and K. P. Schwarz, “Adaptive Kalman filtering for INS/GPS,” Journal of Geodesy, vol. 73, no. 4, pp. 193–203, 1999.

G. Sun, M. Wang, and L. Wu, “Unexpected results of extended fractional kalman filter for parameter identification in fractional order chaotic systems,” International Journal of Innovative Computing, Information and Control, vol. 7, no. 9, pp. 5341–5352, 2011.

X. F. He, Y. Q. Chen, and B. Vik, “Design of minimax robust filtering for an integrated GPS/INS system,” Journal of Geodesy, vol. 73, no. 8, pp. 407–411, 1999.

X.-K. Yue and J.-P. Yuan, “ sub-optimal filter for low-cost integrated navigation system,” Chinese Journal of Aeronautics, vol. 17, no. 4, pp. 200–206, 2004.

N. Abdelkrim, N. Aouf, A. Tsourdos, and B. White, “Robust nonlinear filtering for INS/GPS UAV localization,” in Proceedings of the Mediterranean Conference on Control and Automation (MED '08), pp. 695–702, June 2008.

G. A. Einicke, G. Falco, and J. T. Malos, “Bounded constrained filtering for GPS/INS integration,” IEEE Transactions on Automatic Control, vol. 58, no. 1, pp. 125–133, 2013.

J. Löfberg, “YALMIP: a toolbox for modeling and optimization in MATLAB,” in Proceedings of the IEEE International Symposium on Computer Aided Control System Design, pp. 284–289, September 2004.

P. Gahinet, A. Nemirovskii, A. J. Laub, and M. Chilali, “LMI control toolbox,” in Proceedings of the 2nd IEEE International Symposium on Requirements Engineering, pp. 2038–2041, March 1994.

L. Wu, J. Lam, W. Paszke, K. Gałkowski, E. Rogers, and A. Kummert, “Control and filtering for discrete linear repetitive processes with and performance,” Multidimensional Systems and Signal Processing, vol. 20, no. 3, pp. 235–264, 2009.

U. Shaked and Y. Theodor, “H-optimal estimation: a tutorial,” in Proceedings of the 31st IEEE Conference on Decision and Control, pp. 2278–2286, 1992.

D. Simon, Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches, Wiley-Interscience, 2009.

X. Shen and L. Deng, “Game theory approach to discrete filter design,” IEEE Transactions on Signal Processing, vol. 45, no. 4, pp. 1092–1095, 1997.

S. H. Jin and J. B. Park, “Robust filtering for polytopic uncertain systems via convex optimisation,” IEE Proceedings: Control Theory and Applications, vol. 148, no. 1, pp. 55–59, 2001.

A. Noureldin, T. B. Karamat, M. D. Eberts, and A. El-Shafie, “Performance enhancement of MEMS-based INS/GPS integration for low-cost navigation applications,” IEEE Transactions on Vehicular Technology, vol. 58, no. 3, pp. 1077–1096, 2009.

L. Xie, C. E. de Souza, and Y. C. Soh, “Robust filtering for uncertain systems with unstable modes,” in Proceedings of the 33rd IEEE Conference on Decision and Control, pp. 3929–3930, December 1994.

K. Barbosa, C. E. de Souza, and A. Trofino, “Robust filtering for uncertain linear systems with unstable modes,” in Proceedings of the IFAC Symposium on System, Structure and Control, vol. 4, pp. 253–258, 2004.

Y. Gomgmin, “Strapdown inertial navigation simulation toolbox in matlab,” Northwestern Polytechnical University of China, 2013, http://blog.sina.com.cn/u/1089338825