8/10/2019 25-Shi

1/5

An Adaptive Robust Estimation Algorithm for

MEMS-INS/GPS

Jing ShiSchool of Automation,Northwestern Polytechnical University,

Xian 710072, China

Email: shijingac@126.com

Jianhua YangSchool of Automation,Northwestern Polytechnical University,

Xian 710072, China

Email: yangjianhua@nwpu.edu.cn

Huiying LiuSchool of Automation,Northwestern Polytechnical University,

Xian 710072, China

Email: lhy2005@nwpu.edu.cn

AbstractIn micro-electro-mechanical system based inertialnavigation system (MEMS-INS)/global positioning system(GPS)integrated navigation systems, there exist unknown disturbances.This paper deals with the problem of adaptive robust estimation(ARE) to obtain high estimation accuracy. A filter gain matrix isdesigned to obtain the adaptive robust algorithm, in which theH norm of the transfer function from noise inputs to estimation

error outputs is limited within a certain range, and further morethe estimation error is minimized by calculating the adaptiveweight matrix. Therefore, the algorithm can guarantee smallestimation errors in the presence of disturbances. The proposedmethod is evaluated in the integrated navigation system, and thesimulation results show that it is effective in position and velocityestimation.

I. Introduction

Because of low cost and small scale, MEMS based INSshave gained considerable attention in recent years. INS andglobal navigation satellite system (GNSS) such as GPS arewell known to be an ideal pair for sensor fusion and temporalfiltering. However, the errors of MEMS inertial sensors arecorrelated and complex, so it is difficult to obtain the accuratestochastic model and the exact priori information and, as aresult, the filtering accuracy will be reduced, and even thefilter will be divergent [1].

The goal of ARE is to estimate the system state accuratelyin the presence of disturbances, with as small estimationerrors as possible. Many researchers have devoted their effortsto the study of adaptive filtering concerned with unknowndisturbances and/or noises. For example, in [2]-[4], adaptivefilters are designed based on Kalman filter with the assumptionof white noise, which may diverge in the presence of colornoise. In [5]-[7], robust estimators are designed to limit theworst case H norm of a transfer function from noise inputs

to estimation error outputs within a certain range. In [8], arobust Kalman-Bucy filter is designed to guarantee that thesteady-state variance of the state estimation error is upperbounded by a certain constant value for all admissible modeluncertainties. In [9], a robust estimator has been formulatedwithout minimizing the estimation error. In the above studies,filtering accuracy is sacrificed in exchange for robutness.

Low accuracy may not meet the requirement of somepractical application, such as the MEMS-INS/GPS. The mainpurpose of this paper is to develop a robust algorithm as wellas minimize the estimation error. We will give the design

objective of ARE first, then formulate the problem as to thedesign of adaptive weight matrix and filter gain matrix.

Notation.Rn denotes the n dimensional Euclidean space.M2 denotes the 2-norm of matrix M. (k) l2[0,N] meansN

k=0T (k) (k) < . col {i}

Ni=0 denotes the column vector

stacked by vectors 0, 1, . . . , N.

mds

September 25, 2014

II. Problem formulation

Consider the descriptor system as follows:

x(k+ 1)= A(k+ 1, k)x(k) +Bd(k)d(k) (1)

z(k)= C(k)x(k) + Dd(k)d(k)) (2)

where k {1, 2,...} is the discrete time, x(k) Rn is the state

vector, z(k) Rm is the measurement vector where x(k) Rnis the state vector, y(k) Rm is the measurement vector, andd(k) Rnd is the uncertain/unknown disturbances and noise.All coefficient matrices in (1) are bounded functions of timetkwith compatible dimensions.

Assumption 1. Bd(i)DTd

(j) = 0(0 i, j k), i.e. the plantand measurement noise are independent.

The estimate of x(k) is given as follows:

x(k+ 1)= A(k+ 1, k) x(k) + K(k+ 1)(y(k+ 1)C(k+ 1)A(k+ 1, k) x(k))

(3)

and the estimation error is

