# Study of RobustH∞ Filtering Application in Loosely Coupled INS/GPS System

Authors: Lin Zhao; Haiyang Qiu; Yanming Feng
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

Since a celebrate linear minimum mean square (MMS) Kalman filter in integration GPS/INS system cannot guarantee the robustness performance, aH∞ 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 H∞ filter is first reviewed; next we utilize linear matrix inequalities (LMI) approach to design the robust H∞ 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 H∞ filter and Kalman filter. A loosely coupled INS/GPS simulation system is given here to verify this application. Result shows that the robust H∞ filter has a better performance when system suffers uncertainty; also it is more robust compared to the conventional Kalman filter.


---

## Body

## 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 covarianceQ and measurement noise covariance R. 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]. H∞ 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 H∞ 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]. H∞ 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 H∞ filter is based on the “conservatism,” an important index in robust control. To the authors’ knowledge, there is no investigation of the robust H∞ 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 betweenH∞ filter and Kalman filter and then modify the LMI design approach, making it possible to apply the robust H∞ 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 H∞ filtering implementation in INS/GPS system.The rest of the paper is organized as follows. Derivation of recursiveH∞ filter formulation based on “game theory” [12] is introduced in Section 2; in Section 3 gives designing procedure for robust H∞ 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 H∞ filter and Kalman filter performance in different situation for loosely coupled INS/GPS model.The notation used here is fairly standard:∥·∥2 is a L2 norm; σ-(D) denotes the largest singular value of D; Tab denotes a transform operator from a to b; and Rn is nth dimensional Euclidian state space. Tr⁡(M) stands for the trace of matrix M, symbol * represents the symmetry term in a symmetric matrix, sup⁡ means the supremum, and AT represents the transposed matrix A.


## 2. Game Theory RecursiveH∞ Filter

Since many previous authors had utilizedH∞ filter in INS/GPS model, we discuss the game theory H∞ approach in this section as a fundamental illustration of the robust H∞ filter. In order to give a definition of H∞ norm. We begin with the L2 norm. For a continuous infinite systems during the period (0,T), L2 norm is defined as
(1)∥x(t)∥2=∫0Tx(t)xT(t)dt,
where to a discrete system, suppose the sampling number is N in the period T; then
(2)∥x(k)∥2=∑k=1Nx(k)xT(k).L
2 reflects the meaning of “energy” of the variable. Then the H∞ norm of a linear, time-invariant operator g=z(w) is defined as
(3)∥Tgw∥∞=sup⁡⁡∥z∥2∥w∥2=sup⁡⁡σ-(Tgw(jω)),
where σ-(·) denotes the max singular value of the square matrix T*TT and jω is the frequency parameter in the range of [-π,π]. In frequency domain, H∞ norm means a peak value of all the frequency ranges, while in time domain, it means the largest energy “transfer” form variable w to variable z; in other words, w has the largest influence on z. So what is the H∞ filter problem? Consider the following state-space system:
(4)x(k+1)=Ax(k)+Bw(k),y(k)=Cx(k)+v(k),z(k)=Lx(k),x(0)=x0,
where x∈Rn is the dynamic state, y∈Rv is the measurement, z∈Rr is the output to be estimated, Lx(t) is linear combination of x, and w∈Rq and v∈Rp are the process and measurement noise, respectively. (A,B) is controllable and (A,C) 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:
(5)J=∑k=1N∥z^(k)-z(k)∥2Sk2∥x^0-x0∥2Pk-12+∑k=1N(∥w∥2Qk-12+∥v∥2Rk-12),
where Pk, Qk, Rk, and Sk are symmetric, positive definite weighting matrices, and formulation ∥x∥2Pk2=xPkx′. 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 Q, R, P.TheH∞ filter problem is to find an estimation strategy z^(t) which minimizes J. 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 J, while his opponent, the nature tries to maximize J from the disturbances and initial state adversely. As a contrast to Kalman filter (KF), what KF pursuits is to minimize ∑k=1N∥z^(k)-z(k)∥2Sk2, but it cannot guarantee the J under some fixed extent, meaning that there may be a huge noise influence on the estimation result when achieves the minimum MSE.Substituting,∥v∥2Rk-12=∥y(k)-Cx(k)∥2Rk-12; this problem can turn to seeking a saddle point J(x0*,w*,y*,x^*), satisfying(6)J(x0,w,y,x^)<J(x0*,w*,y,x^)<J(x0*,w*,y*,x^*).
In fact, this saddle point, that is, the smallest J, 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 theoryH∞ filter model.A summary for the game theoryH∞ filter approach is: fix a prescribed parameter γ satisfying J=γ2, γ>0; try to achieve minimum ∑k=1N∥z^(k)-z(k)∥2Sk2 subject to (5). This derivation process can be implemented by using the Lagrange multiplier [13, 14]. The steady-state H∞ filter formulations are given by
(7)x^k+1=Akx^k+AkKk(yk-Ckxk),Kk=Pk(I-γ2Pk+CkTRk-1CkPk)-1CkTRk-1,Pk+1=AkPk(I-γ2Pk+CkTRk-1CkPk)-1AkT+Qk.
For an intuitive contrast with recursive Kalman filter, some transformations are carried out here. A typical Kalman gain Kk can be written as
(8)Kk=(I+Pk-CkTRk-1Ck)-1Pk-CkTRk-1.
Premultiplying outside by Pk- and postmultiplying each term inside by the inverse of Pk-, we obtain
(9)Kk=Pk-(Pk-+Pk-CkTRk-1CkPk-)-1Pk-CkTRk-1.
Postmultiplying outside the parentheses by the inverse of Pk- and premultiplying each term inside parentheses by the inverse of Pk-, we have
(10)Kk=Pk-(I+CkTRk-1CkPk-1)-1CkTRk-1.
So the reformed Kalman filter is given as
(11)x^k+1-=Akx^k-+AkKk(yk-Ckxk-),Kk=Pk-(I+CkTRk-1CkPk-)-1CkTRk-1,Pk+1-=AkPk-(I+CkTRk-1CkPk-)-1AkT+Qk.Note that there isPk- in (10) while Pk in (11). For a LTI system, it is easy to prove that the Pk- and Pk+ converse to the same stable constant when T→∞, proof procedure is omitted here for brief. Now suppose that γ2=0; the H∞ 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 H∞ 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 H∞ filter.


