- Email: [email protected]
Design and implementation of SMO for a nonlinear MIMO AHRS
Mechanical Systems and Signal Processing 32 (2012) 94–115
Contents lists available at SciVerse ScienceDirect
Mechanical Systems and Signal Processing journal homepage: www.elsevier.com/locate/ymssp
Design and implementation of SMO for a nonlinear MIMO AHRS Parisa Doostdar, Jafar Keighobadi n University of Tabriz, Faculty of Mechanical Engineering, 29 Bahman, Tabriz, P.C. 5166614766, Iran
a r t i c l e in f o
abstract
Article history: Received 13 April 2011 Received in revised form 3 February 2012 Accepted 18 February 2012 Available online 16 March 2012
In a low-cost attitude heading reference system (AHRS), the measurements made by MEMS inertial and magnetic sensors are affected by large parameter uncertainties, stochastic noises and unknown disturbances. In this paper, considering the robustness of the sliding mode observers (SMO) against both structured and unstructured uncertainties as well as exogenous inputs, the process of design and implementation of a nonlinear SMO is proposed for a low-cost AHRS. For simultaneous estimation of orientation variables and calibration biases of gyroscopes, a nonlinear and non-affine model of the AHRS is considered. Therefore, based on the Lie-algebraic method, the estimation algorithm is designed for a general class of non-affine nonlinear MIMO systems. In the proposed observer, owing to decreasing the required assumptions for coordinate transformation in recent literatures, the design process of the SMO is simplified. The gain matrices of the proposed SMO are obtained through ensuring the stability and the convergence of estimation errors based on Lyapunov’s direct method. The expected tracking performance of the robust state and parameter estimation algorithm compared to that of the extended Kalman filter (EKF) is evaluated through simulations and real experiments of a strapped AHRS on a ground vehicle. & 2012 Elsevier Ltd. All rights reserved.
Keywords: MIMO nonlinear system Sliding mode observer Attitude and heading Estimation
1. Introduction To measure 3-axis orientations using MEMS sensors of an AHRS, a nonlinear robust observer is proposed to handle both the modeling uncertainties and the exogenous unknown inputs. The AHRS uses 3-axis inclinometers (based on accelerometers), magnetometers and particularly gyroscopes, in which the mean value of the noises increases due to the time-integration of the measured angular rates. Therefore, considering the 2-norm bounded exogenous inputs and the uncertainties, which are affecting the calibration parameters of the MEMS gyroscopes, the nonlinear SMO is preferred to the extended Kalman filter. To design the nonlinear observer for a single-output nonlinear system, the system is transformed into a canonical form [1]. Krener et al. have presented a generalized version of this method for multi-output systems [2], which is also called the exact error linearization method, see also [3,4]. In this approach, by injection of the outputs and their Lie-derivatives, the nonlinear system is transformed into a linear equivalent system. Isidori has proposed a nonlinear coordinate transformation method which is frequently used in the observer design problems [5]. Using the coordinate transformation, the nonlinear MIMO system is transformed into the output injection model, and therefore a state estimation algorithm which is insensitive to the unknown inputs is developed. Due to the structural property of the considered nonlinear system, the system is decomposed into two interconnected subsystems in
n
Corresponding author. Tel./fax: þ 98411 3354153. E-mail address: [email protected] (J. Keighobadi).
0888-3270/$ - see front matter & 2012 Elsevier Ltd. All rights reserved. doi:10.1016/j.ymssp.2012.02.007
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
95
which the unknown inputs appear only in the last dynamical equation of each subsystem. Isidori’s observer design method requires that the distribution matrix of the unknown inputs satisfies the involutive property. Furthermore, the outputs should have a sufficient vector relative degree corresponding to the distribution matrix [5]. Therefore, when the sum of the relative degrees of all subsystems is smaller than the number of states, defining a full-order transformation matrix is a difficult problem. To solve the above-mentioned problems for decoupling the unknown inputs, a new transformation technique has been proposed which deals directly with the estimation of unknown inputs [6]. Therefore, under the structural assumptions on the distribution matrix of unknown inputs and using the outputs of relative degree ‘one’ with respect to the unknown inputs, an observer has been considered to estimate both the states and the unknown inputs [6]. The above-mentioned transformation methods require that the nonlinear system should be decomposed into two subsystems and the outputs should have a well-defined vector relative degree. Therefore, a suitable transformation may not be possible for a general class of nonlinear systems. In this paper, the nonlinear MIMO system is affected by large uncertainties due to unknown bounded disturbances, modeling uncertainties and measurement noises. Therefore, the SMO as a robust estimator of variable structure systems is used to keep the trajectory of estimated states remaining on the surface of true states in a finite time. Due to using a nonlinear discontinuous term in the SMO, the estimation error becomes completely insensitive to the uncertainties [7]. Using Lyapunov’s direct method in design of SMOs for nonlinear systems subjected to bounded nonlinearities and uncertainties was suggested by Walcott et al. [8]. Misawa and Hedrick [9], and Edwards and Spurgeon [10] further have developed a numerical tractable algorithm for Walcott–Zak’s SMO. Utkin has proposed the other category of SMOs using the equivalent control theory in which the output estimation error and its higher time-derivatives are considered as the sliding manifolds [11]. The idea of Utkin has been extended to a general class of nonlinear systems by Drakunov [12]. In most of the recent works in the field, the SMOs have been proposed for single output systems and there are fewer research works on the MIMO systems. Floquet and Barbot have considered a possible design method of SMOs for MIMO systems satisfying the structural and the matching conditions [13]. The main contribution of this paper is to design a SMO for a general class of nonlinear MIMO systems like as an AHRS, which is not necessarily control affine and is affected by the uncertainty of parameters and the unknown exogenous inputs. In the proposed method, through the transformation of the nonlinear MIMO system into a new state space, the measurement equations are perfectly linearized; and the system’s dynamics is decoupled into linearized subsystems. Therefore, the unstructured uncertainties as well as the state variables of the system could be estimated by the proposed SMO in the new state space. It should be noted that through inverting the transformation, the designed observer is formulated for the original system. Due to decreasing the required assumptions in design of the proposed SMO, the process of design, stability analysis and observability conditions are simplified considerably. The gain matrices of the proposed SMO are designed through ensuring the stability and the convergence of estimation error based on Lyapunov’s direct method. SMOs suffer from the chattering phenomenon, which may be attenuated by different modifying techniques imposed on the original observer. For example, in the boundary layer method, the discontinuous sign term of the observer is replaced by a saturation function along a thin layer neighboring the origin [14]. The proposed nonlinear SMO is implemented for the AHRS to estimate the orientations of a ground vehicle in different road conditions. The estimated attitude and heading angles by the SMO are compared with the reference orientations given by the integrated INS/GPS system. Furthermore, through simulations, the convergence rate and the tracking accuracy of the estimations obtained by the SMO are compared with those of the EKF. The rest of the paper is organized as follows. The paper first presents modeling of AHRS in Section 2. In Section 3, SMO is designed. Section 4 presents implementation of SMO and EKF to AHRS. Simulations and real test results are provided in Section 5. Concluding remarks are made in Section 6.
2. Attitude heading reference system In the AHRS, the 3-axis gyroscopes, accelerometers and magnetometers measure the rotation rates of the system body and the vectors of the Earth’s gravity and magnetic fields all along the body axes of the system, respectively. In Fig. 1, the sequence of the Euler angles, j, y and c represents the rotation of the vehicle’s body axes, x–y–z with respect to the local North–East–Down (NED) axes. Therefore, the orientation of the vehicle’s body could be updated through the numerical integration of Euler’s angles dynamics including the angular rates of the body. However, the considerable uncertainties in the measured angular rates using low cost MEMS gyroscopes causes the rotation angles to drift with respect to time. Therefore, the measurements made by the 3-axis accelerometers and magnetometers are used to update the rotation angles obtained from the Euler angles dynamics [15].
2.1. Sensor error model The bias of a rate gyroscope is considered its average output with respect to a zero input rotation rate over a specified period of time. The measurements made by the MEMS gyroscopes are not accurate due to the stability (change) of the calibration biases. In this paper, the bias term of the MEMS gyroscopes is modeled as the following first order
96
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 1. Representation of Euler angles.
Gauss–Markov process with a relatively large correlation time [16] qffiffiffiffiffiffiffiffiffiffiffiffi 1 _ dwðtÞ ¼ dwðtÞ þ 2bs2 wðtÞ
b
ð1Þ
where, b and s denote the correlation time and the standard deviation of the zero-mean noise process, w(t), respectively. 2.2. Attitude In this paper, the following attitude dynamics is used for the evolution of the roll and pitch angles [17] 0 1 0 1 sin y j_ cos j cos sin y y 1 sin j B C B C cos y @ A¼@ A ðo þ wþ dwÞ _y 0 cos j sin j
ð2Þ
o ¼ ½ ox oy oz T w ¼ ½wx
wy
wz T
where, the superscript, T stands for the transpose of the vectors. The dynamics of the roll and pitch angles, j and y (2) are expressed in terms of the body referenced body to NED frame rotation rate vector, o and the gyroscopes’ noise and bias vectors, w and dw, respectively. In high-resolution gyroscopes for example the ring-laser type, the angular rate of the body, o is composed with the rotation rate of the Earth and the transportation rate of the vehicle. However, the last two rates are considerably small compared with the noise floor of the MEMS gyroscopes. Corresponding to the three gyroscopes along the body x–y–z axes, the three components of the bias vector, dw as, dwx, dwy and dwz are augmented to the roll and pitch angles in the state vector. Therefore, the nonlinear dynamics of the attitude determination subsystem is completed as 0 1 0 _ 1 ðdwx þ ox Þ þ ðdwy þ oy Þsin j tan y þ ðdwz þ oz Þcos j tan y j B C ðdwy þ oy Þcos jðdwz þ oz Þsin j B y_ C B C B C B C 1 B C B C d w x C¼B C b _ d w x_ ¼ B x C B B C 1 B _ C B C b dwy @ dw y A B C @ A 1 _z dw b dwz 0 1 1 sin j tan y cos j tan y B 0 1 0 cos j sin j C B C wx B pffiffiffiffiffiffiffiffiffiffiffiffi C B 2bs2 C B C 0 0 ð3Þ þB C@ wy A pffiffiffiffiffiffiffiffiffiffiffiffi B C 2 B C wz b s 0 2 0 @ pffiffiffiffiffiffiffiffiffiffiffiffi A 0 0 2bs2
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
97
Fig. 2. Resolution of gravity vector along body axes.
In a strapdown AHRS, as Fig. 2 shows, the 3-axis accelerometers are assigned to measure the Earth’s gravity field vector of magnitude, g resolved along the vehicle’s body axes as [18] yx ¼ g sin y yy ¼ g cos y sin j yz ¼ g cosy cos j
ð4Þ
where, yx, yy and yz denote the measurements made by the accelerometers fixed along the body axes x, y and z, respectively. Therefore, the attitude angles could be computed through the matching of the Earth’s reference gravity vector in NED frame with the measurements of accelerometers along the body axes. In the matching equations (4), all the nongravitational accelerations are considered zero values. However, unlike the coriolis and centrifugal accelerations which could not be measured by the MEMS accelerometers, the linear accelerations owing to the possible accelerated movements of the AHRS will affect the accuracy of the obtained roll and pitch angles. Hence, during initial alignment process, the AHRS should not be moved to obtain the initial states of (3) as y ð5Þ y0 ¼ sin1 x g
j0 ¼ sin1
yy g cosðy0 Þ
ð6Þ
dwx0 ¼ ox
ð7Þ
dwy0 ¼ oy
ð8Þ
dwz0 ¼ oz
ð9Þ
where, the symbol, - denotes the average output of the inertial sensors over the alignment time. It is worth noting that, the mathematical singularity of (6) at y0 ¼901 does not occur in vehicular applications of the AHRS. 2.3. Heading The Earth’s magnetic field vector is generally used as the north direction reference of a MEMS AHRS to the alignment and the monitoring of heading angle. In the AHRS, once the roll and pitch angles are estimated, the measurements made by the 3-axis magnetometers are used to make a vector matching with respect to the resolved components of the Earth’s magnetic field vector in the local level plane [18]. Therefore, to update the heading angle, c the following dynamics and the computed magnetic heading will be integrated through the state estimation algorithms, in this paper 1 1 ðoy þw þ dwy Þ þ cos j ðoz þ wþ dwz Þ cos y cos y H1 ym ¼ cm ¼ cos1 HH
c_ ¼ sin j
H1 ¼ mx cos y þ my sin j sin y þ mz cos j sin y
ð10Þ
ð11Þ ð12Þ
98
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
H2 ¼ my cos jmz sin j
ð13Þ
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi HH ¼ H1 2 þ H2 2
ð14Þ
where, cm is the magnetic heading angle; as shown in Fig. 1, the horizontal component of the Earth’s magnetic field vector, HH is resolved to H1 and H2 along the local level axes, XH and YH, respectively. The components of the Earth’s magnetic field vector along the body x–y–z axes are denoted by mx, my and mz, respectively. 3. Uncertain nonlinear system In this paper, the SMO is proposed for a general class of MIMO nonlinear uncertain systems which are non-affine with respect to inputs u and o as _ ¼ f ðxðtÞ,uðtÞ, oðtÞÞ þ gðxðtÞÞdðtÞ xðtÞ yðtÞ ¼ hðxðtÞÞ
ð15Þ n
m
q
where, f(x,u,o), h(x) and g(x) are sufficiently smooth functions on the vector space, M; x 2 R , u 2 R , o 2 R and d 2 Rr are the state, known input, measurable input and unknown input vectors, respectively; g(x) is the distribution matrix of d. The measurement vector, y 2 Rp is assumed to be completely known and measurable at each sample time, t. 3.1. State transformation As the first step in design process of the nonlinear SMO, the nonlinear system (15) is transformed into an equivalent linear system based on the following state vector: z ¼ ½ðz1 ÞT i
z ¼
½zi1 ,
ðz2 ÞT
zi2 ,
. . .,
...
zikj T
ðzp ÞT T ¼ fðx,u, oÞ Lf hi
¼ ½hi
...
Lf ki 1 hi T ,
zi 2 ki
for i ¼ 1 : p
ð16Þ
where, the new state variables of the transformed system, ziki are the Lie derivatives of components of, h(x) with respect to vector f(x,u,o) in (15); for example as, Lfhi(x)¼[@hi(x)/@x]f. Regarding the fact that in nearly all real systems, the time derivatives of inputs u and o are not available; therefore, the transformed state vector should be independent of both u_ _ . To obtain a complete n-dimensional transformation matrix, @f/@x the fixed observability indices, k1 ,k2 ,. . .,kp should and o satisfy the following condition: p X
ki ¼ n
ð17Þ
i¼1
Assumption 1. The mapping, f(x, u, o) (16), which is used to transform the MIMO uncertain nonlinear system (15) into the new space, is a diffeomorphism, i.e. both the Jacobian matrix, @f/@x and its inverse, [@f/@x] 1 exist. Under Assumption 1, taking the time derivative of the transformed state vector (16) results in the following nonlinear system: h iT z_ ¼ ðz_ 1 ÞT ðz_ 2 ÞT . . . ðz_ p ÞT h iT y ¼ y1 T y2 T . . . yp T ð18Þ where, the p-tuple linearized subsystems are produced as _, z_ i ¼ Ai zi þF i ðzi ,u, oÞ þ Gi ðzi Þd þB1 i ðzi ,u, oÞu_ þ B2 i ðzi ,u, oÞo
for i ¼ 1 : p
yi ¼ C i zi 0
ð19Þ 0
1
0
1
0
...
0
B0 B Ai ¼ B @^
0
1
...
^
^
&
0C C C 2 RK i K i , 1A
0
0
0
...
0
B1 i ðzi ,u, oÞ ¼ 01m B2 i ðzi ,u, oÞ ¼ 01q
@ ðL h Þ @u f i
...
@ ðL h Þ @o f i
...
B B B F i ðz ,u, oÞ ¼ B B B @ i
0 0 ^ 0 Lf ki hi
1 C C C C 2 RK i 1 , C C A
@ K i 1 T Lf hi @u @ @ K i 1 T LfK i 2 hi Lf hi @o @o
0
1 Lg hi B L Lh C B g f i C B C C i ^ Gi ðz Þ ¼ B B C 2 RK i r B L Ln2 h C B g f iC @ A Lg Ln1 hi f
@ K i 2 L hi @u f
ð20Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
99
C i ¼ ½1 0 . . . 0 2 R1K i where, the superscript, T stands for the transposed matrix/vector. The decoupled subsystems (19) of the transformed MIMO nonlinear system (15) are grouped together to obtain the equivalent linear system as _ z_ ¼ Az þFðz,u, oÞ þ GðzÞd þB1 ðz,u, oÞu_ þ B2 ðz,u, oÞo
ð21Þ
A ¼ diag½A1 ,A2 ,. . .,Ap 2 Rnn , B1 ¼ ½B1 1 B1 2 . . .B1 p T 2 Rnm , B2 ¼ ½B2 1 B2 2 . . .B2 p T 2 Rnq and C ¼ diag½C 1 ,C 2 ,. . .,C p 2 Rpn
3.2. Observability In general, the observability of a non-linear system is affected by its inputs, unless the system is uniformly observable. Definition 1. The nonlinear system (15) is uniformly observable if Assumption 1 holds, i.e. there exist a p-tuple set {k1,k2,y,kp} such that the mapping (16) is invertible with respect to x and therefore, the observability rank condition is satisfied as [19] rank
@f ¼n @x
ð22Þ
3.3. Observer design The SMO is designed to estimate the state vector of the transformed system (21) as _ þLðyC z^ Þ þ K signðyC z^ Þ z_^ ¼ Az^ þ Fðz^ ,u, oÞ þ B1 ðz^ ,u, oÞu_ þ B2 ðz^ ,u, oÞo
ð23Þ
where, z^ denotes the estimate of z; L and K are the estimation gain matrices; and sign(.) represents the common sign function. For direct estimation of the original state vector, x under Assumption 1 and using the inverse transformation matrix, [@f/@x] 1 the proposed observer (23) is transformed into the original state space as 1 1 @f @f Lðyhðx^ ÞÞ þ K signðyhðx^ ÞÞ ð24Þ x_^ ¼ f ðx^ ,u, oÞ þ @x @x
3.4. Observer stability Using (21) and (23), the observer error dynamic is obtained as e_ ¼ z_ z_^ ¼ AeLCe þðFðz,u, oÞFðz^ ,u, oÞÞ þ ðB1 ðz,u, oÞB1 ðz^ ,u, oÞÞ u_ _ þðGdK signðyhðx^ ÞÞ þ ðB2 ðz,u, oÞB2 ðz^ ,u, oÞÞo ^ þðB1 B^ 1 Þu_ þ ðB2 B^ 2 Þo _ þ GdK signðyhðx^ ÞÞ ¼ A0 e þðFFÞ
ð25Þ
To prove the stability of the SMO, the following assumptions are considered. Assumption 2. The pair (A, C) is assumed to be detectable, therefore, there exist an observer gain, L such that A0 ¼A LC is a strictly Hurwitz matrix. _ :ro _ r u_ where the _ and :u: Assumption 3. The rates of the bounded input signals are also bounded values as, :o symbols :U: and – stand for the 2-norm of a vector/matrix and the upper bound value, respectively. Assumption 4. The unknown disturbance vector, d is bounded, i.e. there exists a positive real value, d that, :d: r d. Assumption 5. The 2-norm of the distribution matrix, G is bounded to a known upper bound, G with respect to its argument, z.
100
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Assumption 6. The nonlinear functions F(z,u,o), B1(z,u,o) and B2(z,u,o) satisfy the Lipschitz condition with respect to, z as :Fðz1 ,u, oÞFðz2 ,u, oÞ: rLF :z1 z2 :
ð26Þ
:B1 ðz1 ,u, oÞB1 ðz2 ,u, oÞ: rLB1 :z1 z2 :
ð27Þ
:B2 ðz1 ,u, oÞB2 ðz2 ,u, oÞ: rLB2 :z1 z2 :
ð28Þ
where, LF ,LB1 and LB2 are the Lipschitz constants. Under Assumptions 1 through 6, the stability of the SMO (23) can be investigated using the following Lyapunov function: V ¼ eT Pe e ¼ zz^
ð29Þ T
where, the positive definite matrix, P¼P is the unique solution of the well-known Lyapunov equation as PA0 þ A0 T P ¼ Q
ð30Þ
Differentiating the Lyapunov equation (29) with respect to time yields as ^ þ 2eT PðB1 B^ 1 Þu_ V_ ¼ eT Pe_ þ e_ T Pe ¼ eT ðPA0 þ A0 T PÞe þ 2eT PðFFÞ _ þ2ePðGdK signðyhðx^ ÞÞ þ 2eT PðB2 B^ 2 Þo
ð31Þ
Replacing (30) in (31) leads to ^ þ 2eT PðB1 B^ 1 Þu_ þ2eT PðB2 B^ 2 Þo _ þ 2eP Gd2ePK signðyhðx^ ÞÞ V_ ¼ eT Q eþ 2eT PðFFÞ
ð32Þ
Now, considering Assumptions 2 through 6, the following inequality is obtained: 2 2 2 2 _ lmax ðPÞ V_ r:e: lmin ðQ Þ þ2:e: LF lmax ðPÞ þ 2:e: LB1 u_ lmax ðPÞ þ 2:e: LB2 o
þ 2:e:Gdlmax ðPÞ þ 2:e:K lmax ðPÞ
ð33Þ
where, K denotes the upper bound of the gain matrix, K. Rearranging (33) results in 2 _ Þ þ 2:e:lmax ðPÞðGd þKÞ ¼ a:e:2 þb:e: V_ r :e: ½lmin ðQ Þlmax ðPÞð2LF þ 2LB1 u_ þ 2LB2 o
ð34Þ
_ Þ and b ¼ 2lmax ðPÞðGd þ KÞ. A negative definite, V_ requires that, a:e: Zb and where, a ¼ lmin ðQ Þlmax ðPÞð2LF þ2LB1 u_ þ 2LB2 o a 40 Therefore, a bounded error dynamics is ensured considering as
lmin ðQ Þ lmax ðPÞ
ð35Þ
a:e:2lmax ðPÞGd 2lmax ðPÞ
ð36Þ
_ o a 4 0 ) LF þ LB1 u_ þ LB2 o and a:e:Z b ) K r
Using the following well known inequality [20]: 2
2
2
a:e: þ 2b:e: þc r ða1Þ:e: þ b þ c,
8 a,b,c,e 2 R
ð37Þ
The inequality (34) is changed as 2
b 2 V_ r ða1Þ:e: þ 4
ð38Þ
The following inequality is valid for the positive definite matrix, P [14]: 2
2
lmin ðPÞ:e: reT Pe r lmax ðPÞ:e:
ð39Þ
Now, using the second inequality of (39), (38) yields V_ r ða1Þ
V
lmax ðPÞ
2
þ
b 4
ð40Þ
and therefore
# " 2 1a b 1a t þ t VðtÞ r Vð0Þ exp 1exp lmax ðPÞ 4ða1Þ lmax ðPÞ
Similarly, from the first inequality of (39) sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi V :eðtÞ:r lmin ðPÞ
ð41Þ
ð42Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Replacing (41) in (42) yields vffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi " #ffi u 2 2 u b 1a b t t þ exp :eðtÞ: r lmin ðPÞ Vð0Þ 2 4ða1Þ 4ða1Þlmin ðPÞ Therefore, the ultimate stability of the designed SMO is guaranteed as sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2 b lim :eðtÞ: r t-1 4ða1Þlmin ðPÞ
101
ð43Þ
ð44Þ
Consequently, the upper bound (36) is completely determined for the gain matrix, K . The gain matrices, L and K are properly designed to obtain robust estimations against measurement noises and modeling uncertainties. First, the feedback gain, L is designed to satisfy Lyapunov’s equation (30) and; therefore, the matrix A0 ¼A LC is a negative-definite matrix. Second, the sliding gain, K is designed to satisfy the stability conditions (34) and (43). Furthermore, regarding (44), the estimation error of the SMO, e is a small and bounded value. 3.5. Boundary layer observer By increase of the sliding gain, owing to the large oscillations and the short settling time, the discontinuous term, K signðyy^ Þ removes the effects of the uncertainties on the estimated states and also guarantees the stability of the error dynamics (25). However, the undesirable chattering phenomenon occurs in the estimations of the SMO. The chattering is decreased through ensuring the convergence of the estimations to the arbitrary small neighboring of the true states, and therefore, the boundary layer technique is considered in the SMO [21]. For example, the following simple version of the boundary layer method results in a continuous and smooth switching observer: 1 1 @f @f x_^ ¼ f ðx^ ,u, oÞ þ Lðyhðx^ ÞÞ þ K satðyhðx^ ÞÞ ð45Þ @x @x ( satðyhðx^ ÞÞ ¼
signðyhðx^ ÞÞ
if:yhðx^ Þ:4 l
yhðx^ Þ
if:yhðx^ Þ:r l
l
ð46Þ
where, l denotes the width of the boundary layer. The chattering phenomenon can be effectively decreased for large values of l; however, the tracking accuracy will be decreased and the convergence condition (44) may be deteriorated. Therefore, through adjusting, l a tradeoff among the tracking accuracy and the chattering suppression should be considered. 4. Implementation of SMO for AHRS The measurement noise filtering capability of the SMO is similar to that of the Kalman filter. However, the SMO has a high robustness in the presence of uncertainties in the measurement noises. In the nonlinear MIMO AHRS, due to the possibility of using different numbers/arrangements of the outputs in the mapping vector of coordinate transformation (16), different transformation matrices may be obtained for coordinate transformation. Therefore, to design a successful SMO for the nonlinear AHRS, a transformation vector which satisfies the observability condition (22) should be constructed. The AHRS is a measurement system which is affected by the measured input vector, o and therefore, without loss of generality, the SMO is continued without considering the known input vector, u. The elements of nonlinear matrices, f(x,o), g(x) and h(x) are smooth functions of the states, x¼[j y c d wx dwy dwz]T and Earth’s gravity term, g. 4.1. Attitude estimation In the implementation of SMO (24) for the AHRS, using (3) and (4) the attitude determination subsystem is considered as 0 1 j_ 0 1 wx B _ C B y C C C ¼ f a ðxa , oÞ þ ga ðxa ÞB x_ a ¼ B ð47Þ @ wy A B dw C @ _xA wz _y dw
ya ðxa Þ ¼ ha ðxa Þ ¼
g cos y sin j g cos y cos j
! ð48Þ
102
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
where, the nonlinear matrices, fa(xa,o), ga(xa) and ha(xa)are given as follows: 1 0 1 0 ðdwx þ ox Þ þ ðdwy þ oy Þsin j tan y þ ðdwz þ oz Þcos j tan y f a1 B C Bf C B ðdwy þ oy Þcos jðdwz þ oz Þsin j C B a2 C B C B C 1 f a ðxa , oÞ ¼ B ¼B C d w C x C b @ f a3 A B @ A 1 f a4 b dwy 0
1
sin j tan y
B B
0
cos j
0
0 pffiffiffiffiffiffiffiffiffiffiffiffi 2bs2
pffiffiffiffiffiffiffiffiffiffiffiffi ga ðxa Þ ¼ B B 2bs2 @
cos j tan y
ð49Þ
1
sin j C C C C 0 A 0
ð50Þ
Fig. 3. SMO block diagram.
Table 1 Specifications of inertial and magnetic sensors. Sensor type
Model
FSa range
Noise density
Bias stability
Initial bias error 7 1s
Nonlinearity (%FS)
Rate gyro Accelerometer (single-axis) Accelerometer (dual-axis) Magnetometer
ADXRS150 ADXL202 ADXL210E HMC1022
7 150 1/s 7 20 m/s2 7 100 m/s2 7 200 mTb
0.05 1/s/OHz 0.01 m/s2 0.01 m/s2 10 5 mT/OHz
0.01 1/s 0.05 m/s2 0.05 m/s2 0.10 m/s2
73 1/s 70.02g 70.02g 2 10 5 mT
0.1 0.2 0.2 0.1
a b
Full-scale. Micro-Tesla.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Table 2 A priori information and gain matrices for SMO and EKF. SMO
EKF
b ¼ 0:10
P
s ¼ 0:01
a
1
0
0
1
P
a¼1
0
1
10 0 B C La ¼ @ 10 1 A 1 1 0 1 5 1 B C K a ¼ @ 10 1 A 1 1 10 Lh ¼ 0:001
Kh ¼
¼ 105
0:01 0:001
5 h ¼ 10
0 1 1 0 0 B C qda ¼ ð7:615 10-5 Þ@ 0 1 0 A 0 0 1 1 0 qdh ¼ ð3:808 10-5 Þ 0 1 0
qva
1 B ¼ 0:015@ 0 0
0 1
1 1 C 0A
0
1
qvh ¼ 1:745 10-3
The subscripts ‘a’ and ‘h’ stand for attitude and heading estimation parameters, respectively.
Fig. 4. Estimated (a) roll, (b) pitch and (c) heading angles using SMO compared with true values.
103
104
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Considering the observability indices for the output vector (48) as, k1 ¼ k2 ¼2 the new state vector according to (17) and thereby, the transformation matrix to the attitude determination subsystem is obtained as 1 0 1 0 ha1 g cosy sin j C BL h C B B f a1 C B g cos y cos jðdwx þ ox Þgðdwz þ oz Þsin j C C C¼B ð51Þ fa ¼ B C B ha2 C B g cos y cos j A @ A @ Lf ha2 g cos y sin jðdwx þ ox Þg sin yðdwy þ oy Þ 0
@ha1
B @j B @Lf ha 1 B @j @fa B ¼B B @ha2 @xa B @j B @ @Lf ha @j
2
@ha1 @y
@ha1 @ðdwx Þ
@Lf ha1 @y
@Lf ha1 @ðdwx Þ
@ha2 @y
@ha2 @ðdwx Þ
@Lf ha2 @y
@Lf ha2 @ðdwx Þ
1
@ha1 @ðdwy Þ C
0
g cos y cos j B a1 B C B @ha2 C ¼ B g sin j cos y @ C @ðdwy Þ C a4 @Lf ha A @Lf ha1 C C @ðdwy Þ C
2
g sin j sin y a2
0 a3
g sin y cos j
0
a5
a6
1 0 C 0C C 0C A a7
ð52Þ
@ðdwy Þ
with a1 ¼ g cos y sin jðdwx þ ox Þgðdwz þ oz Þcos j a2 ¼ g sin y cos jðdwx þ ox Þ a3 ¼ g cos y cos j a4 ¼ g cos y cos jðdwx þ ox Þ a5 ¼ g sin y sin jðdwx þ ox Þg cos yðdwy þ oy Þ a6 ¼ g cos y sin j a7 ¼ g sin y
ð53Þ
Fig. 5. Estimation error of (a) roll, (b) pitch and (c) heading angles using SMO from true values.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
105
Now, the SMO (24) is completed for the attitude determination subsystem (47) as follows: 0 1 0 1 ^ x þ ox Þ þ ðdw ^ y þ oy Þsin j ^ z þ oz Þcos j ^ tan y^ þ ðdw ^ tan y^ ðdw j_^ B C B C ^ y þ oy Þcos j ^ z þ oz Þsin j ^ ðdw ^ B ^_ C B C ðdw B y C B C x_^ a ¼ B ¼B C C 1 ^ w d _ x B dw C B C ^ b x @ A @ A _^ ^y b1 dw dw y þ
@fa @xa
1
La ðya ha ðx^ a ÞÞþ
@fa @xa
1
K a signðya ha ðx^ a ÞÞ
ð54Þ
4.2. Heading estimation The following dynamics is used as the base of the SMO to estimate the states of the heading angle determination subsystem: 10 w 1 1 0 ! 0 sin j x j j cos j ðoy þ dwy Þ þ cos ðoz þ dwz Þ 0 sin c_ cos y cos y cos y cos y C AB Aþ@ w ffiffiffiffiffiffiffiffiffiffiffi ffi p x_ h ¼ ð55Þ ¼@ y @ A _z b1 dwz dw 0 0 2bs2 wz where 0
sin j
cos y f h ðxh , oÞ ¼ @
j ðoy þ dwy Þ þ cos ðoz þ dwz Þ cos y
b1 dwz
1 A
Fig. 6. Estimation error of (a) roll, (b) pitch and (c) heading angles using EKF from true values.
ð56Þ
106
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
0
gh ðxh Þ ¼ @
0
sin j cos y
0
0
1
cos j cos y
pffiffiffiffiffiffiffiffiffiffiffiffi A 2bs2
ð57Þ
yh ¼ cm ¼ hh ðxh Þ
ð58Þ
cm ¼ c þ a dwz
ð59Þ
where, using a, the declination angle is considered as a linear function of the bias of z-axis gyroscope. Considering (58) and (59), the mapping is constructed as ! ! c þ dwz hh fh ðxh , oÞ ¼ ¼ sin j cos j 1 Lf hh cos y ðoy þ dwy Þ þ cos y ðoz þ dwz Þ b dwz 0 @h h @fh @ @c ¼ @Lf hh @xh @c
@hh @ðdwz Þ @Lf hh @ðdwz Þ
1 A¼
1 0
ð60Þ
!
1
ð61Þ
cos j 1 b cosy
The SMO of the heading determination subsystem (55) is completed as 0 1 0 sin j^ 1 1 1 _^ ^ y Þ þ cos j^^ ðoz þ dw ^ zÞ ðoy þ dw @fh @fh cos y _x^ ¼ @ c A ¼ @ cos y^ A ^ þ L ðy h ð x ÞÞ þ K h signðyh hh ðx^ h ÞÞ h h h h h _^ ^z @xh @xh b1 dw dw z
ð62Þ
Fig. 7. Estimation of gyroscope bias along x-axis using SMO. Table 3 Maximum values and total magnitudes of measurement data. Test no.
1 2 3 4 5 a
Maximum accelerometer output (m/s2) a
Maximum magnetometer output (mT)
Maximum gyroscope output (1/s)
9yx9
9yy9
9yz9
Mgnt
9mx9
9my9
9mz9
Mgnt
9ox9
9oy9
9oz9
Mgnt
3.289 2.415 2.312 1.675 3.125
4.358 2.456 3.291 2.566 2.741
10.72 10.29 10.84 10.49 10.43
10.8 10.46 10.97 10.61 10.45
38.02 37.13 36.11 36.06 32.93
39.9 37.92 45.77 37.64 35.72
72.37 59.09 77.73 59.28 61.45
78.69 68.65 81.71 68.38 68.01
0.4814 0.2876 0.4705 0.225 0.4635
0.4508 0.2113 0.3628 0.4904 0.2916
0.6137 0.4633 0.4779 0.5101 0.5716
0.6535 0.466 0.4925 0.512 0.5817
Magnitude.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 8. Vehicle path during test#1 in highway.
Fig. 9. Output data of (a) gyroscopes, (b) accelerometers and (c) magnetometers during test#1.
107
108
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
^ x and dw ^ y (54), the observer (62) can ^ and y^ , and the gyroscopes’ biases dw Considering the estimated attitude angles, j now be implemented using the gains, Lh and Kh. The complete details for the implementation of the SMO to both the attitude and the heading reference subsystems are given in Fig. 3. 4.3. Extended Kalman filter For a fair comparison, all the states in (54) and (62) are also estimated through the EKF. Therefore, the dynamic system is considered in the following standard form for the EKF: _ ¼ f ðxðtÞ,uðtÞ, oðtÞÞ þ gðxðtÞÞdðtÞ xðtÞ yðtÞ ¼ hðxðtÞÞ þ vðtÞ
ð63Þ
where, d and v are considered white Gaussian noises. Therefore the estimation of the state vector, x^ is computed by the EKF as follows: X X T X X X X X T _ þ Gqd GT H qv 1 H ; ð0Þ ¼ ð64Þ ¼ F þF 0 x_^ ¼ f ðx^ ,u, oÞ þ
X
HT qv 1 ðyhðx^ ÞÞ;
x^ ð0Þ ¼ x^ 0
ð65Þ
P
where, , qd and qv are the covariance matrices of the estimated states, x^ , the process noise, d and the measurement noise, v respectively. Furthermore, F, H and G are obtained as F¼
@f ðx,u, oÞ 9x ¼ x^ @x
Fig. 10. Estimation of (a) roll, (b) pitch and (c) heading angles using EKF compared with reference values of INS/GPS.
ð66Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
H¼
@h 9 @x x ¼ x^
@g 9 @x x ¼ x^ 5. Simulation and test results
G¼
109
ð67Þ
ð68Þ
5.1. Simulation results In this section, suitable simulations are made to assess the performance of the proposed SMO compared with that of the EKF. To generate reference attitude and heading trajectories, the Euler angles dynamics is simulated using the rotation rates of a vehicle’s body as inputs. Based on the reference trajectories, the measurement vectors of the simulated AHRS are obtained through the transformation of the Earth’s gravity and magnetic field vectors from the NED frame to the vehicle’s body frame. After considering the stochastic noises according to the specifications of the real MEMS sensors in Table 1, the performances of both the SMO and the EKF are simulated. According to Table 2, a priori information and the gain matrices corresponding to the EKF and the SMO algorithms are used in simulations and in experimental tests of the AHRS. Using the SMO, the estimated attitude-heading angles and the corresponding tracking errors with respect to the reference true values are shown in Figs. 4 and 5, respectively. Similarly, using the EKF, Fig. 6 shows the tracking errors of the estimated attitude-heading angles with respect to the reference true values. From Figs. 5 and 6, compared with the EKF, the SMO significantly accelerates the convergence rate of the estimation errors to zero. Furthermore, the estimated calibration bias of the x-axis gyroscope, dwx in Fig. 7 shows the parameter estimation performance of the SMO, for example. As shown in Figs. 5 and 6, the main superiority of the SMO to the EKF is its tracking performance which in turn results in the fast convergence of initial alignment errors of roll, pitch and heading angles to zero. However, the chattering error of the SMO could not be perfectly removed. By increasing the thickness of the boundary layer, l (45) the chattering phenomenon can be
Fig. 11. Errors of estimated (a) roll, (b) pitch and (c) heading angles using EKF with respect to reference values of INS/GPS.
110
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
effectively decreased. However, the tracking accuracy of the SMO decreases near to that of the EKF and also the convergence condition (44) may be deteriorated. Due to the additive uncertainties affecting directly the first equation of (3), the chattering error on the estimated roll angle is more considerable with respect to that of the other two angles. As the simulation results show, the EKF approximately yields the chattering-free estimation of attitude and heading angles. However, unlike the SMO, the EKF could not result in robust estimations in the presence of bias uncertainty and changing the intensity of noises. 5.2. Test results In this section, the proposed SMO is experimentally verified using the vehicular tests data of the Vitans inertial navigation system (INS) integrated with a ‘‘Garmin 35’’ GPS and a magnetic compass [22].Table 1 shows the main specifications of the magnetic and the inertial sensors included in the Vitans system. The raw measurements of the 3-axis gyroscopes, magnetometers and accelerometers of the Vitans system are used in the SMO and the EKF algorithms implemented for the AHRS. In the navigation mode of the Vitans system, providing qualified GPS signals, the integrated INS/GPS results in reference position and attitude-heading angles [22]. Therefore, compared to the reference attitudeheading angles, the computed attitude-heading angles through the SMO and the EKF are evaluated. Table 3 represents the maximum values and also the total magnitudes of the measurements made by the Vitans’ 3-axis sensors during 5 outdoor tests on a vehicle. The tests are executed in different maneuvering conditions of the vehicle in highways, mountain roads and urban areas to generate as much as possible extensive data, [23]. Therefore, after required alignment time of the integrated INS/GPS, the vehicle performs different maneuvers involving zigzag, velocity changes and both gradual and fast changes of attitude-heading angles. According to the data of Table 3 and considering the local magnitudes of the Earth’s gravity and magnetic fields as, 9.808 ms 2 and 50.90 micro-Tesla (mT), the vehicle is accelerated up to 1.2 ms 2 and is affected by magnetic disturbances up to 32 mT [24]. In other words, the intensity of magnetic disturbances due to local soft- and hard-iron magnetic fields may increase above 50% of the intensity of the Earth’s magnetic field. Furthermore, the non-gravity accelerations which are in fact the exogenous disturbances affecting the accelerometers of the AHRS are under 15% of the total magnitude of the Earth’s gravity field vector. Therefore, the
Fig. 12. Estimation of (a) roll, (b) pitch and (c) heading angles using SMO compared with reference values of INS/GPS.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
111
estimation accuracy of the heading angle, particularly using the EKF, will be lower than that may be obtained for the roll and pitch angles. Figs. 8 and 9 show the geographical latitude–longitudes of the tracked path and the output data of all the 3-axis sensors obtained during the highway test#1, respectively. During maneuvering along highway test trajectories, for example in the interval 300 s to 480 s and the interval 600 s to 770 s in Fig. 10, the vehicle’s roll and pitch angles are subjected to fast changes while the changes of the heading angle are gradual and smooth. The accuracy of navigated trajectory by the INS/ GPS mode of the Vitans system in Fig. 8 confirms the high-quality reference attitude and heading angles. According to Fig. 9, the vehicle movement from the start point, p1 to the point p2 on the trajectory of Fig. 8 takes about 100 s. During 40 s stopping at p2, the outputs of all the sensors may change only due to the measurement noises, as vivid in Fig. 9.
Fig. 13. Errors of estimated (a) roll, (b) pitch and (c) heading angles using SMO with respect to reference values of INS/GPS.
Table 4 Mean values and standard deviations of estimation errors. Test no. SMO
EKF
Mean value of estimation error (1)
1 2 3 4 5 a
Standard deviation of estimation error ( 71s)
Mean value of estimation error Standard deviation of estimation (1) error (7 1s)
Roll
Pitch
Heada
Roll
Pitch
Head
Roll
Pitch
Head
Roll
Pitch
Head
0.001 0.318 0.112 0.5338 0.249
0.428 0.384 0.007 0.1177 0.17
8.331 6.497 2.724 5.12 8.44
1.299 2.021 2.603 1.827 1.854
1.186 1.889 2.363 1.749 2.076
16.96 10.44 11.82 9.289 13.731
1.678 0.29 0.48 0.11 0.79
5.995 0.591 0.427 0.574 0.497
39.1 28.31 33.65 23.3 30.62
6.202 2.009 4.382 1.757 4.043
6.536 2.101 3.401 1.523 2.814
18.01 13.49 17.82 21.99 16.51
Heading.
112
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 14. Vehicle path during test#2 in urban area.
Fig. 15. Output data of (a) gyroscopes, (b) accelerometers and (c) magnetometers during test#2.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
113
Afterward, as may be seen in both Figs. 8 and 9, the vehicle moves to p3 to enter into the highway and then moves along it up to p4. Through crossing a bridge over the highway, the vehicle returns back along the other side of the highway in the interval p5 to p6. Along the test path of Fig. 8, compared with the reference attitude-heading angles of the INS/GPS, Figs. 10 and 11 show the estimated angles and their tracking errors made by the EKF. Similarly, the estimation performance of the SMO using the data of Fig. 9 for test#1 is represented in Figs. 12 and 13. Comparison of Figs. 11 and 13 shows that the SMO results in a better estimation quality of the vehicle’s attitude-heading with respect to the EKF. Using the EKF, the non-white and time-varying uncertainties in the bias term of the MEMS gyroscopes could not be perfectly estimated and compensated. On the other hand, owing to the non-gravity accelerations and the local magnetic fields affecting the measurement signals, P y (63), the theoretic covariance matrix, (65) could not be increased to place a greater weight on the data of the measurement equations. Consequently, as shown in Figs. 10 through 13, the tracking errors of the estimated angles increase with respect to time. It should be noted that the magnetometers are subjected to the magnetic disturbances of higher intensity compared with that of the non-gravity accelerations affecting the accelerometers. Therefore, in all of the tests recorded in Tables 3 and 4, the increase of estimation error of the heading angle is greater than those of the attitude angles. Besides, due to the linearization error of nonlinear terms in the EKF, by increasing the change rate of the under estimation attitude and heading angles, abrupt changes are observed in the estimation errors. The estimation errors for the heading angle could be decreased significantly through online calibration of the 3-axis magnetometers with respect to the local magnetic fields, see [23]. Furthermore, owing to using the magnetic sensors in the AHRS, both the EKF and the SMO estimate the magnetic heading of the vehicle, see Fig. 1. However, the heading angle of the reference INS/GPS is updated with respect to the true/geographic north of the Earth. Therefore, the mean values of the heading angle errors in Table 4 include a 71 declination angle which is approximately fixed in the tests location.
Fig. 16. Estimation of (a) roll, (b) pitch and (c) heading angles using EKF compared with reference values of INS/GPS during test#2.
114
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 17. Estimation of (a) roll, (b) pitch and (c) heading angles using SMO compared with reference values of INS/GPS during test#2.
In the urban area test#2, the vehicle performs a zigzag maneuver along the path of Fig. 14 and thereby imposes large and fast changes to the heading angle. Using the data of sensors in Fig. 15, the estimated attitude-heading angles of the vehicle through both the EKF and the SMO are shown in Figs. 16 and 17, respectively. From these figures, the superiority of the SMO to the EKF is vivid. The other tests numbered 3, 4 and 5 in Table 3 are performed in the mountain road, highway and urban area trajectories, respectively. For all the tests in Table 3, the mean values and the standard deviations of the estimation errors obtained using both the EKF and the SMO are shown in Table 4. Comparison of Fig. 10 with 12 and also Fig. 16 with 17 shows that by use of the nonlinear SMO, the tracking errors in particular during the fast changes of the heading and attitude angles are decreased. As a matter of fact, the nonlinear transformation method of the SMO results in significant decrease of estimation errors which are produced by the linearization of nonlinear terms in the EKF algorithm. Besides the nonlinear structure, the switching term of the SMO makes it more robust against the uncertainties due to the measurement noises, unknown changes of calibration parameters and modeling errors. Therefore, the long-time increasing errors in Fig. 11 which are produced owing to the integration of uncompensated uncertainties in the EKF (65), are reduced to the results of Fig. 13 through the SMO dynamics (24). According to Table 4, the statistical values of the estimation errors are decreased through replacing the EKF by the nonlinear and robust SMO. However, the non-gravity accelerations due to the changes of the vehicle’s velocity, particularly during braking and accelerating maneuvers, lead to deterioration of the estimated roll and pitch angles from the corresponding reference values. Owing to the considerable braking and acceleration times in the mountain road and zigzag maneuvers, the estimation errors of the attitude angles in tests#2, 4 and 5 may be more considerable values compared with those obtained in the highway tests numbered 1 and 3. 6. Conclusion As a replacement to the EKF, a nonlinear SMO algorithm has been proposed to estimate the attitude and heading angles of ground vehicles using a low-cost AHRS. Considering the increasing applications of non-affine nonlinear systems, the
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
115
SMO has been developed to a general class of nonlinear systems which are not necessarily control affine. As the simulation results showed, compared with the EKF, the nonlinear nature of the SMO yields a high convergence rate of estimation errors to zero. Therefore, the alignment process to determine the initial orientation and the calibration biases of gyroscopes, which is performed before the movement of a vehicle, is shortened by use of the SMO. Consequently, due to decreasing the random walk drifts of the MEMS navigation sensors, the SMO decreases the alignment errors. A short time alignment is also desired for a quick pre-flight preparation of light air vehicles. Unlike the EKF which uses the noise covariance matrices in the estimation algorithm, the SMO uses the upper bounds of the uncertainties in the system dynamics. Therefore, compared with the EKF, the SMO yields less estimation errors of the attitude-heading angles owing to the considerable bias uncertainties of the low-cost MEMS gyroscopes. References [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24]
A.J. Krener, A. Isidori, Linearization by output injection and nonlinear observers, Syst. Control Lett. 3 (1983) 47–52. A.J. Krener, W. Respondek, Nonlinear observers with linearizable error dynamics, SIAM J. Control Optim. 23 (1985) 197–216. A. Isidori, Nonlinear Control Systems, third ed., Springer, Berlin, 1995. S. Sastry, Nonlinear Systems: Analysis, Stability, and Control, first ed., Springer-Verlag, New York, 1999. Y. Xiong, M. Saif, Sliding mode observer for uncertain systems—part I: linear systems case, in: Proceedings of the 39th IEEE Conference Decision Control, Sydney, December, 2000, pp. 316–321. K.C. Veluvolu, S.Y. Chai, High gain observers with multiple sliding mode for state and unknown input estimations, in: Proceedings of the Fourth IEEE Conference on ICIEA, Xi’an, China, May, 2009, pp. 1179–1186. H. Jing, Q. Jing, Z. Changfan, L. Cheng, Design of sliding mode observer for uncertain nonlinear systems, in: Proceedings of the Seventh WCICA, Chongqing, China, June, 2008, pp. 1720–1723. B.L. Walcott, M.J. Corless, S.H. Zak, Comparative study of nonlinear state—observation techniques, Int. J. Control 45 (1987) 2109–2132. E.A. Misawa, J.K. Hedrick, Nonlinear observers, a state of the art survey, ASME J. Dyn. Syst. Meas., Control 111 (1989) 344–351. C. Edwards, S. Spurgeon, On the development of discontinuous observers, Int. J. Control 59 (1994) 1211–1229. V.I. Utkin, Sliding Modes in Control Optimization, first ed., Springer-Verlag, New York, 1992. S.V. Drakunov, Sliding mode observers based on equivalent control method, in: Proceedings of the 31st lEEE Conference on Decision Control, Tucson, Arizona, 1992, pp. 2368–2369. T. Floquet, J.P. Barbot, W. Perruquetti, M. Djemai, On the robust fault detection via a sliding mode disturbance observer, Int. J. Control 77 (7) (2004) 622–629. J.J.E. Slotine, W. Li, Applied Nonlinear Control, second ed., Prentice Hall, Englewood, Cliffs, NJ, 1991. D.H. Titterton, J.L. Weston, Strapdown Inertial Navigation Technology, second ed., AIAA and IEE, Sevenage, UK, 2004. S. Nasser, K.P. Schwartz, N. El-Sheimy, A. Noureldin, Modeling inertial sensors using autoregressive (AR) models, Navig.: J. Inst. Navig. 51 (4) (2004) 259–268. R.M. Rogers, Applied Mathematics in Integrated Navigation Systems, second ed., AIAA Series, Virginia, 2003. R.P.G. Collinson, Introduction to Avionic Systems, second ed., Kluwer Academic Publishers, Boston, 2003. T Fluguet, J Barbot, Super twisting algorithm-based step-by-step sliding mode observers for nonlinear systems with unknown inputs, Int. J. Syst. Sci. 38 (10) (2007) 803–815. R. Raoufi, A.S.I. Zinober, Smooth adaptive sliding mode observers in uncertain chaotic communication, Int. J. Syst. Sci. 38 (10) (2007) 931–942. B.L. Walcott, S.H. Zak, Observation of dynamical systems in the presence of bounded nonlinearities/uncertainties, in: Proceedings of the 25th IEEE Conference on Decision Control, Athens, Greek, 1986, pp. 961–966. TeKnol Ltd., COMPANAV-2, Integrated MEMS INS/GPS System for Aviation Applications, available at: /http://www.teknol.ru/pdf/en/CN-2_overvie w_en.pdfS, 2009. J. Keighobadi, Fuzzy calibration of a magnetic compass for vehicular applications, Mech. Syst. Signal Process. 25 (6) (2011) 1973–1987. J. Keighobadi, M.J. Yazdanpanah, M. Kabganian, An enhanced fuzzy H8 estimator applied to low-cost attitude-heading reference system, Kybernetes 40 (1/2) (2011) 300–326.
Contents lists available at SciVerse ScienceDirect
Mechanical Systems and Signal Processing journal homepage: www.elsevier.com/locate/ymssp
Design and implementation of SMO for a nonlinear MIMO AHRS Parisa Doostdar, Jafar Keighobadi n University of Tabriz, Faculty of Mechanical Engineering, 29 Bahman, Tabriz, P.C. 5166614766, Iran
a r t i c l e in f o
abstract
Article history: Received 13 April 2011 Received in revised form 3 February 2012 Accepted 18 February 2012 Available online 16 March 2012
In a low-cost attitude heading reference system (AHRS), the measurements made by MEMS inertial and magnetic sensors are affected by large parameter uncertainties, stochastic noises and unknown disturbances. In this paper, considering the robustness of the sliding mode observers (SMO) against both structured and unstructured uncertainties as well as exogenous inputs, the process of design and implementation of a nonlinear SMO is proposed for a low-cost AHRS. For simultaneous estimation of orientation variables and calibration biases of gyroscopes, a nonlinear and non-affine model of the AHRS is considered. Therefore, based on the Lie-algebraic method, the estimation algorithm is designed for a general class of non-affine nonlinear MIMO systems. In the proposed observer, owing to decreasing the required assumptions for coordinate transformation in recent literatures, the design process of the SMO is simplified. The gain matrices of the proposed SMO are obtained through ensuring the stability and the convergence of estimation errors based on Lyapunov’s direct method. The expected tracking performance of the robust state and parameter estimation algorithm compared to that of the extended Kalman filter (EKF) is evaluated through simulations and real experiments of a strapped AHRS on a ground vehicle. & 2012 Elsevier Ltd. All rights reserved.
Keywords: MIMO nonlinear system Sliding mode observer Attitude and heading Estimation
1. Introduction To measure 3-axis orientations using MEMS sensors of an AHRS, a nonlinear robust observer is proposed to handle both the modeling uncertainties and the exogenous unknown inputs. The AHRS uses 3-axis inclinometers (based on accelerometers), magnetometers and particularly gyroscopes, in which the mean value of the noises increases due to the time-integration of the measured angular rates. Therefore, considering the 2-norm bounded exogenous inputs and the uncertainties, which are affecting the calibration parameters of the MEMS gyroscopes, the nonlinear SMO is preferred to the extended Kalman filter. To design the nonlinear observer for a single-output nonlinear system, the system is transformed into a canonical form [1]. Krener et al. have presented a generalized version of this method for multi-output systems [2], which is also called the exact error linearization method, see also [3,4]. In this approach, by injection of the outputs and their Lie-derivatives, the nonlinear system is transformed into a linear equivalent system. Isidori has proposed a nonlinear coordinate transformation method which is frequently used in the observer design problems [5]. Using the coordinate transformation, the nonlinear MIMO system is transformed into the output injection model, and therefore a state estimation algorithm which is insensitive to the unknown inputs is developed. Due to the structural property of the considered nonlinear system, the system is decomposed into two interconnected subsystems in
n
Corresponding author. Tel./fax: þ 98411 3354153. E-mail address: [email protected] (J. Keighobadi).
0888-3270/$ - see front matter & 2012 Elsevier Ltd. All rights reserved. doi:10.1016/j.ymssp.2012.02.007
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
95
which the unknown inputs appear only in the last dynamical equation of each subsystem. Isidori’s observer design method requires that the distribution matrix of the unknown inputs satisfies the involutive property. Furthermore, the outputs should have a sufficient vector relative degree corresponding to the distribution matrix [5]. Therefore, when the sum of the relative degrees of all subsystems is smaller than the number of states, defining a full-order transformation matrix is a difficult problem. To solve the above-mentioned problems for decoupling the unknown inputs, a new transformation technique has been proposed which deals directly with the estimation of unknown inputs [6]. Therefore, under the structural assumptions on the distribution matrix of unknown inputs and using the outputs of relative degree ‘one’ with respect to the unknown inputs, an observer has been considered to estimate both the states and the unknown inputs [6]. The above-mentioned transformation methods require that the nonlinear system should be decomposed into two subsystems and the outputs should have a well-defined vector relative degree. Therefore, a suitable transformation may not be possible for a general class of nonlinear systems. In this paper, the nonlinear MIMO system is affected by large uncertainties due to unknown bounded disturbances, modeling uncertainties and measurement noises. Therefore, the SMO as a robust estimator of variable structure systems is used to keep the trajectory of estimated states remaining on the surface of true states in a finite time. Due to using a nonlinear discontinuous term in the SMO, the estimation error becomes completely insensitive to the uncertainties [7]. Using Lyapunov’s direct method in design of SMOs for nonlinear systems subjected to bounded nonlinearities and uncertainties was suggested by Walcott et al. [8]. Misawa and Hedrick [9], and Edwards and Spurgeon [10] further have developed a numerical tractable algorithm for Walcott–Zak’s SMO. Utkin has proposed the other category of SMOs using the equivalent control theory in which the output estimation error and its higher time-derivatives are considered as the sliding manifolds [11]. The idea of Utkin has been extended to a general class of nonlinear systems by Drakunov [12]. In most of the recent works in the field, the SMOs have been proposed for single output systems and there are fewer research works on the MIMO systems. Floquet and Barbot have considered a possible design method of SMOs for MIMO systems satisfying the structural and the matching conditions [13]. The main contribution of this paper is to design a SMO for a general class of nonlinear MIMO systems like as an AHRS, which is not necessarily control affine and is affected by the uncertainty of parameters and the unknown exogenous inputs. In the proposed method, through the transformation of the nonlinear MIMO system into a new state space, the measurement equations are perfectly linearized; and the system’s dynamics is decoupled into linearized subsystems. Therefore, the unstructured uncertainties as well as the state variables of the system could be estimated by the proposed SMO in the new state space. It should be noted that through inverting the transformation, the designed observer is formulated for the original system. Due to decreasing the required assumptions in design of the proposed SMO, the process of design, stability analysis and observability conditions are simplified considerably. The gain matrices of the proposed SMO are designed through ensuring the stability and the convergence of estimation error based on Lyapunov’s direct method. SMOs suffer from the chattering phenomenon, which may be attenuated by different modifying techniques imposed on the original observer. For example, in the boundary layer method, the discontinuous sign term of the observer is replaced by a saturation function along a thin layer neighboring the origin [14]. The proposed nonlinear SMO is implemented for the AHRS to estimate the orientations of a ground vehicle in different road conditions. The estimated attitude and heading angles by the SMO are compared with the reference orientations given by the integrated INS/GPS system. Furthermore, through simulations, the convergence rate and the tracking accuracy of the estimations obtained by the SMO are compared with those of the EKF. The rest of the paper is organized as follows. The paper first presents modeling of AHRS in Section 2. In Section 3, SMO is designed. Section 4 presents implementation of SMO and EKF to AHRS. Simulations and real test results are provided in Section 5. Concluding remarks are made in Section 6.
2. Attitude heading reference system In the AHRS, the 3-axis gyroscopes, accelerometers and magnetometers measure the rotation rates of the system body and the vectors of the Earth’s gravity and magnetic fields all along the body axes of the system, respectively. In Fig. 1, the sequence of the Euler angles, j, y and c represents the rotation of the vehicle’s body axes, x–y–z with respect to the local North–East–Down (NED) axes. Therefore, the orientation of the vehicle’s body could be updated through the numerical integration of Euler’s angles dynamics including the angular rates of the body. However, the considerable uncertainties in the measured angular rates using low cost MEMS gyroscopes causes the rotation angles to drift with respect to time. Therefore, the measurements made by the 3-axis accelerometers and magnetometers are used to update the rotation angles obtained from the Euler angles dynamics [15].
2.1. Sensor error model The bias of a rate gyroscope is considered its average output with respect to a zero input rotation rate over a specified period of time. The measurements made by the MEMS gyroscopes are not accurate due to the stability (change) of the calibration biases. In this paper, the bias term of the MEMS gyroscopes is modeled as the following first order
96
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 1. Representation of Euler angles.
Gauss–Markov process with a relatively large correlation time [16] qffiffiffiffiffiffiffiffiffiffiffiffi 1 _ dwðtÞ ¼ dwðtÞ þ 2bs2 wðtÞ
b
ð1Þ
where, b and s denote the correlation time and the standard deviation of the zero-mean noise process, w(t), respectively. 2.2. Attitude In this paper, the following attitude dynamics is used for the evolution of the roll and pitch angles [17] 0 1 0 1 sin y j_ cos j cos sin y y 1 sin j B C B C cos y @ A¼@ A ðo þ wþ dwÞ _y 0 cos j sin j
ð2Þ
o ¼ ½ ox oy oz T w ¼ ½wx
wy
wz T
where, the superscript, T stands for the transpose of the vectors. The dynamics of the roll and pitch angles, j and y (2) are expressed in terms of the body referenced body to NED frame rotation rate vector, o and the gyroscopes’ noise and bias vectors, w and dw, respectively. In high-resolution gyroscopes for example the ring-laser type, the angular rate of the body, o is composed with the rotation rate of the Earth and the transportation rate of the vehicle. However, the last two rates are considerably small compared with the noise floor of the MEMS gyroscopes. Corresponding to the three gyroscopes along the body x–y–z axes, the three components of the bias vector, dw as, dwx, dwy and dwz are augmented to the roll and pitch angles in the state vector. Therefore, the nonlinear dynamics of the attitude determination subsystem is completed as 0 1 0 _ 1 ðdwx þ ox Þ þ ðdwy þ oy Þsin j tan y þ ðdwz þ oz Þcos j tan y j B C ðdwy þ oy Þcos jðdwz þ oz Þsin j B y_ C B C B C B C 1 B C B C d w x C¼B C b _ d w x_ ¼ B x C B B C 1 B _ C B C b dwy @ dw y A B C @ A 1 _z dw b dwz 0 1 1 sin j tan y cos j tan y B 0 1 0 cos j sin j C B C wx B pffiffiffiffiffiffiffiffiffiffiffiffi C B 2bs2 C B C 0 0 ð3Þ þB C@ wy A pffiffiffiffiffiffiffiffiffiffiffiffi B C 2 B C wz b s 0 2 0 @ pffiffiffiffiffiffiffiffiffiffiffiffi A 0 0 2bs2
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
97
Fig. 2. Resolution of gravity vector along body axes.
In a strapdown AHRS, as Fig. 2 shows, the 3-axis accelerometers are assigned to measure the Earth’s gravity field vector of magnitude, g resolved along the vehicle’s body axes as [18] yx ¼ g sin y yy ¼ g cos y sin j yz ¼ g cosy cos j
ð4Þ
where, yx, yy and yz denote the measurements made by the accelerometers fixed along the body axes x, y and z, respectively. Therefore, the attitude angles could be computed through the matching of the Earth’s reference gravity vector in NED frame with the measurements of accelerometers along the body axes. In the matching equations (4), all the nongravitational accelerations are considered zero values. However, unlike the coriolis and centrifugal accelerations which could not be measured by the MEMS accelerometers, the linear accelerations owing to the possible accelerated movements of the AHRS will affect the accuracy of the obtained roll and pitch angles. Hence, during initial alignment process, the AHRS should not be moved to obtain the initial states of (3) as y ð5Þ y0 ¼ sin1 x g
j0 ¼ sin1
yy g cosðy0 Þ
ð6Þ
dwx0 ¼ ox
ð7Þ
dwy0 ¼ oy
ð8Þ
dwz0 ¼ oz
ð9Þ
where, the symbol, - denotes the average output of the inertial sensors over the alignment time. It is worth noting that, the mathematical singularity of (6) at y0 ¼901 does not occur in vehicular applications of the AHRS. 2.3. Heading The Earth’s magnetic field vector is generally used as the north direction reference of a MEMS AHRS to the alignment and the monitoring of heading angle. In the AHRS, once the roll and pitch angles are estimated, the measurements made by the 3-axis magnetometers are used to make a vector matching with respect to the resolved components of the Earth’s magnetic field vector in the local level plane [18]. Therefore, to update the heading angle, c the following dynamics and the computed magnetic heading will be integrated through the state estimation algorithms, in this paper 1 1 ðoy þw þ dwy Þ þ cos j ðoz þ wþ dwz Þ cos y cos y H1 ym ¼ cm ¼ cos1 HH
c_ ¼ sin j
H1 ¼ mx cos y þ my sin j sin y þ mz cos j sin y
ð10Þ
ð11Þ ð12Þ
98
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
H2 ¼ my cos jmz sin j
ð13Þ
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi HH ¼ H1 2 þ H2 2
ð14Þ
where, cm is the magnetic heading angle; as shown in Fig. 1, the horizontal component of the Earth’s magnetic field vector, HH is resolved to H1 and H2 along the local level axes, XH and YH, respectively. The components of the Earth’s magnetic field vector along the body x–y–z axes are denoted by mx, my and mz, respectively. 3. Uncertain nonlinear system In this paper, the SMO is proposed for a general class of MIMO nonlinear uncertain systems which are non-affine with respect to inputs u and o as _ ¼ f ðxðtÞ,uðtÞ, oðtÞÞ þ gðxðtÞÞdðtÞ xðtÞ yðtÞ ¼ hðxðtÞÞ
ð15Þ n
m
q
where, f(x,u,o), h(x) and g(x) are sufficiently smooth functions on the vector space, M; x 2 R , u 2 R , o 2 R and d 2 Rr are the state, known input, measurable input and unknown input vectors, respectively; g(x) is the distribution matrix of d. The measurement vector, y 2 Rp is assumed to be completely known and measurable at each sample time, t. 3.1. State transformation As the first step in design process of the nonlinear SMO, the nonlinear system (15) is transformed into an equivalent linear system based on the following state vector: z ¼ ½ðz1 ÞT i
z ¼
½zi1 ,
ðz2 ÞT
zi2 ,
. . .,
...
zikj T
ðzp ÞT T ¼ fðx,u, oÞ Lf hi
¼ ½hi
...
Lf ki 1 hi T ,
zi 2 ki
for i ¼ 1 : p
ð16Þ
where, the new state variables of the transformed system, ziki are the Lie derivatives of components of, h(x) with respect to vector f(x,u,o) in (15); for example as, Lfhi(x)¼[@hi(x)/@x]f. Regarding the fact that in nearly all real systems, the time derivatives of inputs u and o are not available; therefore, the transformed state vector should be independent of both u_ _ . To obtain a complete n-dimensional transformation matrix, @f/@x the fixed observability indices, k1 ,k2 ,. . .,kp should and o satisfy the following condition: p X
ki ¼ n
ð17Þ
i¼1
Assumption 1. The mapping, f(x, u, o) (16), which is used to transform the MIMO uncertain nonlinear system (15) into the new space, is a diffeomorphism, i.e. both the Jacobian matrix, @f/@x and its inverse, [@f/@x] 1 exist. Under Assumption 1, taking the time derivative of the transformed state vector (16) results in the following nonlinear system: h iT z_ ¼ ðz_ 1 ÞT ðz_ 2 ÞT . . . ðz_ p ÞT h iT y ¼ y1 T y2 T . . . yp T ð18Þ where, the p-tuple linearized subsystems are produced as _, z_ i ¼ Ai zi þF i ðzi ,u, oÞ þ Gi ðzi Þd þB1 i ðzi ,u, oÞu_ þ B2 i ðzi ,u, oÞo
for i ¼ 1 : p
yi ¼ C i zi 0
ð19Þ 0
1
0
1
0
...
0
B0 B Ai ¼ B @^
0
1
...
^
^
&
0C C C 2 RK i K i , 1A
0
0
0
...
0
B1 i ðzi ,u, oÞ ¼ 01m B2 i ðzi ,u, oÞ ¼ 01q
@ ðL h Þ @u f i
...
@ ðL h Þ @o f i
...
B B B F i ðz ,u, oÞ ¼ B B B @ i
0 0 ^ 0 Lf ki hi
1 C C C C 2 RK i 1 , C C A
@ K i 1 T Lf hi @u @ @ K i 1 T LfK i 2 hi Lf hi @o @o
0
1 Lg hi B L Lh C B g f i C B C C i ^ Gi ðz Þ ¼ B B C 2 RK i r B L Ln2 h C B g f iC @ A Lg Ln1 hi f
@ K i 2 L hi @u f
ð20Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
99
C i ¼ ½1 0 . . . 0 2 R1K i where, the superscript, T stands for the transposed matrix/vector. The decoupled subsystems (19) of the transformed MIMO nonlinear system (15) are grouped together to obtain the equivalent linear system as _ z_ ¼ Az þFðz,u, oÞ þ GðzÞd þB1 ðz,u, oÞu_ þ B2 ðz,u, oÞo
ð21Þ
A ¼ diag½A1 ,A2 ,. . .,Ap 2 Rnn , B1 ¼ ½B1 1 B1 2 . . .B1 p T 2 Rnm , B2 ¼ ½B2 1 B2 2 . . .B2 p T 2 Rnq and C ¼ diag½C 1 ,C 2 ,. . .,C p 2 Rpn
3.2. Observability In general, the observability of a non-linear system is affected by its inputs, unless the system is uniformly observable. Definition 1. The nonlinear system (15) is uniformly observable if Assumption 1 holds, i.e. there exist a p-tuple set {k1,k2,y,kp} such that the mapping (16) is invertible with respect to x and therefore, the observability rank condition is satisfied as [19] rank
@f ¼n @x
ð22Þ
3.3. Observer design The SMO is designed to estimate the state vector of the transformed system (21) as _ þLðyC z^ Þ þ K signðyC z^ Þ z_^ ¼ Az^ þ Fðz^ ,u, oÞ þ B1 ðz^ ,u, oÞu_ þ B2 ðz^ ,u, oÞo
ð23Þ
where, z^ denotes the estimate of z; L and K are the estimation gain matrices; and sign(.) represents the common sign function. For direct estimation of the original state vector, x under Assumption 1 and using the inverse transformation matrix, [@f/@x] 1 the proposed observer (23) is transformed into the original state space as 1 1 @f @f Lðyhðx^ ÞÞ þ K signðyhðx^ ÞÞ ð24Þ x_^ ¼ f ðx^ ,u, oÞ þ @x @x
3.4. Observer stability Using (21) and (23), the observer error dynamic is obtained as e_ ¼ z_ z_^ ¼ AeLCe þðFðz,u, oÞFðz^ ,u, oÞÞ þ ðB1 ðz,u, oÞB1 ðz^ ,u, oÞÞ u_ _ þðGdK signðyhðx^ ÞÞ þ ðB2 ðz,u, oÞB2 ðz^ ,u, oÞÞo ^ þðB1 B^ 1 Þu_ þ ðB2 B^ 2 Þo _ þ GdK signðyhðx^ ÞÞ ¼ A0 e þðFFÞ
ð25Þ
To prove the stability of the SMO, the following assumptions are considered. Assumption 2. The pair (A, C) is assumed to be detectable, therefore, there exist an observer gain, L such that A0 ¼A LC is a strictly Hurwitz matrix. _ :ro _ r u_ where the _ and :u: Assumption 3. The rates of the bounded input signals are also bounded values as, :o symbols :U: and – stand for the 2-norm of a vector/matrix and the upper bound value, respectively. Assumption 4. The unknown disturbance vector, d is bounded, i.e. there exists a positive real value, d that, :d: r d. Assumption 5. The 2-norm of the distribution matrix, G is bounded to a known upper bound, G with respect to its argument, z.
100
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Assumption 6. The nonlinear functions F(z,u,o), B1(z,u,o) and B2(z,u,o) satisfy the Lipschitz condition with respect to, z as :Fðz1 ,u, oÞFðz2 ,u, oÞ: rLF :z1 z2 :
ð26Þ
:B1 ðz1 ,u, oÞB1 ðz2 ,u, oÞ: rLB1 :z1 z2 :
ð27Þ
:B2 ðz1 ,u, oÞB2 ðz2 ,u, oÞ: rLB2 :z1 z2 :
ð28Þ
where, LF ,LB1 and LB2 are the Lipschitz constants. Under Assumptions 1 through 6, the stability of the SMO (23) can be investigated using the following Lyapunov function: V ¼ eT Pe e ¼ zz^
ð29Þ T
where, the positive definite matrix, P¼P is the unique solution of the well-known Lyapunov equation as PA0 þ A0 T P ¼ Q
ð30Þ
Differentiating the Lyapunov equation (29) with respect to time yields as ^ þ 2eT PðB1 B^ 1 Þu_ V_ ¼ eT Pe_ þ e_ T Pe ¼ eT ðPA0 þ A0 T PÞe þ 2eT PðFFÞ _ þ2ePðGdK signðyhðx^ ÞÞ þ 2eT PðB2 B^ 2 Þo
ð31Þ
Replacing (30) in (31) leads to ^ þ 2eT PðB1 B^ 1 Þu_ þ2eT PðB2 B^ 2 Þo _ þ 2eP Gd2ePK signðyhðx^ ÞÞ V_ ¼ eT Q eþ 2eT PðFFÞ
ð32Þ
Now, considering Assumptions 2 through 6, the following inequality is obtained: 2 2 2 2 _ lmax ðPÞ V_ r:e: lmin ðQ Þ þ2:e: LF lmax ðPÞ þ 2:e: LB1 u_ lmax ðPÞ þ 2:e: LB2 o
þ 2:e:Gdlmax ðPÞ þ 2:e:K lmax ðPÞ
ð33Þ
where, K denotes the upper bound of the gain matrix, K. Rearranging (33) results in 2 _ Þ þ 2:e:lmax ðPÞðGd þKÞ ¼ a:e:2 þb:e: V_ r :e: ½lmin ðQ Þlmax ðPÞð2LF þ 2LB1 u_ þ 2LB2 o
ð34Þ
_ Þ and b ¼ 2lmax ðPÞðGd þ KÞ. A negative definite, V_ requires that, a:e: Zb and where, a ¼ lmin ðQ Þlmax ðPÞð2LF þ2LB1 u_ þ 2LB2 o a 40 Therefore, a bounded error dynamics is ensured considering as
lmin ðQ Þ lmax ðPÞ
ð35Þ
a:e:2lmax ðPÞGd 2lmax ðPÞ
ð36Þ
_ o a 4 0 ) LF þ LB1 u_ þ LB2 o and a:e:Z b ) K r
Using the following well known inequality [20]: 2
2
2
a:e: þ 2b:e: þc r ða1Þ:e: þ b þ c,
8 a,b,c,e 2 R
ð37Þ
The inequality (34) is changed as 2
b 2 V_ r ða1Þ:e: þ 4
ð38Þ
The following inequality is valid for the positive definite matrix, P [14]: 2
2
lmin ðPÞ:e: reT Pe r lmax ðPÞ:e:
ð39Þ
Now, using the second inequality of (39), (38) yields V_ r ða1Þ
V
lmax ðPÞ
2
þ
b 4
ð40Þ
and therefore
# " 2 1a b 1a t þ t VðtÞ r Vð0Þ exp 1exp lmax ðPÞ 4ða1Þ lmax ðPÞ
Similarly, from the first inequality of (39) sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi V :eðtÞ:r lmin ðPÞ
ð41Þ
ð42Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Replacing (41) in (42) yields vffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi " #ffi u 2 2 u b 1a b t t þ exp :eðtÞ: r lmin ðPÞ Vð0Þ 2 4ða1Þ 4ða1Þlmin ðPÞ Therefore, the ultimate stability of the designed SMO is guaranteed as sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2 b lim :eðtÞ: r t-1 4ða1Þlmin ðPÞ
101
ð43Þ
ð44Þ
Consequently, the upper bound (36) is completely determined for the gain matrix, K . The gain matrices, L and K are properly designed to obtain robust estimations against measurement noises and modeling uncertainties. First, the feedback gain, L is designed to satisfy Lyapunov’s equation (30) and; therefore, the matrix A0 ¼A LC is a negative-definite matrix. Second, the sliding gain, K is designed to satisfy the stability conditions (34) and (43). Furthermore, regarding (44), the estimation error of the SMO, e is a small and bounded value. 3.5. Boundary layer observer By increase of the sliding gain, owing to the large oscillations and the short settling time, the discontinuous term, K signðyy^ Þ removes the effects of the uncertainties on the estimated states and also guarantees the stability of the error dynamics (25). However, the undesirable chattering phenomenon occurs in the estimations of the SMO. The chattering is decreased through ensuring the convergence of the estimations to the arbitrary small neighboring of the true states, and therefore, the boundary layer technique is considered in the SMO [21]. For example, the following simple version of the boundary layer method results in a continuous and smooth switching observer: 1 1 @f @f x_^ ¼ f ðx^ ,u, oÞ þ Lðyhðx^ ÞÞ þ K satðyhðx^ ÞÞ ð45Þ @x @x ( satðyhðx^ ÞÞ ¼
signðyhðx^ ÞÞ
if:yhðx^ Þ:4 l
yhðx^ Þ
if:yhðx^ Þ:r l
l
ð46Þ
where, l denotes the width of the boundary layer. The chattering phenomenon can be effectively decreased for large values of l; however, the tracking accuracy will be decreased and the convergence condition (44) may be deteriorated. Therefore, through adjusting, l a tradeoff among the tracking accuracy and the chattering suppression should be considered. 4. Implementation of SMO for AHRS The measurement noise filtering capability of the SMO is similar to that of the Kalman filter. However, the SMO has a high robustness in the presence of uncertainties in the measurement noises. In the nonlinear MIMO AHRS, due to the possibility of using different numbers/arrangements of the outputs in the mapping vector of coordinate transformation (16), different transformation matrices may be obtained for coordinate transformation. Therefore, to design a successful SMO for the nonlinear AHRS, a transformation vector which satisfies the observability condition (22) should be constructed. The AHRS is a measurement system which is affected by the measured input vector, o and therefore, without loss of generality, the SMO is continued without considering the known input vector, u. The elements of nonlinear matrices, f(x,o), g(x) and h(x) are smooth functions of the states, x¼[j y c d wx dwy dwz]T and Earth’s gravity term, g. 4.1. Attitude estimation In the implementation of SMO (24) for the AHRS, using (3) and (4) the attitude determination subsystem is considered as 0 1 j_ 0 1 wx B _ C B y C C C ¼ f a ðxa , oÞ þ ga ðxa ÞB x_ a ¼ B ð47Þ @ wy A B dw C @ _xA wz _y dw
ya ðxa Þ ¼ ha ðxa Þ ¼
g cos y sin j g cos y cos j
! ð48Þ
102
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
where, the nonlinear matrices, fa(xa,o), ga(xa) and ha(xa)are given as follows: 1 0 1 0 ðdwx þ ox Þ þ ðdwy þ oy Þsin j tan y þ ðdwz þ oz Þcos j tan y f a1 B C Bf C B ðdwy þ oy Þcos jðdwz þ oz Þsin j C B a2 C B C B C 1 f a ðxa , oÞ ¼ B ¼B C d w C x C b @ f a3 A B @ A 1 f a4 b dwy 0
1
sin j tan y
B B
0
cos j
0
0 pffiffiffiffiffiffiffiffiffiffiffiffi 2bs2
pffiffiffiffiffiffiffiffiffiffiffiffi ga ðxa Þ ¼ B B 2bs2 @
cos j tan y
ð49Þ
1
sin j C C C C 0 A 0
ð50Þ
Fig. 3. SMO block diagram.
Table 1 Specifications of inertial and magnetic sensors. Sensor type
Model
FSa range
Noise density
Bias stability
Initial bias error 7 1s
Nonlinearity (%FS)
Rate gyro Accelerometer (single-axis) Accelerometer (dual-axis) Magnetometer
ADXRS150 ADXL202 ADXL210E HMC1022
7 150 1/s 7 20 m/s2 7 100 m/s2 7 200 mTb
0.05 1/s/OHz 0.01 m/s2 0.01 m/s2 10 5 mT/OHz
0.01 1/s 0.05 m/s2 0.05 m/s2 0.10 m/s2
73 1/s 70.02g 70.02g 2 10 5 mT
0.1 0.2 0.2 0.1
a b
Full-scale. Micro-Tesla.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Table 2 A priori information and gain matrices for SMO and EKF. SMO
EKF
b ¼ 0:10
P
s ¼ 0:01
a
1
0
0
1
P
a¼1
0
1
10 0 B C La ¼ @ 10 1 A 1 1 0 1 5 1 B C K a ¼ @ 10 1 A 1 1 10 Lh ¼ 0:001
Kh ¼
¼ 105
0:01 0:001
5 h ¼ 10
0 1 1 0 0 B C qda ¼ ð7:615 10-5 Þ@ 0 1 0 A 0 0 1 1 0 qdh ¼ ð3:808 10-5 Þ 0 1 0
qva
1 B ¼ 0:015@ 0 0
0 1
1 1 C 0A
0
1
qvh ¼ 1:745 10-3
The subscripts ‘a’ and ‘h’ stand for attitude and heading estimation parameters, respectively.
Fig. 4. Estimated (a) roll, (b) pitch and (c) heading angles using SMO compared with true values.
103
104
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Considering the observability indices for the output vector (48) as, k1 ¼ k2 ¼2 the new state vector according to (17) and thereby, the transformation matrix to the attitude determination subsystem is obtained as 1 0 1 0 ha1 g cosy sin j C BL h C B B f a1 C B g cos y cos jðdwx þ ox Þgðdwz þ oz Þsin j C C C¼B ð51Þ fa ¼ B C B ha2 C B g cos y cos j A @ A @ Lf ha2 g cos y sin jðdwx þ ox Þg sin yðdwy þ oy Þ 0
@ha1
B @j B @Lf ha 1 B @j @fa B ¼B B @ha2 @xa B @j B @ @Lf ha @j
2
@ha1 @y
@ha1 @ðdwx Þ
@Lf ha1 @y
@Lf ha1 @ðdwx Þ
@ha2 @y
@ha2 @ðdwx Þ
@Lf ha2 @y
@Lf ha2 @ðdwx Þ
1
@ha1 @ðdwy Þ C
0
g cos y cos j B a1 B C B @ha2 C ¼ B g sin j cos y @ C @ðdwy Þ C a4 @Lf ha A @Lf ha1 C C @ðdwy Þ C
2
g sin j sin y a2
0 a3
g sin y cos j
0
a5
a6
1 0 C 0C C 0C A a7
ð52Þ
@ðdwy Þ
with a1 ¼ g cos y sin jðdwx þ ox Þgðdwz þ oz Þcos j a2 ¼ g sin y cos jðdwx þ ox Þ a3 ¼ g cos y cos j a4 ¼ g cos y cos jðdwx þ ox Þ a5 ¼ g sin y sin jðdwx þ ox Þg cos yðdwy þ oy Þ a6 ¼ g cos y sin j a7 ¼ g sin y
ð53Þ
Fig. 5. Estimation error of (a) roll, (b) pitch and (c) heading angles using SMO from true values.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
105
Now, the SMO (24) is completed for the attitude determination subsystem (47) as follows: 0 1 0 1 ^ x þ ox Þ þ ðdw ^ y þ oy Þsin j ^ z þ oz Þcos j ^ tan y^ þ ðdw ^ tan y^ ðdw j_^ B C B C ^ y þ oy Þcos j ^ z þ oz Þsin j ^ ðdw ^ B ^_ C B C ðdw B y C B C x_^ a ¼ B ¼B C C 1 ^ w d _ x B dw C B C ^ b x @ A @ A _^ ^y b1 dw dw y þ
@fa @xa
1
La ðya ha ðx^ a ÞÞþ
@fa @xa
1
K a signðya ha ðx^ a ÞÞ
ð54Þ
4.2. Heading estimation The following dynamics is used as the base of the SMO to estimate the states of the heading angle determination subsystem: 10 w 1 1 0 ! 0 sin j x j j cos j ðoy þ dwy Þ þ cos ðoz þ dwz Þ 0 sin c_ cos y cos y cos y cos y C AB Aþ@ w ffiffiffiffiffiffiffiffiffiffiffi ffi p x_ h ¼ ð55Þ ¼@ y @ A _z b1 dwz dw 0 0 2bs2 wz where 0
sin j
cos y f h ðxh , oÞ ¼ @
j ðoy þ dwy Þ þ cos ðoz þ dwz Þ cos y
b1 dwz
1 A
Fig. 6. Estimation error of (a) roll, (b) pitch and (c) heading angles using EKF from true values.
ð56Þ
106
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
0
gh ðxh Þ ¼ @
0
sin j cos y
0
0
1
cos j cos y
pffiffiffiffiffiffiffiffiffiffiffiffi A 2bs2
ð57Þ
yh ¼ cm ¼ hh ðxh Þ
ð58Þ
cm ¼ c þ a dwz
ð59Þ
where, using a, the declination angle is considered as a linear function of the bias of z-axis gyroscope. Considering (58) and (59), the mapping is constructed as ! ! c þ dwz hh fh ðxh , oÞ ¼ ¼ sin j cos j 1 Lf hh cos y ðoy þ dwy Þ þ cos y ðoz þ dwz Þ b dwz 0 @h h @fh @ @c ¼ @Lf hh @xh @c
@hh @ðdwz Þ @Lf hh @ðdwz Þ
1 A¼
1 0
ð60Þ
!
1
ð61Þ
cos j 1 b cosy
The SMO of the heading determination subsystem (55) is completed as 0 1 0 sin j^ 1 1 1 _^ ^ y Þ þ cos j^^ ðoz þ dw ^ zÞ ðoy þ dw @fh @fh cos y _x^ ¼ @ c A ¼ @ cos y^ A ^ þ L ðy h ð x ÞÞ þ K h signðyh hh ðx^ h ÞÞ h h h h h _^ ^z @xh @xh b1 dw dw z
ð62Þ
Fig. 7. Estimation of gyroscope bias along x-axis using SMO. Table 3 Maximum values and total magnitudes of measurement data. Test no.
1 2 3 4 5 a
Maximum accelerometer output (m/s2) a
Maximum magnetometer output (mT)
Maximum gyroscope output (1/s)
9yx9
9yy9
9yz9
Mgnt
9mx9
9my9
9mz9
Mgnt
9ox9
9oy9
9oz9
Mgnt
3.289 2.415 2.312 1.675 3.125
4.358 2.456 3.291 2.566 2.741
10.72 10.29 10.84 10.49 10.43
10.8 10.46 10.97 10.61 10.45
38.02 37.13 36.11 36.06 32.93
39.9 37.92 45.77 37.64 35.72
72.37 59.09 77.73 59.28 61.45
78.69 68.65 81.71 68.38 68.01
0.4814 0.2876 0.4705 0.225 0.4635
0.4508 0.2113 0.3628 0.4904 0.2916
0.6137 0.4633 0.4779 0.5101 0.5716
0.6535 0.466 0.4925 0.512 0.5817
Magnitude.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 8. Vehicle path during test#1 in highway.
Fig. 9. Output data of (a) gyroscopes, (b) accelerometers and (c) magnetometers during test#1.
107
108
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
^ x and dw ^ y (54), the observer (62) can ^ and y^ , and the gyroscopes’ biases dw Considering the estimated attitude angles, j now be implemented using the gains, Lh and Kh. The complete details for the implementation of the SMO to both the attitude and the heading reference subsystems are given in Fig. 3. 4.3. Extended Kalman filter For a fair comparison, all the states in (54) and (62) are also estimated through the EKF. Therefore, the dynamic system is considered in the following standard form for the EKF: _ ¼ f ðxðtÞ,uðtÞ, oðtÞÞ þ gðxðtÞÞdðtÞ xðtÞ yðtÞ ¼ hðxðtÞÞ þ vðtÞ
ð63Þ
where, d and v are considered white Gaussian noises. Therefore the estimation of the state vector, x^ is computed by the EKF as follows: X X T X X X X X T _ þ Gqd GT H qv 1 H ; ð0Þ ¼ ð64Þ ¼ F þF 0 x_^ ¼ f ðx^ ,u, oÞ þ
X
HT qv 1 ðyhðx^ ÞÞ;
x^ ð0Þ ¼ x^ 0
ð65Þ
P
where, , qd and qv are the covariance matrices of the estimated states, x^ , the process noise, d and the measurement noise, v respectively. Furthermore, F, H and G are obtained as F¼
@f ðx,u, oÞ 9x ¼ x^ @x
Fig. 10. Estimation of (a) roll, (b) pitch and (c) heading angles using EKF compared with reference values of INS/GPS.
ð66Þ
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
H¼
@h 9 @x x ¼ x^
@g 9 @x x ¼ x^ 5. Simulation and test results
G¼
109
ð67Þ
ð68Þ
5.1. Simulation results In this section, suitable simulations are made to assess the performance of the proposed SMO compared with that of the EKF. To generate reference attitude and heading trajectories, the Euler angles dynamics is simulated using the rotation rates of a vehicle’s body as inputs. Based on the reference trajectories, the measurement vectors of the simulated AHRS are obtained through the transformation of the Earth’s gravity and magnetic field vectors from the NED frame to the vehicle’s body frame. After considering the stochastic noises according to the specifications of the real MEMS sensors in Table 1, the performances of both the SMO and the EKF are simulated. According to Table 2, a priori information and the gain matrices corresponding to the EKF and the SMO algorithms are used in simulations and in experimental tests of the AHRS. Using the SMO, the estimated attitude-heading angles and the corresponding tracking errors with respect to the reference true values are shown in Figs. 4 and 5, respectively. Similarly, using the EKF, Fig. 6 shows the tracking errors of the estimated attitude-heading angles with respect to the reference true values. From Figs. 5 and 6, compared with the EKF, the SMO significantly accelerates the convergence rate of the estimation errors to zero. Furthermore, the estimated calibration bias of the x-axis gyroscope, dwx in Fig. 7 shows the parameter estimation performance of the SMO, for example. As shown in Figs. 5 and 6, the main superiority of the SMO to the EKF is its tracking performance which in turn results in the fast convergence of initial alignment errors of roll, pitch and heading angles to zero. However, the chattering error of the SMO could not be perfectly removed. By increasing the thickness of the boundary layer, l (45) the chattering phenomenon can be
Fig. 11. Errors of estimated (a) roll, (b) pitch and (c) heading angles using EKF with respect to reference values of INS/GPS.
110
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
effectively decreased. However, the tracking accuracy of the SMO decreases near to that of the EKF and also the convergence condition (44) may be deteriorated. Due to the additive uncertainties affecting directly the first equation of (3), the chattering error on the estimated roll angle is more considerable with respect to that of the other two angles. As the simulation results show, the EKF approximately yields the chattering-free estimation of attitude and heading angles. However, unlike the SMO, the EKF could not result in robust estimations in the presence of bias uncertainty and changing the intensity of noises. 5.2. Test results In this section, the proposed SMO is experimentally verified using the vehicular tests data of the Vitans inertial navigation system (INS) integrated with a ‘‘Garmin 35’’ GPS and a magnetic compass [22].Table 1 shows the main specifications of the magnetic and the inertial sensors included in the Vitans system. The raw measurements of the 3-axis gyroscopes, magnetometers and accelerometers of the Vitans system are used in the SMO and the EKF algorithms implemented for the AHRS. In the navigation mode of the Vitans system, providing qualified GPS signals, the integrated INS/GPS results in reference position and attitude-heading angles [22]. Therefore, compared to the reference attitudeheading angles, the computed attitude-heading angles through the SMO and the EKF are evaluated. Table 3 represents the maximum values and also the total magnitudes of the measurements made by the Vitans’ 3-axis sensors during 5 outdoor tests on a vehicle. The tests are executed in different maneuvering conditions of the vehicle in highways, mountain roads and urban areas to generate as much as possible extensive data, [23]. Therefore, after required alignment time of the integrated INS/GPS, the vehicle performs different maneuvers involving zigzag, velocity changes and both gradual and fast changes of attitude-heading angles. According to the data of Table 3 and considering the local magnitudes of the Earth’s gravity and magnetic fields as, 9.808 ms 2 and 50.90 micro-Tesla (mT), the vehicle is accelerated up to 1.2 ms 2 and is affected by magnetic disturbances up to 32 mT [24]. In other words, the intensity of magnetic disturbances due to local soft- and hard-iron magnetic fields may increase above 50% of the intensity of the Earth’s magnetic field. Furthermore, the non-gravity accelerations which are in fact the exogenous disturbances affecting the accelerometers of the AHRS are under 15% of the total magnitude of the Earth’s gravity field vector. Therefore, the
Fig. 12. Estimation of (a) roll, (b) pitch and (c) heading angles using SMO compared with reference values of INS/GPS.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
111
estimation accuracy of the heading angle, particularly using the EKF, will be lower than that may be obtained for the roll and pitch angles. Figs. 8 and 9 show the geographical latitude–longitudes of the tracked path and the output data of all the 3-axis sensors obtained during the highway test#1, respectively. During maneuvering along highway test trajectories, for example in the interval 300 s to 480 s and the interval 600 s to 770 s in Fig. 10, the vehicle’s roll and pitch angles are subjected to fast changes while the changes of the heading angle are gradual and smooth. The accuracy of navigated trajectory by the INS/ GPS mode of the Vitans system in Fig. 8 confirms the high-quality reference attitude and heading angles. According to Fig. 9, the vehicle movement from the start point, p1 to the point p2 on the trajectory of Fig. 8 takes about 100 s. During 40 s stopping at p2, the outputs of all the sensors may change only due to the measurement noises, as vivid in Fig. 9.
Fig. 13. Errors of estimated (a) roll, (b) pitch and (c) heading angles using SMO with respect to reference values of INS/GPS.
Table 4 Mean values and standard deviations of estimation errors. Test no. SMO
EKF
Mean value of estimation error (1)
1 2 3 4 5 a
Standard deviation of estimation error ( 71s)
Mean value of estimation error Standard deviation of estimation (1) error (7 1s)
Roll
Pitch
Heada
Roll
Pitch
Head
Roll
Pitch
Head
Roll
Pitch
Head
0.001 0.318 0.112 0.5338 0.249
0.428 0.384 0.007 0.1177 0.17
8.331 6.497 2.724 5.12 8.44
1.299 2.021 2.603 1.827 1.854
1.186 1.889 2.363 1.749 2.076
16.96 10.44 11.82 9.289 13.731
1.678 0.29 0.48 0.11 0.79
5.995 0.591 0.427 0.574 0.497
39.1 28.31 33.65 23.3 30.62
6.202 2.009 4.382 1.757 4.043
6.536 2.101 3.401 1.523 2.814
18.01 13.49 17.82 21.99 16.51
Heading.
112
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 14. Vehicle path during test#2 in urban area.
Fig. 15. Output data of (a) gyroscopes, (b) accelerometers and (c) magnetometers during test#2.
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
113
Afterward, as may be seen in both Figs. 8 and 9, the vehicle moves to p3 to enter into the highway and then moves along it up to p4. Through crossing a bridge over the highway, the vehicle returns back along the other side of the highway in the interval p5 to p6. Along the test path of Fig. 8, compared with the reference attitude-heading angles of the INS/GPS, Figs. 10 and 11 show the estimated angles and their tracking errors made by the EKF. Similarly, the estimation performance of the SMO using the data of Fig. 9 for test#1 is represented in Figs. 12 and 13. Comparison of Figs. 11 and 13 shows that the SMO results in a better estimation quality of the vehicle’s attitude-heading with respect to the EKF. Using the EKF, the non-white and time-varying uncertainties in the bias term of the MEMS gyroscopes could not be perfectly estimated and compensated. On the other hand, owing to the non-gravity accelerations and the local magnetic fields affecting the measurement signals, P y (63), the theoretic covariance matrix, (65) could not be increased to place a greater weight on the data of the measurement equations. Consequently, as shown in Figs. 10 through 13, the tracking errors of the estimated angles increase with respect to time. It should be noted that the magnetometers are subjected to the magnetic disturbances of higher intensity compared with that of the non-gravity accelerations affecting the accelerometers. Therefore, in all of the tests recorded in Tables 3 and 4, the increase of estimation error of the heading angle is greater than those of the attitude angles. Besides, due to the linearization error of nonlinear terms in the EKF, by increasing the change rate of the under estimation attitude and heading angles, abrupt changes are observed in the estimation errors. The estimation errors for the heading angle could be decreased significantly through online calibration of the 3-axis magnetometers with respect to the local magnetic fields, see [23]. Furthermore, owing to using the magnetic sensors in the AHRS, both the EKF and the SMO estimate the magnetic heading of the vehicle, see Fig. 1. However, the heading angle of the reference INS/GPS is updated with respect to the true/geographic north of the Earth. Therefore, the mean values of the heading angle errors in Table 4 include a 71 declination angle which is approximately fixed in the tests location.
Fig. 16. Estimation of (a) roll, (b) pitch and (c) heading angles using EKF compared with reference values of INS/GPS during test#2.
114
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
Fig. 17. Estimation of (a) roll, (b) pitch and (c) heading angles using SMO compared with reference values of INS/GPS during test#2.
In the urban area test#2, the vehicle performs a zigzag maneuver along the path of Fig. 14 and thereby imposes large and fast changes to the heading angle. Using the data of sensors in Fig. 15, the estimated attitude-heading angles of the vehicle through both the EKF and the SMO are shown in Figs. 16 and 17, respectively. From these figures, the superiority of the SMO to the EKF is vivid. The other tests numbered 3, 4 and 5 in Table 3 are performed in the mountain road, highway and urban area trajectories, respectively. For all the tests in Table 3, the mean values and the standard deviations of the estimation errors obtained using both the EKF and the SMO are shown in Table 4. Comparison of Fig. 10 with 12 and also Fig. 16 with 17 shows that by use of the nonlinear SMO, the tracking errors in particular during the fast changes of the heading and attitude angles are decreased. As a matter of fact, the nonlinear transformation method of the SMO results in significant decrease of estimation errors which are produced by the linearization of nonlinear terms in the EKF algorithm. Besides the nonlinear structure, the switching term of the SMO makes it more robust against the uncertainties due to the measurement noises, unknown changes of calibration parameters and modeling errors. Therefore, the long-time increasing errors in Fig. 11 which are produced owing to the integration of uncompensated uncertainties in the EKF (65), are reduced to the results of Fig. 13 through the SMO dynamics (24). According to Table 4, the statistical values of the estimation errors are decreased through replacing the EKF by the nonlinear and robust SMO. However, the non-gravity accelerations due to the changes of the vehicle’s velocity, particularly during braking and accelerating maneuvers, lead to deterioration of the estimated roll and pitch angles from the corresponding reference values. Owing to the considerable braking and acceleration times in the mountain road and zigzag maneuvers, the estimation errors of the attitude angles in tests#2, 4 and 5 may be more considerable values compared with those obtained in the highway tests numbered 1 and 3. 6. Conclusion As a replacement to the EKF, a nonlinear SMO algorithm has been proposed to estimate the attitude and heading angles of ground vehicles using a low-cost AHRS. Considering the increasing applications of non-affine nonlinear systems, the
P. Doostdar, J. Keighobadi / Mechanical Systems and Signal Processing 32 (2012) 94–115
115
SMO has been developed to a general class of nonlinear systems which are not necessarily control affine. As the simulation results showed, compared with the EKF, the nonlinear nature of the SMO yields a high convergence rate of estimation errors to zero. Therefore, the alignment process to determine the initial orientation and the calibration biases of gyroscopes, which is performed before the movement of a vehicle, is shortened by use of the SMO. Consequently, due to decreasing the random walk drifts of the MEMS navigation sensors, the SMO decreases the alignment errors. A short time alignment is also desired for a quick pre-flight preparation of light air vehicles. Unlike the EKF which uses the noise covariance matrices in the estimation algorithm, the SMO uses the upper bounds of the uncertainties in the system dynamics. Therefore, compared with the EKF, the SMO yields less estimation errors of the attitude-heading angles owing to the considerable bias uncertainties of the low-cost MEMS gyroscopes. References [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24]
A.J. Krener, A. Isidori, Linearization by output injection and nonlinear observers, Syst. Control Lett. 3 (1983) 47–52. A.J. Krener, W. Respondek, Nonlinear observers with linearizable error dynamics, SIAM J. Control Optim. 23 (1985) 197–216. A. Isidori, Nonlinear Control Systems, third ed., Springer, Berlin, 1995. S. Sastry, Nonlinear Systems: Analysis, Stability, and Control, first ed., Springer-Verlag, New York, 1999. Y. Xiong, M. Saif, Sliding mode observer for uncertain systems—part I: linear systems case, in: Proceedings of the 39th IEEE Conference Decision Control, Sydney, December, 2000, pp. 316–321. K.C. Veluvolu, S.Y. Chai, High gain observers with multiple sliding mode for state and unknown input estimations, in: Proceedings of the Fourth IEEE Conference on ICIEA, Xi’an, China, May, 2009, pp. 1179–1186. H. Jing, Q. Jing, Z. Changfan, L. Cheng, Design of sliding mode observer for uncertain nonlinear systems, in: Proceedings of the Seventh WCICA, Chongqing, China, June, 2008, pp. 1720–1723. B.L. Walcott, M.J. Corless, S.H. Zak, Comparative study of nonlinear state—observation techniques, Int. J. Control 45 (1987) 2109–2132. E.A. Misawa, J.K. Hedrick, Nonlinear observers, a state of the art survey, ASME J. Dyn. Syst. Meas., Control 111 (1989) 344–351. C. Edwards, S. Spurgeon, On the development of discontinuous observers, Int. J. Control 59 (1994) 1211–1229. V.I. Utkin, Sliding Modes in Control Optimization, first ed., Springer-Verlag, New York, 1992. S.V. Drakunov, Sliding mode observers based on equivalent control method, in: Proceedings of the 31st lEEE Conference on Decision Control, Tucson, Arizona, 1992, pp. 2368–2369. T. Floquet, J.P. Barbot, W. Perruquetti, M. Djemai, On the robust fault detection via a sliding mode disturbance observer, Int. J. Control 77 (7) (2004) 622–629. J.J.E. Slotine, W. Li, Applied Nonlinear Control, second ed., Prentice Hall, Englewood, Cliffs, NJ, 1991. D.H. Titterton, J.L. Weston, Strapdown Inertial Navigation Technology, second ed., AIAA and IEE, Sevenage, UK, 2004. S. Nasser, K.P. Schwartz, N. El-Sheimy, A. Noureldin, Modeling inertial sensors using autoregressive (AR) models, Navig.: J. Inst. Navig. 51 (4) (2004) 259–268. R.M. Rogers, Applied Mathematics in Integrated Navigation Systems, second ed., AIAA Series, Virginia, 2003. R.P.G. Collinson, Introduction to Avionic Systems, second ed., Kluwer Academic Publishers, Boston, 2003. T Fluguet, J Barbot, Super twisting algorithm-based step-by-step sliding mode observers for nonlinear systems with unknown inputs, Int. J. Syst. Sci. 38 (10) (2007) 803–815. R. Raoufi, A.S.I. Zinober, Smooth adaptive sliding mode observers in uncertain chaotic communication, Int. J. Syst. Sci. 38 (10) (2007) 931–942. B.L. Walcott, S.H. Zak, Observation of dynamical systems in the presence of bounded nonlinearities/uncertainties, in: Proceedings of the 25th IEEE Conference on Decision Control, Athens, Greek, 1986, pp. 961–966. TeKnol Ltd., COMPANAV-2, Integrated MEMS INS/GPS System for Aviation Applications, available at: /http://www.teknol.ru/pdf/en/CN-2_overvie w_en.pdfS, 2009. J. Keighobadi, Fuzzy calibration of a magnetic compass for vehicular applications, Mech. Syst. Signal Process. 25 (6) (2011) 1973–1987. J. Keighobadi, M.J. Yazdanpanah, M. Kabganian, An enhanced fuzzy H8 estimator applied to low-cost attitude-heading reference system, Kybernetes 40 (1/2) (2011) 300–326.