e(k)= x(k) x(k) (4)

where x(k) is the state estimate of x(k), and K(k) is the filtergain matrix. The initial estimation error is e(0)= x(0) x(0).

Assumption 2.The initial estimation error e(0) is indepen-dent with the plant and measurement noise.

In order to guarantee the estimation accuracy in the pres-ence of uncertain disturbances, we aim to obtain such a filtergain matrix K(k+ 1) that the H norm of the transfer function

8/10/2019 25-Shi

2/5

from the noise inputs to the estimation error outputs is limitedwithin a certain range, i.e., >0

ged = supdl2[0,k]

ki=0

e(i)2

e(0)2 +k

i=0d(i)2

< ( >0), (5)

Based on the above discussion, we are in the position tostate the ARE problem as follows: design K(k+ 1) to satisfyEq. (5) and minimize the estimation error.

The filter is given as follows [9]:

x(k+ 1)= A(k+ 1, k) x(k) + K(k+ 1)(y(k+ 1)

C(k+ 1)A(k+ 1, k) x(k))(k+ 1)= y(k+ 1) C(k+ 1)A(k+ 1, k) x(k)x(0)= x0

(6)and the estimation error is

e(k)= x(k) x(k) (7)

where x(k) is the state estimate of x(k), (k) is the generatedresidual, K(k+ 1) is the filter gain matrix, and x0 is a guessof initial state.

III. MainResults

In this section, the ARE algorithm is proposed.

The adaptive weight matrix is necessary to yield theadaptive filter gain matrix. The filter gain matrix K0(k + 1)can be derived from:

minJ(K0(k+ 1))=

1

k

k

i=0

Ee

T

(i + 1)e(i + 1)

where

eT(i + 1)e(i + 1)= [(A(i + 1, i) K0(i + 1)C(i + 1)A(i + 1, i))e(i)+

(I K0(i + 1)C(i + 1))Bd(i)d(i)

K0(i + 1)Dd(i + 1)d(i + 1)]T

[(A(i + 1, i) K0(i + 1)C(i + 1)A(i + 1, i))e(i)+(I K0(i + 1)C(i + 1))Bd(i)d(i) K0(i + 1)Dd(i + 1)d(i + 1)].

The necessary condition for K0(k+ 1) to yield a minimum

J(K0(k+ 1)) is J(K0(k+1))

K0(i+1)

= 0,

K0(i + 1) = P0(i + 1, i)CT(i + 1)(C(i + 1)P0(i + 1, i) C

T(i + 1)+

Dd(i + 1)Ed(k+ 1)dT(k+ 1)

DT

d(i + 1))1,

(0 i k)

with

P0(i + 1, i) = A(i + 1, i)Ee(i)eT(i)

A(i + 1, i) +Bd(i)B

Td(i).

Since the measurement noise d(k+ 1) is uncertain, without

loss of generality, it is assumed that Ed(i + 1)dT(i + 1)

= I,

and using the following adaptive weight matrix (k+ 1) tocorrect, that is

K0(i + 1)= P0(i + 1, i)CT(i + 1)(C(i + 1)P0(i + 1, i)

CT(i + 1) + (i + 1)Dd(i + 1)DTd

(i + 1)T(i + 1))1

(8)where

(i + 1)= diag {1(i + 1), 2(i + 1),...,m(i + 1)} ,

j 0, 1 j m.

The residual signal of the estimation is:

(k+ 1)= y(k+ 1) C(k+ 1)A(k+ 1, k) x(k)= C(k+ 1)A(k+ 1, k)e(k)+

C(k+ 1)Bd(k)d(k) + Dd(k+ 1)d(k+ 1)(9)

The variance of the residual is expressed as:

S(k+ 1)= E(k+ 1)T(k+ 1)

= C(k+ 1)P(k+ 1, k)CT(k+ 1)+

Dd(k+ 1)Ed(k+ 1)dT(k+ 1)

DT

d(k+ 1)

= C(k+ 1)P(k+ 1, k)CT(k+ 1)+