## 3. RobustH∞ Filter

Consider a system(12)x˙(t)=Ax(t)+Bw(t),y(t)=Cx(t)+Dw(t),z(t)=Lx(t),x(0)=x0.Note that the difference with system (1) lies in the description of noise. Setting B=[B0], D=[0D], and w=[wv]′ 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 matricesA, B, C, D 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
(13)S=[ABCD]∈{∑i=1Nα(i)[A(i)B(i)C(i)D(i)][A(α)B(α)C(α)D(α)]1111111111111=∑i=1Nα(i)[A(i)B(i)C(i)D(i)]}=Λ.The vector time-varying parameterα(t) belongs to the unit simplex as
(14)Ω={α(i)∈Rn,∑i=1Nα(i)=1;α(i)≥0}.Any uncertain matrices setS(α) belongs to a polytopic model with convex bound uncertain domain Λ; hence any S(α)∈Λ, S(α) can be written as a combination of vertices S(i) of the convex polytope. And the model is supposed to be entirely known if the vertices number N=1.Another important descriptor is the norm-bounded uncertainty. The modeS subject to norm-bounded parameter uncertainty is written in the form
(15)S=S0+HFE
for the uncertain matrix parameter F such that FF′=I, H, and E are known matrices.The purpose of the filter problem is to design an estimationz which is given by z=Ϝ·y, where Ϝ is an operator filter set with the form
(16)x^˙(t)=Afx^(t)+Bfy(t),z^(t)=Cfx^(t),x^(0)=0
for all the admissible uncertainty Λ such that {Af,Bf,Cf}∈Ϝ. The estimation error e can easily be yielded as e=z-z^. Construct state variable ξ=[xx^]T, the closed loop state-space form is
(17)ξ^(t)=Aaξ(t)+Baw(t);e(t)=Caxi(t),
where
(18)Aa=[A0KfCAf],Ba=[BKfD],Ca=[L-Cf].Defining the transfer function from the noise inputw to estimation error e is Twe(s); hence Twe(s)=Ca(sI-Aa)-1Ba.RobustH∞filtering problem: determining Af, Bf, Cf, for all the admissible uncertainty Λ, satisfy the following:(i)
model (17) is asymptotic stabilization;(ii)
transfer function infinity norm holds that∥Twe(s)∥∞≤γ.According to thebounded real lemma, the closed loop system (17) with the upper bounded γ is asymptotically stable if and only if there exists a matrix P=PT to such the algebraic Riccati inequality:
(19)AaTPa+PaAa+γ-2PaBaBaTPa+CaTCa<0.
Implement Schur complement lemma, this inequality is equivalent with the LMI:
(20)[AaP+PATPaCaTBaCaPa-γI0BaT0-γI]<0.


