CC BY
157
ISSN 1992-6502 ( P ri nt)_
2014. Vol. 18, no. 3 (64). P. 3-15
Swhmm QjrAQnQj
ISSN 2225-2789 (Online) http://journal.ugatu.ac.ru
UDK 004.65
Gaussian sum based adaptive cubature Kalman filtering
applied to UAV's integrated navigation system
1 2 3
Hamza Benzerrouk , Alexander Nebylov , Hassen Salhi
1 hb.iiaat@gmail.com, 2 nebylov@aanet.ru, 3 hassensalhi@yahoo.fr
1,2 Saint-Petersburg State University of Aerospace Instrumentation (SUAI), Russia, International Institute for Advanced Aerospace Technologies (IIAAT) 3 Saad Dahlab University of Blida, Algeria, Electronic Department, SET Laboratory (Systèmes Electriques et Télécommandé)
Date submitted 15.04.2014
Abstract. In this paper, adaptive and robust non Gaussian sensor fusion INS/GNSS is proposed to solve specific problem of non linear time variant state space estimation with measurement outliers, different algorithms are proposed to solve this specific problem generally occurs in intentional and non intentional interferences caused by other radio navigation sources, or by the GNSS receivers deterioration. Non linear approximation techniques such as Extended Kalman filter EKF and modern Cubature based Kalman Filters are computed to estimate the navigation states for UAV flight control. Several comparisons are conduced and analyzed in order to compare the accuracy and the convergence of different approaches usually applied in navigation data fusion purposes. The modern non linear filter algorithm called Cubature Kalman Filter CKF which provides more accurate estimation with more stability in Tracking data fusion application is compared with conventional non linear filters. In this work, CKF is compared with EKF in ideal conditions and during GNSS impulsive interferences modeled as non Gaussian noises "Sum of Gaussian" supposed to occur during specific interval of time, during the same interval, we assume additional denied environment which consists in the variation of the Gaussian sum noise covariance, then, innovation based adaptive fading approach is selected and used to modify the covariance calculation of the parallel non linear filters performed in this work. Interesting results are observed, discussed with real perspectives in navigation data fusion for real time applications under multiple denied environment parameters.
Key words: IMU; MEMS; GPS; GNSS; Kalman filtering; Cubature Kalman Filter CKF; Gaussian sum.
1. INTRODUCTION
Data fusion in denied environment for non linear system is one the most important problem in Multisensory fusion and integrated navigation systems today. In this paper, sensors used are gyro-
scopes and accelerometers components of inertial as the main system with external aid provided by GPS and GLONASS receivers known recently as Global Navigation Satellite System "GNSS" solutions. Multiple basic and complex algorithms for data fusion based on IMU/GNSS have been widely discussed in the specialized literature [1-4]. In this paper, it is assumed that accelerometers and gyroscopes are in the category "Low cost" which gives a special interest for real time applications where most sensors are MEMS Micro Electrical Mechanical Systems based technology. The most inconvenient of these inertial sensors are the biases and drifts
growing during time, which needs to be bounded by another technology of sensors such as used in our work, called GNSS. GNSS gives today's satellite trajectory and high-precision navigation. Inertial sensors combined with GNSS receiver are a good alternative and reliable integrated system for navigation purposes. However, "GNSS bands'' suffer interference from the services in the frequency band, in particular, high power pulsed signals from Distance Measuring Equipment (DME) and Tactical Air Navigation (TACAN) systems embedded on most aircrafts. The pulsed interference degrades received Signal to Interference and Noise Ratio (SINR), lowers the acquisition sensitivity and even causes the tracking loops to diverge. To ensure robust navigation accuracy and integrity, interference mitigation is necessary. As a first step, adaptive integrated navigation systems INS/GNSS is developed for different aerospace applications. However
in our work, we focus on GNSS outliers caused by multipath scenario, with bad satellite visibility due to flights in canyon environment, or due to non intentional interferences such caused by multiple GSM signals, radio, Bluetooth, Jammer, multiple satellite communication technologies such as Iridi-um,...etc [5-7]. Nevertheless, the classic form of INS/GNSS data fusion is not adaptive against measurement's outliers ... etc. In this paper, a tentative to create robust adaptive non linear filters for navigation sensors is performed, a solution is proposed based on Gaussian sum adaptive extended Kalman filter (GSAEKF) and Gaussian sum adaptive Cubature Kalman Filter GSACKF, robust adaptive version of CKF algorithm [7].
2. INERTIAL SENSORS
Inertial Measurement Unit "IMU" is the basis of inertial navigation system. It is based on 03 ac-celerometers and 03 gyroscopes in addition to 03 magnetometers in most modern strapdown IMU's. The technology used during the last 50 years has been developed into: Gimbaled INS and Strapdown INS. In our work, the model used is related to strapdown technology with fixed inertial sensors MEMS based, in parallel with body axes. Most of today's inertial sensors are micro electromechanical systems (MEMS). This technology was first used for commercial purposes in the 1990's, and enabled new applications through high miniaturization and cost reduction. Inertial sensors began to be used in completely new domains, such as robotics, aerospace, guidance navigation and control and Pedestrian navigation. However, this miniaturization and cost reduction influences the performance of the accelerometers and gyroscopes, which explains why some inertial sensors based on previous technologies are still used for high-performance purposes. Gimbaled INS are mechanical with special Horizontal stabilization control with very expensive cost, they are usually used onboard satellites, spacecrafts, submarine ... etc.
IMUs based on MEMS sensors are strap-down systems, which mean the sensor's orientation depends of the orientation of the object it is on. Theoretically, all types of previously shown MEMS in-ertial sensors are mounted in an IMU. As an example, a in Revo board, there are accelerometers, gyroscopes, magnetometers and baro-altimeter based on MPU6000 sensor fusion compound [8].
2.1 Mechanization of inertial measurement unit
Inertial navigation system is divided in two principal parts: IMU and Digital Signal Processing of sensors data fusion. The following section describes how inertial sensors outputs are integrated and fused especially how navigation is processed
Fig. 1. Revo board 'Autopilot' with integrated
Strapdown Inertial Measurement Unit with MCU STM/MEMS gyroscopes, accelerometers and magnetometers
Strapdown INS mechanization is described such as in Fig. 1 and Fig. 2 with a general diagram of SINS as described , based on inertial sensors output; accelerometers and gyroscopes. Inertial navigation system can't ensure long term accuracy of its output, and depends on external aid such as GNSS and other sensors in most of aerospace applications [9, 10].
Accelerometer x, y, z
Fig. 2. IMU/INS Sensors output integration based on gyroscopes and accelerometers
2.2 IMU Sensors output integration
Inertial measurement units (IMUs) typically contain three orthogonal rate-gyroscopes and three orthogonal accelerometers, measuring angular velocity and linear acceleration respectively. Ideally, the output of the rate-gyroscopes is written as
©b(0=k*(0 MO (1)
In practice, however, the outputs contain errors and are written as the formula given below:
© b (t ) = ©b (t )-S© b (t 8©b (t) = [s© bx(t) 8©by(t) 8© bz(t)f. (2)
Integrating these yields the updated attitude information for the system provides the following equation:
(t) = C (t)Q(t), (3)
Q(t ) =
dt 0
© bz (t)
- ©
bz
(t )
©
by\
'by (t ) © bx (t )
e; (t+At )=e; (t > ^)dt = e; (t )
,(t )■
-© bx (t ) 0
(4)
I —
"j© b ) Q(t )At + 1 - C0S(©b Atl) (Q(t )At )2 ©j An ©&At
(5)
Similarly, accelerometers outputs can be written as
ab (t)=[abx(t) aby(t) abz(t)]\
a,
(t ) = ab (t )-8a& (t ).
(6)
Two integrations subsequently yield velocity and position updates as follows Velocity integration:
V„k = K,*-i + " g„). (7)
Position integration:
Pos„k = Pos„,k_i +At (V„gk ), (8)
where g is the estimated gravity vector and At is the data period. Collectively, equations (1) to (10) describe the system model.
3. GNSS GLOBAL NAVIGATION SATELLITE SYSTEM (GPS, GLONASS, GALILEO, COMPASS)
GNSS signal processing is much explored based on different algorithms tested more and more
in real time conditions and in simulations through the specialized literature. We focus on the effect of more satellite visibility in order improve geometry dilution of precision due to the high number of satellites GPS+GLONASS (36-40). GLONASS satellites also broadcast signals in the L1 and L2 subbands of the radio frequency spectrum as described in the literature. It is observed in some situation several interferences from different sources for GPS and GLONASS during static and dynamic positioning. GNSS outages or outliers cause accuracy degradation, and sometimes undelivered GNSS receiver positioning [10].
Fig. 3. Power Spectral Densities of GNSS Signals
However unlike GPS, GLONASS (Russian) uses frequency division multiple access (FDMA) in both L1and L2 frequency sub-bands. This means that each satellite modulates the same ranging code on carrier signals with slightly different frequencies and is identified by a slot number rather than a Pseudo random Noise (PRN) number. GNSS based on GPS and GLONASS (European system Galileo and Chinese system Compass in the future) , are well known satellite navigation systems and uses parallel positioning techniques; the only difference is that GPS sends different messages on the same frequency (L1, L2, L5) and GLONASS sends the same message on multiple frequencies (L1, L2, ...). It is important to consider in the near future the new statement of GNSS constellation including Galileo future European system and COMPASS the future Chinese system. Each space constellation has slightly different orbital plane parameters. In this paper, GPS and GLONASS C/A codes are considered in INS/GNSS data fusion.
3.1 Equations of GPS Navigation
The In this work, the standard, non differential, civilian signal is used. It provides a lower accuracy
0
x
but acceptable in low cost integrated navigation systems, is the lowest cost GPS solution is advantageous because of its availability. The standard measurement of the GPS system is the pseudorange and the pseudo-rate. This defines the approximate range from the user GPS receiver reference point to GPS or GNSS satellites. The pseudo-range is the true distance corrupted with differential errors specified by the following formula:
p j = r} +ôp r clk +ôp Ion -Sp
s,clk
+ v,
(9)
where:
r Pseudo-range from the user to the jth satellite Geometric range from the user to the jth satellite;
Sp Range equivalent receiver clock bias offset
from GPS system time; Sp Range equivalent satellite clock bias offset
from GPS system time; Sp.on Ionospheric signal attenuation error;
v Zero mean white noise.
GNSS Satellite position is determined from broadcast ephemeris parameters. Included is a correction for the satellite clock bias which causes hundred meters error in positioning. There are multiple correction models that account for atmospheric perturbations of the signal and its attenuation. In major research works, the model of Klobuchar Ionospheric model is used. This model is defined by the following equation:
Sp ion = 5 x 10 9 + A cos
- to )
P
(10)
where A and P represent the broadcast Klobuchar co-efficients summed with the latitude of the ionospheric sub-point and t0 represents the time of day (usually midday) at which ionospheric attenuation is greatest. Once compensation for satellite clock bias and atmospheric effects are applied Eq. (9) is reduced to:
p j = ri +Sp r.clk + v •
(11)
Eq. (10) is then simultaneously solved for position and user clock bias resulting in the desired navigation solution. The geometric range from Eq. (11) can be expanded explicitly:
Pj = [(X -x)2 +{YJ -yj +{Zj -z)2f2 + Sp^ + v, (12)
where [x ., Y., Zj ]r [3 x l] are components of the yth satellite's position in ECEF coordinates and
[x, y, z ]r [3 xl] are components of user's filtered position in ECEF coordinates.
A transformation from NED to ECEF and vice versa is required for the calculation of Eq. (11). These transformations are complicated by an ellipsoidal earth model with large GPS satellite altitudes. This study concerns non-intentional interferences and outliers/outages in GNSS signal for civilian GNSS receivers. All adjacent communication systems to GNSS band which is a potential source of interferences and have been studied in the literature [11] are assumed to affect GNSS signal during an interval of time. In this work direct data fusion technique is applied to an important case when measurement outliers occur and affect GPS and GLONASS receivers during UAV navigation, with an impulsive noise modeled as Gaussian mixture noise [11].
3.1 UAV GNC System (Guidance Navigation & Control)
We now describe the application of the SPKF/CKF to the problem of loosely coupled GPS/INS integration for guidance, navigation and control (GNC) of an unmanned aerial vehicle (UAV). The main subcomponents of such a GNC system is a vehicle control system and a guidance & navigation system (GNS) as shown in Fig. 4. Fixed Wing and Rotor Wing UAVs have been simulated through different dynamical parameters. The embedded system includes an Inertial MEMS based IMU, an 10 Hz GPS / GLONASS synchronized receiver, and a DSP Design Autopilot REVO board for real time implementation. UAV nonlinear control system is based on non linear adaptive non Gaussian state estimators selected as EKF, and CKF with novel formulations Gaussian Sum Adaptive EKF GSAEKF and Gaussian Sum Adaptive CKF GSACKF. We have implemented EKF and Cubature based KF-CKF in sensor fusion algorithms in Gaussian conditions and during outliers (modeled as non Gaussian) in order to observe the effect on the accuracy and the convergence of both filters. In the next sections, we will describe the model of UAV system process and observation (measurement) computed and simulated within EKF and CKF based data fusion algorithm.
Gaussian sum filters have been developed for multiple non linear filters such as EKF, UKF and Particle filters, but have not been well explored using CKF and not yet developed with adaptive forms.
Fig. 4. UAV Guidance, navigation and control
In this work, Gaussian sum based CKF is derivates and is applied to a specific case with non linear state estimation with non Gaussian impulsive noise modeled by Alpha-Stable density in addition to outliers that affect twice Gaussian component of the non Gaussian density. We then focus on the application of the adaptive fading EKF-AEKF and adaptive fading CKF-ACKF to the integrated navigation problem. We specifically detail the development of a Adaptive based Cubature Kalman Filter ACKF for loosely coupled implementation for integrating GPS measurements with an IMU within the context of autonomous UAV navigation.
4. KALMAN FILTER AND ITS EXTENDED VERSION (EKF)
In estimation theory, the optimal linear filter when affected by white Gaussian noises is called Kalman filter which is also equivalent to Maximum Likelihood estimator. However in most real time and practical navigation applications, a nominal trajectory does not exist beforehand. The solution is to use the current estimated state from the filter at each time step k as the linearization reference from which the estimation procedure can proceed. Such algorithm is known as extended Kalman filter (EKF) with its multiple versions [12]. Let us describe below the algorithm of EKF based on state space model as given by :
x
k+i
= f (x
yk = g (xk, wk
(13)
(14)
Linearization using Taylor approximation at the first order gives the state space model given in most referenced literature. Fk(.) is the Jacobian matrix of fk(.) and Hk(.) is the Jacobian matrix of hk(.). Thus, following algorithm is obtained: Initialization:
et Po . (15)
Prediction :
xk+i/k= fk (xk),
Pk / k-i = F (^k) Pk-i F (^k)T + Q.
(16) (17)
Update :
K = P/k-1 HkT(Xklk-i) ¡Hk (Xk/k-i)P/k-1 HkTx/k-i) + RkI-1 , (18)
Xk Xk / k-1 + Kk ¡Zk hk ( Xk / k-1)] Pk = Pk / k-1 - KkHk (xxk / k-1) Pk / k-1
(19)
(20)
The meaning of the extended Kalman filter can be understudied by extending the same equation of Kalman filter at the difference that in the non linear filtering, EKF is sub-optimal filter. It requires then, more adaptive approaches in solving both filtering and control problems. There is another version of extended Kalman filter which could be developed at second order of Taylor approximation, this filter offers better results under high non linearity of the system's dynamic and measurement model instead of high computational cost. In the next section, a modern approach developed in 2009 based on cuba-ture rule technique is presented and discussed in detail [13].
4.1 Cubature Kalman Filter - CKF
Different estimators were introduced to solve non linear estimation problems; Sigma points Kalman filters (SPKF) introduced during the last decade. Both Unscented filters (UKF) and (CDKF) mean SPKF, in this case, the density of probability using a deterministic sigma points is estimated at the first and the second order moments of the RGV. For Central Difference Filter, it adopts an alternative method called central difference approximation. Like UKF, CDKF generates several points about the mean based on varying the covariance matrix along each dimension. It evaluates a non linear function at two different points for each dimension of the state vector that are divided by an
)
appropriate chosen interval, SPKF are strong estimators and superior alternative to the EKF in several applications with high non linearity [13]. CKF is known as the closest known approximation to the Bayesian filter for non linear estimation with Gaussian assumptions. Such as for UKF and CDKF, CKF doesn't require any Jacobian matrix computation of linearization. The basic steps are given in the next paragraphs. One can see [1] for more details. The key assumption of CKF is that the predictive density p(xt / ZH) where Zk_x denotes the history of input measurement pairs up to k-1, and the filter likelihood p{zk / Zk) are both Gaussian, which leads to a posterior Gaussian density p(xt / Zk). Under these conditions, CKF reduces the
computation of mean and covariance with more accuracy. The cubature based Gaussian filter algorithms use cubature rules of the form:
m
I (f *J fe )
(21)
i=1
to approximate the integral of the form:
J g (x)N£(x)dX = _Ljg^/2ïx + y)>-xTxdx. (22)
Eq. (22) is an integral of a non linear function multiplied by Gaussian weight. The unscented transformation can also be interpreted as an approximation of the integral of the form eq. (27). The technique introduced is based on Gaussian sum filters explored and given in detail by [13]. However it is proposed to model jamming GNSS signal by particular kind of Gaussian sum noise which is twin Gaussian sum affecting only measurement equation. Bellow the algorithm of Adaptive CKF proposed and applied in this work:
Prediction step
1. Draw cubature points , i = 1, 2, ..., 2n from the intersections of the n-dimensional unit sphere and the Cartesian axes. Scaled by Vn . We can write then:
Si =
4net,
-Jne, - n
for i =1,
, n, i = n+1, ..., 2n1. (23)
2. Propagate cubature points. The matrix square root is the lower triangular Cholesky factor:
Xi,k-Uk-1 = VPk-1/k-Ai + Щ
k-1/ k-1 •
(24)
3. Evaluate the cubature points with dynamic model function:
i,k/k-1 = f (Xi,k-1/k-1).
X
4. Estimate the predicted state mean:
Y 2n
m
k / k -
Z*
Xi,k / k-1 .
2П 1
(25)
(26)
5. Estimate the predicted error covariance:
Pxk =T~ AXi,k/k-1Xi,k/k-1 - mk/k-!mkfk-1 + Qk-1' (27)
k 2n
Update step
1. Draw cubature points , i = 1, 2, ..., 2n from the intersections of the n-dimensional unit sphere and the Cartesian axes. Scaled byVn (as in step 1).
2. Propagate the cubature points.
Xi,k/k-1 = VPk/k-1 +
m,
k / k-1 •
(28)
3. Evaluate the cubature points with the measurement model.
YtJc / k-i = h£iJc / k-i). (29)
4. Estimate the predicted measurement:
Y 2n
У-/k-1 = ^^2 Yi,k/k-1 .
(30)
i=1
5. Estimate the innovation covariance matrix:
1
2n
Sk/k-1 _ _ /k-1Yi,k/k-1 yk/k-iyk/k-1 + Rk .
(31)
6. Estimate the cross covariance matrix:
2 2n
Pх-У-/k-1 X''k/k-1Y''k/k-1 -
7. Estimate the Kalman gain:
K — P "s-1
Kk — 1 xy,k/k-1Sk/k-1 ■
8. Estimate the update state:
k / k-1 yk / k-1 .
(32)
(33)
m = m / k-1
+ Kk {yk - yk / k-1 ). (34) 9. Estimate the error covariance:
Pk/k - Pk/k-i KkSk/k-iKk . (35)
Note: Comparing with SPKF, there are no parameters to tune in CKF approximating non linear
<
functions of the system and measurement. Another alternative to approximate the lower bound for non linear state estimation with additive Gaussian noises is given and described in [15].
4.2 Non Gaussian noise mathematical model
Generally, all filtering problems in linear and/or in nonlinear dynamics are assumed to process in Gaussian environment, in such conditions, Kalman filter is the optimal solution for linear state space model and its non linear variant EKF provide a good results in major aerospace and control application. Nevertheless that in real application and in most practice, noises and especially measurement noises are not Gaussian but non Gaussian. In this case, all previous Kalman filtering algorithms will not work well. For this purpose and in the case of GNSS interferences and outliers which is a possible and recurrent problem in some navigation environments, we propose to model measurement noises as "v" impulsive non Gaussian noise. The non Gaussian noise simulated in this paper and use to derive the Gaussian sum filters is defined by the mathematical function as given below:
f (*) =
1 -e
I-""eXP 2 ' I-
л/2жг12 v 2ai )
,.2 \
exp
2 Л
2a,
2 / (36)
p(n(k)) = (l - s)n(o, a2) + sn(o, a2). (37)
The following figure describes how direct filtering approach based on Gaussian sum filters is working under interferences modeled by Gaussian sum density. The direct approach has been demonstrated as superior alternative to the indirect approach in the case of outliers and is selected to improve this accuracy by application of modern non linear filtering approaches. It is expected to get much better results than EKF and UKF used by authors in [16].
In Fig. 5, one can observe the Gaussian mixture of two Gaussian densities following the mathematical function given in Eq. (23) and Eq. (24), which provide the sum of two symmetric weighted Gaussian densities.
In green color, it is possible to observe the modification of the Gaussian curve amplitude 0.5 and 1. The twine Gaussian densities and their sum are centered on zero.
In Fig. 5, one can observe the Gaussian mixture of two non-centered Gaussian densities following the mathematical function given in Eq. (23) and Eq. (24), which provide the sum of two symmetric
weighted Gaussian densities with respective mean and covariance (5, Sigma1) and (20, Sigma2). In green color, it is possible to observe the mixture appearing in the Gaussian curve amplitude 0.5 and 0.9 . The twine Gaussian densities and their sum are non centered on zero.
-40 -20 0 20 40 60 non-gaussmf/ epsilon=0.5
Fig. 5. Pdf of non Gaussian density function of (S = 0.5) for centred noises
Gaussian (5,Sigma1) Gaussian (20,Sigma2) impulsive noise
-60 -40 -20 0 20 40 60 80 100 120 non-gaussmf/ epsilon=0.5
Fig. 6. Pdf of non Gaussian density function of (S =0.5) for non-centered noises
Note: Comparing with SPKF, there are no parameters to tune in CKF approximating non linear functions of the system and measurement. Based on similar idea such as for sub optimal fading factor, it is possible to combine Sigma Point Kalman Filters (UKF, CDKF, CKF) and adaptive fading approach. It is possible to define then the fading factor such as given bellow: One of the approaches for adaptive processing is on the incorporation of fading factors concept of adaptive fading Kalman filter (AFKF) and solved the state estimation problem. The AFKF is essentially a covariance scaling-based Kalman filter (scaling to the P matrix). The approach tries to estimate a scale factor to increase the predicted variance components of the state vector. In the AFKF, suboptimal fading factors are intro-
e
2
duced into the algorithm. The idea of fading Kalman filtering is to apply a factor matrix to the predicted covariance matrix to deliberately increase the variance of the predicted state vector:
К =
tfe-гЯк ]_f|
I 1
with parameters defined below:
Ik > 1,
^ 1,
(38)
Vk И
V0V0
pK-i + vkvk
1 + P
k > 2.
(39)
The time-varying suboptimal scaling factor is incorporated, for on-line tuning the covariance of the predicted state, which adjusts the filter gain, and accordingly the STKF is developed. The suboptimal scaling factor in the time-varying filter gain matrix is given by: Thus, the covariance matrices need to be updated based on the adaptive fading factor such as given in the following equations:
P,
S,.
-In ь
T T
y - m m.
J,i,k / k -1 J,i,k/k-1 J, k/k-1 J, k/k-
! + Qk-1,
(40)
л 2n
-K yY -1 2n y J
Y,
J ,i,k / k-1 J, i,k/k-1
y
y-T + R ,
J, k/k-1J J, k/k-1 JV
(41)
л 2n
P yx
J 'xkyk 'k-1 2n '
Y
J ,i,k/k-1 J
T ~ T
- m y.
i,k/k-1 J,k/k-1J J,k/k-1
(42)
The key parameter in this adaptive algorithm is the fading factor. It requires the defined parameters; some other techniques in literature use multiple fading factors which are not always superior to the single fading factor and are commonly selected. Then, the last estimation step of Gaussian mixture algorithm can be applied following the method and equations described in [17]. A system with nonline-arities in both the state and the measurement equa-
tions is considered. Also both noises wk, vk and the
initial state x0 will be described by weighted sums of Gaussian pdfs [18]. Consider the following nonlinear non-Gaussian system:
Robust estimation step y,(k) = N\y k -yk/k-i,Sjk/k-i)...j = 1,2. (43)
a
,(k )-
1J y J (k ) ,
2 '
УIJ У J (k )
J-1
2
Ak )-Уа J(k )m
J k,
(44)
(45)
j-1
PR (k)йУУ a J (k)PJ,k / k + (mj,k - Xi
m
J,k ЛК
xR(k))T ...J -1,2.
j-1
(46)
On Fig. 10, one can observe robust adaptive in-ertial navigation system with external aid "GNSS" based on direct information fusion and parallel non linear adaptive cubature Kalman filters.
Gaussian Mixture "EKF and CKF" are applied in order to prevent divergence due to outliers or multipath problems modeled as non Gaussian noises in one part, and also due to vibration of inertial sensors, temperature elevation, sensors noise...etc. Robust, reliable detection and removal of outliers is essential in order to process these kinds of GNSS data. Conceptually, the implementation principle of SPKF/CKF resembles that of the EKF, the implementation, however, is significantly simpler because it is not necessary to formulate the Jacobian and/or Hessian matrices of partial derivatives of the nonlinear dynamic and measurement equations, which is very important for real time implementation.
Before, implementing adaptive SPKF and CKF, we propose to observe the effect of Innovation based adaptive EKF on the navigation state during
k
T
T
x
T
Fig. 10. Robust Adaptive INS/GNSS integrated navigation system
outliers as a first experience, thus, go in more advanced signal processing for UAV data fusion during GNSS outliers which provides additional added values to previous work in this field [20-23].
5. SIMULATION OF EKF-CKF-AEKF-ACKF-GSAEKF-GSCKF-GSACKF
The conditions of simulation are described in the Table 1 and Table 2: T = 50 s, N = 5000; dt = 0.001; g = 9.81 m/s/s; Outlier duration of time ODT = 19 s, epsilon = 0.15; U = 500.
Table 1
Initial Motion coordinates and Initial Values of Covariance matrices P,Q,R:
Table 2
Estimation Covariance System Covariance Measurement Covariance
Vdiqg( P) Vdiqg( Q) Vdiq§( R)
100 10 100
100 10 100
100 10 150
10 2 2
10 2 2
10 2 5
10 Ж /180 0.5 Ж /180 1.5 % /180
10 Ж /180 0.5 Ж /180 1.5 % /180
10 Ж /180 0.5 Ж /180 1.5 % /180
From Fig. 5 till Fig. 12, one can observe the state estimation results of the non linear part described in the previous section, with velocity and attitude observation based on multiple GNSS antenna during interferences and outliers.
Yaw angle estimation
On Fig. 8, it is possible to observe during 50 s the superiority of GSACKF and GSCKF comparing with GSAEKF and GSEKF. In fact, the best estimator centered and unbiased is GSACKF. On Fig.9, one can distinguish coherent tracking between GSEKF and GSAEKF, between GSACKF and GSCKF. On Fig. 10 it is easy to classify three filtering categories: GSACKF with GCKF converge. GSEKF and GSAEKF with AEK are biased estimators. Finally, CKF, ACKF and EKF diverge from the real yaw angle values.
5
I 4
3 2 1 0 -1
Yaw vs time s
INS Real position GPS EKF CKF AEFK ACKF GMEKF GMCKF GMAEKF GMACKF
Distance N,E,D (meter) Velocity N,E,D (m/s) Attitude angles pitch, roll, yaw (rad)
1000 260 10 % /180 9
1000 70 45 % /180 8 -
1000 50 10 % /180 7 -
25 30
time s
Fig. 8. Yaw angle Estimation
Yaw vs time s
-INS
Real position • GPS
-EKF
CKF ■e—AEFK «—ACKF
-GMEKF
« GMCKF «— GMAEKF GMACKF
23 24 25 26 27 28 29 30 31 time s
Fig. 9. Yaw angle Estimation (zoom)
Yaw vs time s
-INS
Real position • GPS
-EKF
CKF ©—AEFK ACKF GMEKF GMCKF GMAEKF GMACKF
27
28
29
30 time s
31
32
Fig. 10. Yaw angle Estimation (zoom)
8
6
6 -
32
6
5
4
3
2
0
Pitch vs time s
INS
Real position GPS - EKF CKF -AEFK ACKF GMEKF GMCKF GMAEKF GMACKF
24 time s
Fig. 11. Pitch angle estimation
Pitch vs time s
21 22 23 24 25 26 27 28 29 time s
Fig. 12. Pitch angle estimation (zoom)
Pitch vs time s
INS
Real position g GPS
EKF '
CKF S
AEFK
ACKF -
GMEKF GMCKF GMAEKF ' GMACKF
Fig. 13. Pitch angle estimation (zoom)
On Fig. 11 we can observe pitch angle state estimation during 50 s. Measurements are affected by non Gaussian impulsive noise with outliers during 19 s where it is easy to distinguish instability of EKF, GSEKF and ACKF, when the opposite with
stable GSACKF, GSCKF and AEKF. On Fig. 12 it is also possible to compare GSACKF with GSCKF in one hand and GSAEKF with AEKF in the other hand. The most accurate filters are then GSACKF and GSCKF. On Fig. 13, clearly the difference in magnitude estimation of pitch angle values is between 0-1.5 deg from the true value, GSACKF, GSCKF, GSAEKF, AEKF during our analysis till 23 s.
Roll angle estimation:
Roll vs time s
INS
-Real position
• GPS
-EKF
A CKF -e—AEFK -e—ACKF
-GMEKF
GMCKF GMAEKF GMACKF
12 14 16 18 20 22 24 26 28 time s
Fig. 14. Roll angle estimation
Roll vs time s
2.5 2 1.5 1
"о
о: 0.5 0
-0.5 -1 -1.5
INS Real position GPS EKF CKF AEFK ACKF GMEKF
15 16 17 18 19 20 21 22 23 24 time s
Fig. 15. Roll angle estimation
On Fig. 14, roll angle estimation under symmetric mixture noise with outliers is computed and performed. One can divide three filters categories: CKF, ACKF divergence, EKF, AEKF, GSAEKF with inversed GSEKF tracking characteristics. Finally, GSCKF and GSACKF carried out fast convergence and high stability.
On Fig. 15, one can measure the difference between roll angle amplitude values and multiple filtering algorithms estimation, error is between 0-1.5 deg. Finally, on Fig. 16, it is interesting to observe coherence between EKF, AEKF and
22
23
time s
GSAEKF, in addition to the divergence of CKF and ACKF.
Roll vs time s
-INS
Real position • GPS
-EKF
CKF ■e—AEFK ■e—ACKF
-GMEKF
О GMCKF GMAEKF GMACKF
17 17.5 18 18.5 19 19.5 20 20.5 21 21.5 22 time s
Fig. 16. Roll angle estimation
Velocity estimation
On Fig. 17, vertical velocity estimation is observed, with two distinguished filter classes: EKF, CKF, AEKF and ACKF are instable and cannot track the real velocity. The second class includes GSEKF, GSAEKF, GSCKF and GSACKF. The best estimator is: GSACKF. This is confirmed by different state estimation with different non linearity such as velocity and attitude states. After multiple simulations, it is clear through state and related MSE estimation that Innovation based Adaptive Gaussian sum CKF called GSACKF provide an ameliorated filtering accuracy comparing with CKF, ACKF, GSCKF, and the conventional EKF, AEKF, in addition to GSAEKF.
Down Velocity vs time s
0|"' '
-INS
Real position • GPS
-EKF
CKF -S— AEFK -e— ACKF GMEKF « GMCKF
GMAEKF О GMACKF
4 6 8 10 12 14 16 18 time s
Fig. 17. Vertical velocity estimation
5.1 Gaussian sum based adaptive extended-Cubature Kalman Filters Simulation
The simulation conditions of the previous section are considered in this experience to estimate
first, attitude estimation with yaw, pitch and roll angles estimation. Then, velocity through 03 axes of navigation (north, east, down) are observed and discussed, with finally, the linear part in state estimation not presented in this paper with position state vector estimation which follow linear discrete time model.
East Velocity vs time s
INS
Real position • GPS
-EKF
CKF -e—AEFK -e—ACKF
-GMEKF
ö GMCKF GMAEKF GMACKF
8 10 12 14 16 18 20 time s
Fig. 18. East velocity estimation
NORTH Velocity vs time s
1800
1600
1400
1200
■g 1000 .о
ш
> 800
.с ■g
z 600 400 200 0
INS
-Real position
• GPS
-EKF
CKF -в—AEFK -e—ACKF
-GMEKF
GMCKF GMAEKF GMACKF
12 14 16 time s
Fig. 19. North velocity estimation
5.2 Observation of adaptive non linear state estimation for IMU/GNSS data fusion during outliers
During simulations, the following algorithms are compared and applied ton MEMS IMU/GNSS data fusion during 50 seconds. In this interval, non Gaussian measurement noise affects GNSS receivers with twine outliers scenario simulated and activated to perturb conventional algorithms with their Gaussian sum modifications between 7 s and 17 s. It is then possible to divide the observation in two principal parts: attitude estimation and velocity estimation. Both are non linear but present differences in non linearity degrees.
2
1.5
200
0.5
000
0
800
0.5
600
400
200
0
46
400
200
-200
-400
-600
-800 -
20 22
For roll attitude estimation, during 50 s, an important results consists on divergence of CKF instead of EKF convergence. EKF outperforms CKF during severe conditions which are an interesting drawback of CKF, presented in the specialized literature as the most accurate filter compared with EKF and SPKF variants.
For velocity estimation, during 50 s and especially between 7 s and 17 s, CKF outperforms EKF and their adaptive forms AEKF and ACKF present comparative results during velocity outliers. Again, for attitude estimation, AEKF ensure robust estimation compared with ACKF, where AEKF still stable, ACKF diverges. It is again an important drawback for Cubature Kalman Filter CKF. Finally Gaussian sum based CKF and ACKF are the best estimators with better tracking capability and accuracy during 50 s for both attitude and velocity estimation. It is interesting to observe that GSCKF performs GSACKF which is nevertheless the best filters with the best estimation accuracy in all cases.
CONCLUSIONS
The design of non linear Gaussian sum based adaptive filters and the associated data fusion based on EKF, and CKF were deeply studied for MEMS IMU/GNSS sensor fusion during GNSS outliers in attitude and velocity estimation. Based on the innovation fading factor, 04 non linear filtering approaches EKF, AEKF, CKF and ACKF were modified using this fading factor for covariance estimation during GPS/GLONASS outages. During this interval, measurement noises are assumed non Gaussian and impulsive, which has decreased adaptive EKF and adaptive CKF accuracy, only GSCKF and GSACKF provide good estimation with higher accuracy. For attitude and velocity estimation, GSACKF outperforms all filters GSAEKF, GSEKF, AEKF, ACKF, CKF and EKF. This represents an interesting approach to solve combined unconventional noises environment with non Gaussian and outliers ate ones. By the way, several solutions were proposed and ameliorated the accuracy with time of convergence of each modified filtering algorithm. However according the RMSE criteria, Gaussian sum based Cubature Kalman Filter CKF during outliers demonstrated the best robustness and estimation accuracy. Gaussian sum based adaptive CKF algorithm is expected to be tested on real REVO board embedded on UAV during real flight for long duration tests and validation. It should be compared to most performing filters such as particle filters and modern approaches such as Quadrature Kalman Filter QKF in addition to other robust techniques such as demonstrated by
authors in other tracking application based on Huber estimation [19].
REFERENCES
1. Rudolph van der Merwe and Eric A. Wan, "Sigma-Point Kalman Filters for Nonlinear Estimation and SensorFusion - Applications to Integrated Navigation," GNC American Institute of Aeronautics and Astronautics, 2004.
2. D. Li, J. Wang, and S. Babu, "Enhancing the Performance of Ultra-Tight Integration of GPS/PL/INS: A Federated Filter Approach," J. Global Positioning Systems, vol. 5, no. 1-2, pp. 96-104, 2006.
3. H. Benzerrouk and A. V. Nebylov, "Robust Nonlinear Filtering Applied to Integrated Navigation System under non Gaussian measurement noise Effect," in Proceeding of IEEE AEROSPACE Conference 2012, Big Sky Montana.
4. Gerasimos G. Rigatos, "Nonlinear Kalman Filters and Particle Filters for integrated navigation of unmanned aerial vehicles," Robotics and Autonomous Systems, vol. 60, issue 7, pp. 978-995, July 2012.
5. A R. Runnalls, "A Kullback-Leibler Approach to Gaussian Mixture Reduction," IEEE Transaction on Aerospace and Electronic System, 2006.
6. H. Benzerrouk, "Gaussian vs. Non-Gaussian noise in inertial/GNSS integration," GNSS Solutions, Inside GNSS Magazine, pp. 32-39, november/december 2012.
7. I. Arasaratnam and S. Haykin, "Cubature Kalman Filtering," IEEE Trans. Automatic Control, vol. 54, no. 6, pp. 12541269, June 2009.
8. Revo autopilot board, http://www.openpilot.org/ products/openpilot-revolution-platform/15th october2013
9. Francisco Martín, Vicente Matellán, Pablo Barrera, José M. Cañas, "Localization of legged robots combining a fuzzy-Markov method and a population of extended Kalman filters," Robotics and Autonomous Systems, vol. 55, Issue 12, pp. 870-880, 31 December 2007.
10. P. Closas and C. Fernandez-Prades, "Bayesian Nonlinear Filters for Direct Position Estimation," in Proc. IEEE Aerospace conference, Big Sky, MT (USA), March 2010.
11. Simo Ali-Loytty and Niilo Sirola, "Gaussian Mixture Filter in Hybrid Navigation," in Proc. The European Navigation Conf. GNSS 2007, pp. 831-837.
12. Simo Ali-Loytty, Niilo Sirola & Robert Piché, "Consistency of Three Kalman Filter Extensions in Hybrid Navigation," in Proc. European Navigation Conf. GNSS 2005, July 1922, 2005, Munchen.
13. Ienkaran Arasaratnam, "Sensor Fusion with Square-Root Cubature Information Filtering" pp. 11-17, Pub. Date: February 6, 2013, DOI: 10.4236 /ica.2013.41002.
14. P. Closas and C. Fern'andez-Prades, "The Marginalized Square Root Quadrature Kalman Filter," in Signal Processing Advanced in Wireless Communications (SPAWC), 2010 IEEE 11th Int. Workshop.
15. P. Tchikavsky, C. H. Muravchik, and A. Nehorai, "Posterior Cramer Rao Bounds for Discrete Time Nonlinear Filtering," IEEE Trans. Signal Processing, vol. 46, no. 5, 1998.
16. Leong, P. H.; Coll. of Eng. & Comput. Sci., Australian Nat. Univ., Canberra, ACT, Australia; Arulampalam, S.; Lamahewa, T. A.; Abhayapala, T. D., "A Gaussian-Sum Based Cubature Kalman Filter for Bearings-Only Tracking," 2013, vol. 49, issue 2. 10.1109/TAES.2013.6494405.
17. Mounir Djeddi, Messaoud Benidir, "A Robust Estimator for Polynomial Phase Signals in Non Gaussian Noise Using Parallel Unscented Kalman Filters," 14th European Signal Pro-
cessing Conference (EUSIPCO 2006), Florence, Italy, September 4-8, 2006.
18. Miroslav Simandl, Lecture notes on state estimation of nonlinear non-Gaussian stochastic systems. Pilsen: Department of Cybernetics, Faculty of Applied Sciences University of West Bohemia in Pilsen, 2006.
19. Christopher D. Karlgaard and Hanspeter Schaub. "Huber-Based Divided Difference Filtering," Journal of Guidance, Control, and Dynamics, vol. 30, no. 3 (2007), pp. 885891. doi: 10.2514/1.27968.
20. Shesheng Gao, Yongmin Zhong, Xueyuan Zhang, Bijan Shirinzadeh, "Multi-sensor optimal data fusion for INS/GPS/SAR integrated navigation system," Aerospace Science and Technology, vol. 13, Issues 4-5, pp. 232-237, June-July 2009.
21. Shu Ting Goh, Ossama Abdelkhalik, Seyed A. (Reza) Zekavat, "A Weighted Measurement Fusion Kalman Filter implementation for UAV navigation," Aerospace Science and Technology, vol. 28, Issue 1, pp. 315-323, July 2013.
22. Xiaolin Ning, Jiancheng Fang, "An autonomous celestial navigation method for LEO satellite based on unscented Kalman filter and information fusion," Aerospace Science and Technology, vol. 11, Issues 2-3, pp. 222-228, March-April 2007.
23. Xiaogang Wang, Naigang Cui, Jifeng Guo, "INS/VisNav/GPS relative navigation system for UAV," Aerospace Science and Technology, vol. 28, Issue 1, pp. 242-248, July 2013.
ABOUT AUTHORS
BENZERROUK, Hamza, Postgrad. (PhD) Student, Dept. Aerospace Instrumentation and Complex Information Computing. He completed an aeronautic engineering degree from Saad Dahlab University of Blida, Algeria, a "Magister" in advanced signal processing from the Polytechnic School of Algeria (ex-ENITA), and is now preparing his defense of a Ph.D. degree in aerospace instrumentation at the International Institute for Advanced Aerospace Technologies of Saint Petersburg State University of Aerospace Instrumentation, Russia. NEBYLOV, Alexander, Prof. at the State University of Aerospace Instrumentation in St. Petersburg as Professor and Chairman of Aerospace Devices and Measuring Complexes, and director of the International Institute for Advanced Aerospace Technologies. He graduated with honors as an Engineer in Missile Guidance from the Leningrad Institute of Aircraft Instrumentation in 1971. He led many R&D projects in aerospace instrumentation, motion control systems and avionics, and is a scientific consultant for various Russian design bureaus and research institutes. He was awarded the Candidate of Science degree in 1974 and the Doctor of Science degree in 1985, both in Information Processing and Control Systems, from the State Academy of Aerospace Instrumentation.
SALHI, Hassen, Prof., Dept. of Control systems, was born in 1953 in Boufarik (AG). He obtained his B.S., M.S. and Ph.D. in Electrical Engineering from the University of Illinois at Urbana-Champaign in 1977, 1979 and 1983 respectively. He is currently an Associate Professor in Electrical Engineering at the University Saad Dahlab of Blida (AG), where he is the Head of the Research Laboratory SET in Control Engineering. His interests includes Control and Modeling of Biological Systems, Fractional Order Systems and Robotics.
МЕТАДАННЫЕ
Название: Применение в интегрированной навигационной системе адаптивного фильтра Калмана на базе Гауссовых сумм.
Авторы: Х. Бензеррук, А. Небылов, Х. Салхи. Организации:
Санкт-Петербургский государственный университет
эрокосмического приборостроения (ГУАП), Россия. Университет Блиды им. Саада Далаба, Алжир. Email: hb.iiaat@gmail.com, nebylov@aanet.ru,
hassensalhi@yahoo.fr Язык: анлийский.
Источник: Вестник УГАТУ. 2014. Т. 18, № 3 (64). С. 3-15,
ISSN 2225-2789 (Online), ISSN 1992-6502 (Print). Аннотация: В статье рассматривается задача нелинейной оценивания пространственных состояний подвижного объекта.
Кючевые слова: IMU; MEMS; GPS; GNSS; фильтрация Калмана; CKF; сумма Гаусса. Об авторах:
БЕНЗЕРРУК Хамза, аспирант каф. аэрокосмических изм.-выч. комплексов. Дипл. инж. в обл аэронавтики (Saad Dahlab university of Blida, Алжир), м-р в обл. обработки сигналов (Polytechnic School of Algeria) . Готовит дисс. в обл. аэрокосмического приборостроения.
НЕБЫЛОВ Александр Владимирович, проф., директор междунар. ин-та передовых аэрокосм. технологий. Дипл. инж. (ЛИАП, 1971). Д-р техн. наук (ЛИАП, 1985. Акад. РАЕН (1994). Засл. деят. науки РФ (2006). Иссл. в обл. оптимизации систем измерения, контроля и управления движением.
САХЛИ Хассен, проф. каф. систем управления. PhD в обл. эл.-инж. (Ун-т Иллинойса, США, 1983). Иссл. в обл. управления и моделирования в биологич., фрактальн., робототехнич. системах.