(k+ 1)Dd(k+ 1)DTd(k+ 1)T(k+ 1)= U(k+ 1) + V(k+ 1)

(10)

where

U(k+ 1)= C(k+ 1)P(k+ 1, k)CT(k+ 1),

V(k+ 1)= (k+ 1)Dd(k+ 1)DTd(k+ 1)

T(k+ 1).

Without loss of generality, it is assumed that

C(k+ 1)=

Imm 0m(nm)

,

then we have

U(k+ 1)= Imm 0m(nm) P(k+ 1, k)11,mm P(k+ 1, k)12,m(nm)P(k+ 1, k)21,(nm)m P(k+ 1, k)22,(nm)(nm)

Imm0m(nm)

= P(k+ 1, k)11,mm

Letuii(k+1) and pii(k+1, k)11,mm be the diagonal elementsofU(k+ 1) and P(k+ 1, k)11,mm, respectively, then

uii(k+ 1)= pii(k+ 1, k)11,mm, 1 i m.

Using N residuals from (9), the mean of each measure-ments variance can be calculated as:

vi(k+ 1)= 1

N

Nj=0

2i(k+ 1 N+ j), 1 i m (11)

where, i(k+ 1) is the ith element of(k+ 1).

As both (10) and (11) simultaneously contain the informa-tion of residual, the weight matrix (k+ 1) can be obtainedfrom these equations, and it can be derived from

mini0

J(i) = (vi(k+ 1) pii(k+ 1, k)11,mm id

2d,ii(k+ 1))

2

1 i m

8/10/2019 25-Shi

3/5

where, i = 2i 0 and dd,ii(k+ 1) is the ith diagonal element

of Dd(k+ 1).

The following Theorem 1 present the ARE algorithm.

Theorem 1(ARE algorithm). For the system given by (1)and (2), if K0(k+ 1) is given by

K0(k+ 1)= P0(k+ 1, k)CT(k+ 1)W20 (k+ 1) (12)

with

W0(k)= R1/2

d (k) (13)

Rd(k+ 1) = C(k+ 1)P0(k+ 1, k)CT(k+ 1)+

(k+ 1)Dd(k+ 1)DTd

(k+ 1)(k+ 1) (14)

(k+ 1)= diag {1(k+ 1), 2(k+ 1), ...,m(k+ 1)} , i 0(15)

i = 2i =

| vi(k+ 1) pii(k+ 1, k)11,mm |

d2d,ii

(k+ 1), 1 i m (16)

P0(k+ 1, k)= A(k+ 1, k)P0(k)AT(k+ 1, k) +Bd(k)B

Td(k) (17)

P0(k+1)= (I K0(k+1)C(k+1))P0(k+1, k) (P0(0)= I), (18)

the H norm of the transfer function from the noise inputs tothe estimation error outputs is limited within a certain range,and the estimation error can be minimized.

Proof. Let

dk =cole(0), {d(i)}ki=0

gd(k) =

ge(k, 0) gd(k, 0) gd(k, k 1) (k)Dd(k)

(19)

ge(k, 0) = C(k)A(k, k 1)(k 1, 0) (20)

gd(k, 0)= C(k)A(k, k 1)(k 1, i + 1)(I K(i + 1)C(i + 1))Bd(i)(21)

gd(k, i)= C(k)A(k, k 1)(k 1, i + 1)(I K(i + 1)C(i + 1))Bd(i) C(k)A(k, k 1)(k 1, i)K(i)(i)Dd(i)

(22)

gd(k, k 1) = C(k)Bd(k 1) C(k)A(k, k 1)K(k 1)(k 1)Dd(k 1)

(23)with

1 i < k 1.

From (19)-(24), we have

(k)= gd(k)d(k). (24)

Let rdk=col {rd(i)}ki=0, then rdk = Hrd(k)dk, where

Hrd(k) =

Dd(0) 0 0 0

ge(1, 0) gd(1, 0) Dd(1) 0...

.... . .

. . ....

ge(k, 0) gd(k, 0) gd(k, 1) Dd(k)

.