### 3.1. LMI Approach to RobustH∞ 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 Aa, Ba, Ca, so the relationship between inequality and variablesAf, Bf, Cf 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 P together with its inverse as
(21)P=[P11P12P12TP22],P-1=[P^11P^12P^12TP^22],
substituting the partitioned matrix P in (17) with P11, P22, P^11, P^22; the inequality changes into(22)[AP11+P11AT***BfCP11+AfP12T+P12TATBfCP12+AfP22+P12TCTBfT+P22AfT**LP11-CfP12TLP12-CfP22-γI*BTDTBfT0-γI],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(23)[AP11+P11AT***BfCP11+AfP12T+P12TATBfCP12+AfP22+P12TCTBfT+P22AfT**BTDTBfT-γI*LP11-CfP12TLP12-CfP220-γI].Define the nonsingular matrix(24)Ψ=[P^11-1000P^11P^120000I0000I].
Applying the congruence transformation, pre- and postmultiplying ΨT and Ψ, the inequality can be obtained as
(25)[AX+XAT***ATX+P11A+YC+KP^11A+ATP^11+YC+CTYT**BTXBTP^11+DTYT-γI*L-ZL0-γI]>0,
where X, Y, Z, K are matrix variables defined as
(26)X=P11-1,Y=P^12,Z=CfP12TX,K=P^12AfP12TX,[XXXP^11]>0.The objective is to determine the feasible solutions to the LMI; hence the filter matrices can be obtained:(27)Af=P^12-1K(P12TX)-1,Bf=P^12-1Y,Cf=Z(P12TX)-1
and matrices P^12, P12 can be set arbitrarily to be nonsingular as
(28)P12P^12T=I-P11P^11.
Here the filtering problem can be solved by a convex LMI optimisation approach. Give a summary to this filter problem.LMIH∞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 A, B, C, D by the vertex matrices Ai, Bi, Ci, Di of Λ, and guaranteeing that all the vertex LMIs hold, the robust H∞ problem can be solved.It should be highlighted that this formulation can reduced to standard Kalman filter when restricting some special case. Suppose thatBDT=0 and DDT=I; a standard Kalman state description (4) can recovered from (12) when replace this strict LMI (21) by a non strict one through choosing that X→0, K=-YC-P^11A, and L=Z; then the remaining variable should hold the LMI as
(29)[P^11A+ATP^11+YC+CTYT**BTP^11+DTYT-γI*L0-γI].
Define the new variable P=P^11-1 and Y=-CT. Applying the Schur complement to (29), we obtain
(30)AP+PAT-P(C′C-γ-2LTL)P+BBT<0.
If the parameter γ→∞ (30) is reduced to
(31)AP+PAT-PC′CP+BBT<0,
the optimal solution P 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 P(k), when k→∞, also called stable state of the Kalman filter for a LTI system. Defining P11=P12T and P^12-1=-P^11-1 and holding (28) we have
(32)Cf=Z(P12TX)-1=Z(P12TP11-1)-1=Z=L,Bf=P^12-1Y=-P^11-1Y=PC,Af=P^12-1K(P12TX)-1=P^12-1K=P^12-1(-YC-P^11A)=(-P^12-1P^11)A-(P^12-1Y)C=A-BfC.