Based on the above analysis, one can have

gd,0(0)gTd,0(0)= (0)Dd(0)D

Td(0)(0) (25)

gd,0(k)gTd,0

(k)

=ge,0(k, 0)gTe,0

(k, 0) +k1i=0

gd,0(k, i)gTd,0

(k, i) +

(k)Dd(k)DTd

(k)(k)

= C(k)A(k, k 1)P0(k 1)AT(k, k 1)CT(k) +

C(k)Bd(k 1)BdT(k 1)CT(k) + (k)Dd(k)D

Td

(k)(k)

= C(k)P0(k, k 1)CT(k) + (k)Dd(k)D

Td

(k)(k)(k> 0)(26)

gd,0(i) 0

gTd,0

(j)

= ge,0(i, 0)gTe,0

(j, 0) +i1t=0

gd,0(i, t)gTd,0

(j, t) + (i)Dd(i)gTd,0

(j, i)

=[C(i)P0(i, i 1) (C(i)P0(i, i 1)CT(i) +

(i)Dd(i)DTd

(i)(i))K0T(i)] T(j 1, i)AT(j, j 1)CT(j)

=0(27)

with

P0(k+ 1, k) = A(k+ 1, k)P0(k)A(k+ 1, k) +Bd(k)BTd(k)

P0(k+ 1)= (I K0(k+ 1)C(k+ 1))P0(k+ 1, k)

and

K0(k+ 1)= P0(k+ 1, k)CT(k+ 1)

(C(k+ 1)P0(k+ 1, k)CT(k+ 1)+

(k+ 1)Dd(k+ 1)DTd

(k+ 1)(k+ 1))1(28)

P0(0)= I.

So

H,0(k)HT,0(k)= I. (29)

It is noted that

g = supdl2[0,k]

ki=0

(i)2

e(0)2 +k

i=0d(i)2

= Hrd(k)2 (30)

where

(k+ 1)= (C(k+ 1)A(k+ 1, k)e(k) +C(k+ 1)Bd(k)d(k) + (k+ 1)Dd(k+ 1)d(k+ 1)).

(31)

Since all the elements in (30) and (31) are time bounded,the H norm of the transfer function from the noise inputs to

the estimation error outputs is bounded, that is > 0, suchthat

ged = supdl2[0,k]

ki=0

e(i)2

e(0)2 +k

i=0

d(i)2< .

In addition, from the calculation process of adaptive weightmatrix, it can be deduced that the estimation error is mini-mized. Hence, the theorem is proved.

8/10/2019 25-Shi

4/5

IV. Application

The proposed method is applied in MEMS-INS/GPS inte-grated navigation system. The state-space model is

x(t) = A(t)x(t) + G(t)w(t) (32)

where the dynamic equation A(t) can be obtained accordingto [10] and [11], G(t) is the noise coefficient matrix, w(t) =[x, y, z, x, y, z] is the process noise caused by thegyros and accelerators stochastic drift, and the state vectorx(t) is defined as

x =VE, VN, VU, L,,h, E, N, U,

x, y, z, x, y, zT (33)

The symbols VE, VN, VU represent the velocity errors;L,,h represent the position errors; E, N, Udenote theattitude errors; x, y, z are gyroscope biases; x, y, z areaccelerometer biases.

In the loosely coupled scheme [12], the INS/GPS systemmeasurements is described as follows:

y(t)=

VEVNVU

Lh

=

VINS,E VGPS,EVINS,N VGPS,NVINS,U VGPS,U

LINS LGPSINS GPShINS hGPS

=C(t)x(t)

+v(t) (34)

where, VINS, and VGPS, are the output velocities of INSand GPS, respectively. LINS, INS, hINS and LGPS, GPS, hGPS arethe output positions of INS and GPS, respectively. C(t) =

I66 069

is the measurement matrix, and v(t) is the GPS

measurement noise.

The sampling time for INS is 5ms, the data of GPS isacquired at 0.1s, and the filtering cycle for the integratednavigation system is 0.1s.