## 3.1. LMI Approach to RobustH∞ 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 Aa, Ba, Ca, so the relationship between inequality and variablesAf, Bf, Cf 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 P together with its inverse as
(21)P=[P11P12P12TP22],P-1=[P^11P^12P^12TP^22],
substituting the partitioned matrix P in (17) with P11, P22, P^11, P^22; the inequality changes into(22)[AP11+P11AT***BfCP11+AfP12T+P12TATBfCP12+AfP22+P12TCTBfT+P22AfT**LP11-CfP12TLP12-CfP22-γI*BTDTBfT0-γI],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(23)[AP11+P11AT***BfCP11+AfP12T+P12TATBfCP12+AfP22+P12TCTBfT+P22AfT**BTDTBfT-γI*LP11-CfP12TLP12-CfP220-γI].Define the nonsingular matrix(24)Ψ=[P^11-1000P^11P^120000I0000I].
Applying the congruence transformation, pre- and postmultiplying ΨT and Ψ, the inequality can be obtained as
(25)[AX+XAT***ATX+P11A+YC+KP^11A+ATP^11+YC+CTYT**BTXBTP^11+DTYT-γI*L-ZL0-γI]>0,
where X, Y, Z, K are matrix variables defined as
(26)X=P11-1,Y=P^12,Z=CfP12TX,K=P^12AfP12TX,[XXXP^11]>0.The objective is to determine the feasible solutions to the LMI; hence the filter matrices can be obtained:(27)Af=P^12-1K(P12TX)-1,Bf=P^12-1Y,Cf=Z(P12TX)-1
and matrices P^12, P12 can be set arbitrarily to be nonsingular as
(28)P12P^12T=I-P11P^11.
Here the filtering problem can be solved by a convex LMI optimisation approach. Give a summary to this filter problem.LMIH∞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 A, B, C, D by the vertex matrices Ai, Bi, Ci, Di of Λ, and guaranteeing that all the vertex LMIs hold, the robust H∞ problem can be solved.It should be highlighted that this formulation can reduced to standard Kalman filter when restricting some special case. Suppose thatBDT=0 and DDT=I; a standard Kalman state description (4) can recovered from (12) when replace this strict LMI (21) by a non strict one through choosing that X→0, K=-YC-P^11A, and L=Z; then the remaining variable should hold the LMI as
(29)[P^11A+ATP^11+YC+CTYT**BTP^11+DTYT-γI*L0-γI].
Define the new variable P=P^11-1 and Y=-CT. Applying the Schur complement to (29), we obtain
(30)AP+PAT-P(C′C-γ-2LTL)P+BBT<0.
If the parameter γ→∞ (30) is reduced to
(31)AP+PAT-PC′CP+BBT<0,
the optimal solution P 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 P(k), when k→∞, also called stable state of the Kalman filter for a LTI system. Defining P11=P12T and P^12-1=-P^11-1 and holding (28) we have
(32)Cf=Z(P12TX)-1=Z(P12TP11-1)-1=Z=L,Bf=P^12-1Y=-P^11-1Y=PC,Af=P^12-1K(P12TX)-1=P^12-1K=P^12-1(-YC-P^11A)=(-P^12-1P^11)A-(P^12-1Y)C=A-BfC.


## 4. RobustH∞ 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(33)x=[δϕxδϕyδϕzδVxδVyδVzδLδλδhεxbεybεzb∇xb∇yb∇zb],where δϕx, δϕy, δϕz are the pitch, roll, and yaw errors, Vx, δVy, δVz are the east, north, and up velocity errors, δL, δλ, δh are the latitude, longitude, and height errors, εxb, εyb, εzb, and ∇xb, ∇yb, ∇zb 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
(34)F(t)=[FN(t)9×9FS(t)9×606×9I6×6].Fs(t) is the 9 fundamental parameter transfer matrix of which element detailed definition can be found in [16], I is identity matrix, and FS(t) is the transfer matrix between fundamental parameter and INS sensors bias, written as
(35)FS(t)=[03×3CbnCbn03×303×303×3].Cbn is the rotation matrix to transform the states from body frame to navigation frame. It is obviously that F(t) is a time-varying matrix determined by the real attitude ϕ, velocity V, and position P, although can be seen as a piecewise approximate constant matrix when implementing filtering for each epoch. H∞ estimation calls for no prestatistical character of noise, but accurate system matrices like A, B, C, D. If the INS output is corrupt, then FS cannot be guaranteed, neither is the estimation state accuracy X. And that is why a robust H∞ 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 theH∞ filter context, a precondition that transfer matrix A(FS) 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 A is unstable, P>0, ATP+PA<0 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
(36)x(t)=[xs(t)xu(t)]T
and the system matrices partitions are
(37)A(α)=[As(α)0Aus(α)Au],B=[BsBu],C(α)=[Cs(α)Cu],L=[Ls(α)Lu],
where the subscript “s” denotes the stable component and “u” denotes the unstable component.Assumption. Consider the following.(i)
The subsystemx˙s(t)=Asxs(t) is quadratically stable.(ii)
The matrixAu is unstable.(iii)
(Cs,As) is detectable and (A,B) is controllable.Next, partition the filter matrices, and we obtain
(38)x^(t)=[x^s(t)x^u(t)],Af=[AfsAf1Af2Afu],Bf=[BfsBfu],Cf=[CfsCfu].
Change the variable ξ with a augmented state, ξ will become
(39)ξ=[xsx^x(xu-x^u)].
Equation (17) is reformed as
(40)ξ˙(t)=Aa(α)ξ(t)+Aauxu(t)+Baw(t),e(t)=Ca(α)ξ(t)+Cauxu(t),
where
(41)Aa(α)=[As(α)00BfsCs(α)Afs-Af1Aus-BfuCs(α)-Af2Afu],Aau=[0Af1+BfsCuAu-BfuCu-Afu],Ba=[BsBfsDBu-BfuD],Ca(α)=[Ls-CfsCfu],Cau=[Lu-Cfu].
So the partial state xs is bounded while xu is unbounded. If ξ(t) and e(t) are bounded, there must be assured that Aau=0 and Cau=0, also meaning that
(42)Af1=-BfsCu,Afu=Au-BfuCu,Cfu=Lu.
Now, we have solved the unstable problems and the remaining processor can use a typical LMI approach to the robust H∞ 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
(43)x^(k+1)=(A-KC)x^(t)+Ky(t).
The estimation error dynamic function is
(44)x~(k+1)=(A-KC)x~(k+1)+w(t)+Kv(t).If(A-KC) is stable, then error dynamic function is stable, meaning KF is stable. Now, suppose that A is uncertain, and the real matrix is A′, and A is the nominal matrix in filtering. We obtain
(45)x~(k+1)=x(k+1)-x^(k+1)=A′x(k)-Ax^(k)+KCx^(k)-Ky(k)+w(k)=(A′-A)x(k)+A(x(k)-x^(k))+KC(x^(k)-x(k))-Kv(k)+w(k)=(A′-A)x(k)+(A-KC)x~(k)+w(k)-Kv(k).
The above formulation can be written as
(46)[x(k+1)x~(k+1)]=[A0A′-AA-KC][x(k)x~(k)]+[w(k)w(k)-Kv(k)].
If A′ and A are both unstable, when k→∞, x(t) can be arbitrarily large, so even A′-A is small; hence x~(k) can be arbitrarily large, and the filer is divergent.