The algorithm is verified using the experimental data fromthe MEMS-INS/GPS integrated navigation system. During theexperiment, the gyros and accelerators stochastic drifts werevarying in the ranges of 70/h 126/h and 7103g 7102g, respectively. The GPS recievers position error (95%)is 30m and its velocity error (95%) is 0.5m/s. In this case, wecompare two algorithms: one is the proposed ARE as shownin Fig.1, and the other is the RE algorithm in [] as shown inFig.2.

(a). The velocity estimation errors generated by ARE .

(b). The position estimation errors generated by ARE .Fig.1. The estimation errors generated by ARE .

(a). The velocity estimation errors generated by RE .

8/10/2019 25-Shi

5/5

(b). The position estimation errors generated by RE .

Fig.22. The estimation errors generated by RE .

It is seen from Fig.1 that the velocity and position estima-tion errors are within 0.2m/s and 5m, respectively in 1(a) and1(b). From Fig.2, one can see that the velocity and positionestimation errors are within 0.5m/s and 10m, respectively in

2(a) and 2(b). It is obvious that the estimation errors withARE are smaller than the ones with RF. It can be inferred thatthe ARE with the adaptive weight matrix can achieve higherestimation accuracy.

V. Conclusion

In this paper, the problem of ARE has been investigated forthe MEMS-INS/GPS integrated navigation system. The designobjective of minimizing the estimation error against distur-bances has been formulated as an ARE problem concernedwith finding the adaptive filter gain matrix. The proposedmethod has been evaluated using the off line experimentaldata of the MEMS-INS/GPS integrated navigation system. The

results show that the method can guarantee small estimationerrors in the presence of disturbances.

Acknowledgment

This work was co-supported by the Aeronau-tical Sci-ence Foundation of China (No.20130153002) and the basicresearch foundation of Northwest Polytechnical University(No.JC20120207).

References

[1] A. Noureldin, T. W. Karamat, M. D. Eberts and A. El-Shafie. Perfor-mance enhancement of MEMS-based INS/GPS integration for low-cost

navigation applications. IEEE Transactions on Vehicular Technology, 58:1077-1096, 2009

[2] Y. R. Geng and J. L. Wang.Adaptive estimation of multiple fading factorsin Kalman filter for navigation applications. GPS Slution, 12: 273-279,2008.

[3] W. D. Ding, J. L. Wang and C. Rizos. Improving adaptive Kalmanestimation in GPS/INS integration. The Journal of Navigation, 60: 517-529, 2007.

[4] Y. X. Yang and W. G. Gao. An optimal adaptive Kalman filter. Journalof Geodesy, 80: 177-183, 2006.

[5] B. Hassibi and A. H. Sayed. Linear estimation in Krein space - Part 2:Applications. IEEE Transactions on Automatic Control, 41: 34-48, 1996.

[6] B. Y. Zhang, W. X. Zheng and S. Y. Xu. On robust H filtering ofuncertain Markovian jump time-delay systems. International Journal ofAdaptive Control and Signal Processing, 26: 138-157, 2012.

[7] P. Li, J. Lam and Z. Shu.Hpositive filtering for positive linear discrete-time systems. IEEE Transactions on Automatic control, 55(10): 2337-2342, 2010.

[8] J. George.Robust Kalman-Bucy filter. IEEE Transactions on AutomaticControl, 58: 174-180, 2013.

[9] L. J. Miao and J. Shi.Model-based robust estimation and fault detectionfor MEMS-INS/GPS integrated navigation systems. Chinese Journal ofAeronautics, 27(4): 947-954, 2014.

[10] F. J. Aarrell and M. Barth. The global positioning system & inertial

navigation. New York: McGraw-Hill 1999.[11] D. H. Titterton and J. L.Weston.Strapdown inertial navigation technol-

ogy. United Kingdom: Peter Peregrinus Ltd 1997.

[12] X. X. Wang, Y. Liang, Q. Pan and Zhao CH.Gaussian filter for nonlin-ear systems with one-step randomly delayed measurements. Automatica2013; 49: 976-986.