## 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(33)x=[δϕxδϕyδϕzδVxδVyδVzδLδλδhεxbεybεzb∇xb∇yb∇zb],where δϕx, δϕy, δϕz are the pitch, roll, and yaw errors, Vx, δVy, δVz are the east, north, and up velocity errors, δL, δλ, δh are the latitude, longitude, and height errors, εxb, εyb, εzb, and ∇xb, ∇yb, ∇zb 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
(34)F(t)=[FN(t)9×9FS(t)9×606×9I6×6].Fs(t) is the 9 fundamental parameter transfer matrix of which element detailed definition can be found in [16], I is identity matrix, and FS(t) is the transfer matrix between fundamental parameter and INS sensors bias, written as
(35)FS(t)=[03×3CbnCbn03×303×303×3].Cbn is the rotation matrix to transform the states from body frame to navigation frame. It is obviously that F(t) is a time-varying matrix determined by the real attitude ϕ, velocity V, and position P, although can be seen as a piecewise approximate constant matrix when implementing filtering for each epoch. H∞ estimation calls for no prestatistical character of noise, but accurate system matrices like A, B, C, D. If the INS output is corrupt, then FS cannot be guaranteed, neither is the estimation state accuracy X. And that is why a robust H∞ 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 theH∞ filter context, a precondition that transfer matrix A(FS) 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 A is unstable, P>0, ATP+PA<0 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
(36)x(t)=[xs(t)xu(t)]T
and the system matrices partitions are
(37)A(α)=[As(α)0Aus(α)Au],B=[BsBu],C(α)=[Cs(α)Cu],L=[Ls(α)Lu],
where the subscript “s” denotes the stable component and “u” denotes the unstable component.Assumption. Consider the following.(i)
The subsystemx˙s(t)=Asxs(t) is quadratically stable.(ii)
The matrixAu is unstable.(iii)
(Cs,As) is detectable and (A,B) is controllable.Next, partition the filter matrices, and we obtain
(38)x^(t)=[x^s(t)x^u(t)],Af=[AfsAf1Af2Afu],Bf=[BfsBfu],Cf=[CfsCfu].
Change the variable ξ with a augmented state, ξ will become
(39)ξ=[xsx^x(xu-x^u)].
Equation (17) is reformed as
(40)ξ˙(t)=Aa(α)ξ(t)+Aauxu(t)+Baw(t),e(t)=Ca(α)ξ(t)+Cauxu(t),
where
(41)Aa(α)=[As(α)00BfsCs(α)Afs-Af1Aus-BfuCs(α)-Af2Afu],Aau=[0Af1+BfsCuAu-BfuCu-Afu],Ba=[BsBfsDBu-BfuD],Ca(α)=[Ls-CfsCfu],Cau=[Lu-Cfu].
So the partial state xs is bounded while xu is unbounded. If ξ(t) and e(t) are bounded, there must be assured that Aau=0 and Cau=0, also meaning that
(42)Af1=-BfsCu,Afu=Au-BfuCu,Cfu=Lu.
Now, we have solved the unstable problems and the remaining processor can use a typical LMI approach to the robust H∞ 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
(43)x^(k+1)=(A-KC)x^(t)+Ky(t).
The estimation error dynamic function is
(44)x~(k+1)=(A-KC)x~(k+1)+w(t)+Kv(t).If(A-KC) is stable, then error dynamic function is stable, meaning KF is stable. Now, suppose that A is uncertain, and the real matrix is A′, and A is the nominal matrix in filtering. We obtain
(45)x~(k+1)=x(k+1)-x^(k+1)=A′x(k)-Ax^(k)+KCx^(k)-Ky(k)+w(k)=(A′-A)x(k)+A(x(k)-x^(k))+KC(x^(k)-x(k))-Kv(k)+w(k)=(A′-A)x(k)+(A-KC)x~(k)+w(k)-Kv(k).
The above formulation can be written as
(46)[x(k+1)x~(k+1)]=[A0A′-AA-KC][x(k)x~(k)]+[w(k)w(k)-Kv(k)].
If A′ and A are both unstable, when k→∞, x(t) can be arbitrarily large, so even A′-A is small; hence x~(k) 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 0.02°/h and 0.01°/h; accelerator constant drift and random white noise 1 sigma are 100 μg and 10 μg. GPS position measurement white noise 1 sigma is 1 m/s. Corresponding correlation noise matrices are set as
(47)Q=diag⁡(06×1;[0.01m,0.01m,0.01m];[0.01°/h,0.01°/h,0.01°/h];[10μg,10μg,10μg])2,R=diag⁡([1m,1m,1m])2.
Initial estimation error correlation matrix can be given as
(48)P=diag⁡([1°,1°,1°],[1m/s,1m/s,1m/s],[10m,10m,10m],[0.1°/h,0.1°/h,0.1°/h],[100μg,100μg,100μg]).
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 robustH∞ filter results.Figure 3
Interfered situation Kalman filter and robustH∞ filter results.Triple INS compensate parametersδϕ, δV, δP statistical index STD is shown in Tables 1 and 2 for these two situations.Table 1
Normal situation STD index.
STD
Kalman
RobustH∞
Pitch
Roll
Yaw
Pitch
Roll
Yaw
ϕ
o
0.1130
0.1386
2.4796
0.0129
0.0149
0.035
North
East
Up
North
East
Up
δ
V m/s
0.0296
0.0266
Nan
0.0366
0.0519
Nan
δ
P m
0.2806
0.3487
Nan
0.6351
0.8284
NanTable 2
Interfered situation STD index.
STD
Kalman
RobustH∞
Pitch
Roll
Yaw
Pitch
Roll
Yaw
ϕ
o
0.1430
0.1786
2.9196
0.0159
0.0199
0.055
North
East
Up
North
East
Up
δ
V m/s
0.0473
0.0565
Nan
0.0372
0.0568
Nan
δ
P m
0.4206
0.5787
Nan
0.8321
0.9674
NanIn normal situation, KF shows a more “stable” and “accurate” performance thanH∞ filter; the H∞ filter exhibits a jitter character in the tracking process of real value, whereas in interfered situation, H∞ filter is more robust, takes account of the north δV, H∞ filter STD varies from 0.0366 to 0.0372 while KF from 0.0296 to 0.0473. Note that H∞ filter contributes a better performance to δϕ in both situations because H∞ filter has a “weak” effect on estimation error matrix P which means that the initial value P 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, H∞ is stable during this period.Figure 4
Normal situation Kalman filter and robustH∞ filter results for 1500 s.


## 6. Conclusion

In this paper, we design a robustH∞ filter for a loosely coupled INS/GPS model. Firstly, a game theory gives a H∞ approach in a Kalman form; it can be easily transformed by adjusting the noise attenuation parameter γ. Next, a robust H∞ filter based LMI is discussed; the unstable case is considered. We also illustrate that the robust H∞ 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.



---

*Source: 904062-2014-06-04.xml*