All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.

Share

Description

Bayesian Bootstrap Filter Approach for GPS/INS integration
Khalid TOUIL1 , Abderrahim GHADI2 FSTT-Abdelmalek Essaˆ di University, Morocco, khalid.touil@gmail.com a 2 FSTT-Abdelmalek Essaˆ di University, Morocco, ghadi05@gmail.com a Abstract
Inertial Navigation System (INS) and Global Positioning System (GPS) technologies have been widely used in a variety of positioning and navigation applications. Because the GPS and the INS complement each other, it is common practice to integrate the two syst

Tags

Transcript

Bayesian Bootstrap Filter Approach for GPS/INS integration
Khalid TOUIL
1
, Abderrahim GHADI
21
FSTT-Abdelmalek Essaˆadi University, Morocco, khalid.touil@gmail.com
2
FSTT-Abdelmalek Essaˆadi University, Morocco, ghadi05@gmail.com
Abstract
Inertial Navigation System (INS) and Global PositioningSystem (GPS) technologies have been widely used in a va-riety of positioning and navigation applications. Becausethe GPS and the INS complement each other, it is com-mon practice to integrate the two systems. The advantagesof GPS/INS integration is that the integrated solution can provide continuous navigation capability even during GPS outages. It is well known that Kalman ﬁltering is an opti-mal real-time data fusion method for GPS/INS integration. However, it has some limitations in terms of stability, adapt-ability and observability. To solve this problem, we proposeto use the Bayesian Bootstrap Filtering (BBF) for GPS/INS integration. Bootstrap ﬁlter is a ﬁltering method based on Bayesian state estimation and Monte Carlo method, whichhas the great advantage of being able to handle any func-tional non-linearity and system and/or measurement noiseof any distribution. Experimental result demonstrates that the bootstrap ﬁlter gives better positions estimate than thestandard Extended Kalman ﬁlter (EKF).
Key words: GPS/INS integration, data fusion, BayesianBootstrap Filtering, Extended Kalman ﬁlter.
1. Introduction
The global positioning system (GPS) has been exten-sively used in navigation because of its accuracy and world-wide coverage [1, 2]. It is well established that GPS canprovide accurate position, velocity and timing informationunder good satellite signal tracking environments. The lim-itations of GPS are related to the loss of accuracy in thenarrow-street environment, poor geometrical dilution onprecision (GDOP) coefﬁcient and the multipath reﬂections.So it is suitable to integrate the GPS with a different typeof navigation system in order to reach a greater autonomy.Fromthispointofview, theinertialnavigationsystem(INS)is ideal [3, 4]. INS can provide continuous position, veloc-ity, and also orientation estimates, which are accurate for ashort term, but are subject to drift due to sensor drifts. Theintegration of GPS and INS can overcome the shortcom-ings of the individual systems, namely, the typically lowrate of GPS measurements as well as the long term driftcharacteristics of INS. Integration can also exploit advan-tages of two systems, such as the uniform high accuracytrajectory information of GPS and the short term stabilityof INS. There have been a number of recent approaches toimproving the performance of GPS/INS integration [5, 6,7]. The Kalman ﬁltering is an optimal real-time data fu-sion method for GPS/INS integration [8, 9], it has somelimitations in terms of stability, adaptability and observabil-ity, etc. Different integration ﬁlters have also been investi-gated for example, the Extended Kalman Filter (EKF) [10,11], the Quadratic Extended Kalman Filter (QEKF) [12],the Unscented Kalman Filter (UKF) [13]. The EKF is thetraditional method for GPS/INS integration. Note that theaccuracy of integrated navigation solution is directly relatedto the adequacy of the linearized error models. But for lowquality inertial devices, the EKF may be unstable or evendivergent [14] due to the large linearized errors. It is there-fore highly desirable to have a solution that retains someof these nonlinearities. In this paper, we propose to use theBayesianbootstrapﬁltering(BBF)[15, 16, 17]forGPS/INSintegration.
Khalid TOUIL et al., International Journal of Networks and Systems, 1(1), August - September 2012, 18-25The BBF is used in identiﬁcation of hysteretic systemswith slip [18], target tracking [19, 20] satellite attitudedetermination [21, 22, 23]. Bootstrap ﬁlter is a ﬁlter-ing method based on Bayesian state estimation and MonteCarlo method, which has the great advantage of being ableto handle any functional non-linearity and system and/ormeasurement noise of any distribution. The BBF is inde-pendent on the initial state and can avoid the divergenceproblem, but its drawback is the heavy computational loadin the update stage. The idea of this ﬁltering method is torepresent the required probability density function (PDF)as a set of random samples, rather than as a function overstate space. As the number of samples becomes large, theycan effectively provide an accurate representation of the re-quired PDF. Estimations of states can then be obtained di-rectly from the samples. The paper is organized as fol-lows. Section2presentsamathematicalmodelofintegratedGPS/INS system. In the third section, a Bayesian bootstrapﬁltering algorithm is described. Experimental results arepresented to demonstrate the accuracy of the proposed algo-rithm in section 4. Finally, conclusions are made in section5.
2. Mathematical model of integrated GPS/INSsystem
InanINS,sensedaccelerationsandangularratesfromanInertial Measurement Unit (IMU) are transformed into po-sition, velocity and attitude by solving a set of differentialequations. GPS on the other hand delivers positions withrelatively low accuracy but with a bounded error. INS andGPS have complementary properties and are therefore wellsuited for integration. There are different modes of inte-gration [24, 25]. In this work, a tightly coupled GPS/INSsystem has been implemented [26]. This means primarilythat GPS pseudo-ranges are used as inputs to the navigationﬁlter rather than computed GPS positions.
2.1. State model
The state vector is composed of the INS error that is de-ﬁned as the deviation between the actual dynamic quanti-ties and the INS computed values:
&
X
=
X
−
X
INS
.The state model describes the INS error dynamic behaviordepending on the instrumentation and initialization errors.It is obtained by linearizing the ideal equations around theINS estimates as follow:
δX
=
f
(
X,U
)
−
f
(
X
INS
,U
INS
)
(1)
δX
=
�
f
(
X
INS
,U
INS
)
δX
(2)The state vector is usually augmented with systematicsensor errors:
δX
= (
δr,δν
n
,δρ,b
a
,b
g
,b,d
)
(3)whereallthevariablesareexpressedinthenavigationframeNED (North, East, Down).
ã
δr
= (
δφ,δλ,δh
)
is the geodetic position error in lati-tude, longitude, altitude,
ã
δν
N
= (
δν
E
,δν
D
)
is the velocity error vector,
ã
δρ
is the attitude (roll, pitch, yaw) error vector,
ã
b
a
and
b
g
represent the accelerometers and gyroscopesbiases,
ã
b
=
cτ
r
and d are respectively the GPS clock offsetand its drift.
τ
r
is the receiver clock offset from theGPS time and c is the speed of light
3
x
10
8
m/s
)
.For short-term applications, the accelerometers and gy-roscopes can be properly deﬁned as random walk constants
b
a
=
ω
a
and
b
g
=
ω
g
. Note that the standard deviations of the white noises
ω
a
and
ω
g
are related to the sensor quality.The navigation solution also depends on the receiver clock parameters b and d models as
b
=
d
+
ω
b
and
d
=
ω
d
, where
ω
b
and
ω
d
are mutually independent zero-mean Gaussianrandom variables [27]. For simplicity, denote as
X
(insteadof
δX
) the state vector. The discrete-time state model takesthe following form:
X
INS,k
+1
=
A
k
X
INS,k
+
ν
k
(4)where
ν
k
denote the dynamical zero-mean Gaussiannoise vector with associated covariance matrix
Σ
ν
. Thecoupling effects between the components of
X
INS,k
resultsin a block diagonal matrix
A
k
whose elements are detailedin many standard textbooks such as [27, chap. 6].
2.2 Measurement equation
The standard measurement of the GPS system is thepseudo-range. This deﬁnes the approximate range from theuser GPS receiver antenna to a particular satellite. Conse-quently, the observation equation associated to the
i
th
satel-lite can be deﬁned as:
ρ
i
=
�
(
X
i
−
x
)
2
+ (
Y
i
−
y
)
2
+ (
Z
i
−
z
)
2
+
b
+
ω
i
(5)where,
i
= 1
,
···
,n
s
(recall that
n
s
is the number of visible satellites).The vectors
(
x,y,z
)
T
and
(
X
i
,Y
i
,Z
i
)
T
are respectivelythe positions of the vehicle and the ith satellite expressed in@ 2012, IJNS All Rights Reserved 19
Khalid TOUIL et al., International Journal of Networks and Systems, 1(1), August - September 2012, 18-25the rectangular coordinate system WGS-84 [27, chap. 4].In addition to the measurement of the pseudo-distance, wehave also the measurement of delta-ranges. This latter char-acterizes the Doppler measurement (speed of distancing orapproach of the visible satellites according to the respectiveaxes under which they are seen since the receiver). For the
i
th
satellite, we model the observation of the delta-rangesby the following way:
Δ
ρ
i
=
R
i
+
b
+
λ
i
(6)where,
R
i
=(
X
i
−
x
)(˙
X
−
˙
x
) + (
Y
i
−
y
)(˙
Y
−
˙
y
) + (
Z
i
−
z
)(˙
Z
−
˙
z
)
�
(
X
i
−
x
)
2
+ (
Y
i
−
y
)
2
+ (
Z
i
−
z
)
2
(7)The vectors and are respectively the speeds of the vehicleand the ith satellite expressed in the rectangular coordinatesystem WGS-84. The position and the speed of the vehicleare transformed from the geodetic coordinate to the rectan-gular coordinate system as follows:The vectors
(˙
x,
˙
y,
˙
z
)
T
and
(˙
X
i
,
˙
Y
i
,
˙
Z
i
)
T
are respectivelythespeedsofthevehicleandtheithsatelliteexpressedintherectangular coordinate system WGS-84. The position andthe speed of the vehicle are transformed from the geodeticcoordinate to the rectangular coordinate system as follows:
x
= (
N
+
h
INS
+
δh
)
cos
(
λ
INS
+
δλ
)
cos
(
φ
INS
+
δφ
)
y
= (
N
+
h
INS
+
δh
)
cos
(
λ
INS
+
δλ
)
sin
(
φ
INS
+
δφ
)
z
= (
N
(1
−
e
2
) +
h
INS
+
δh
)
sin
(
φ
INS
+
δφ
)
(8)
˙
x
=
−
(
N
+
h
INS
+
δh
)(˙
φ
INS
+
δ
˙
φ
)
sin
(
φ
INS
+
δφ
)
cos
(
λ
INS
+
δλ
)
−
(
N
+
h
INS
+
δh
)(˙
λ
INS
+
δ
˙
λ
)
cos
(
φ
INS
+
δφ
)
sin
(
λ
INS
+
δλ
)+(˙
h
INS
+
δ
˙
h
)
cos
(
φ
INS
+
δφ
)
cos
(
λ
INS
+
δλ
)˙
y
=
−
(
N
+
h
INS
+
δh
)(˙
φ
INS
+
δ
˙
φ
)
sin
(
φ
INS
+
δφ
)
sin
(
λ
INS
+
δλ
)+(
N
+
h
INS
+
δh
)(˙
λ
INS
+
δ
˙
λ
)
cos
(
φ
INS
+
δφ
)
cos
(
λ
INS
+
δλ
)+(˙
h
INS
+
δ
˙
h
)
cos
(
φ
INS
+
δφ
)
sin
(
λ
INS
+
δλ
)˙
z
= (
N
(1
−
e
2
) +
h
INS
+
δh
)(˙
φ
INS
+
δ
˙
φ
)
cos
(
φ
INS
+
δφ
)+(˙
h
INS
+
δ
˙
h
)
sin
(
φ
INS
+
δ
φ
)
(9)
where,
δν
N
= (
N
(1
−
e
2
) +
h
INS
+
δh
)
δ
˙
φ
+˙
φ
INF
δhδν
E
= (
N
+
h
INS
+
δh
)
sin
(
φ
INS
+
δφ
)
δφ
˙
λ
INF
+(
N
+
h
INS
)
cos
(
φ
INS
+
δφ
)
δ
˙
λ
+
cos
(
φ
INS
+
δφ
)
δh
˙
λ
INS
δν
D
=
−
δ
˙
h
(10)
and
N
=
a/
√
1
−
e
2
sin
2
λ
. The parameters
a
and
e
denote the semi-major axis length and the eccentricity of the earths ellipsoid. These expressions (Eq. 8, Eq. 9 andEq. 10) have to be substituted in (5) and (6) to obtain thehighly nonlinear measurement equation:
Y
GPS,k
=
g
(
X
INS,k
) +
β
k
(11)
where
β
k
˜
and
Y
GPS,k
= (
ρ
)
1
,...,ρ
)
n
,
Δ
ρ
)
1
,...,
Δ
ρ
)
n
)
is the pseudo-ranges and delta-ranges vector. In theBayesian bootstrap ﬁlter, it is not necessary that the mea-surement noise
β
k
must be the white Gaussian. Now wecan apply the bootstrap ﬁlter using the above state and mea-surement models.
3 Bayesian bootstrap ﬁlter
Bootstrap ﬁlter is relatively new concept in ﬁltering. Theadvantage of bootstrap ﬁlter over Kalman and EKF is thatthe system does not have to be linear and the noise in thesystem can be non-Gaussian [
15
]. The Bayesian bootstrapﬁltering approach is to construct the conditional probabilitydensity function (PDF) of the state based on measurementinformation [
15
,
16
,
17
]. The conditional PDF can be re-garded as the solution to the estimation problem. We shallbrieﬂy explain the recursive Bayesian estimation theory andthe Bayesian Bootstrap ﬁlter.
3.1 Recursive Bayesian Estimation
The discrete-time stochastic dynamical system modelcan be described by the stochastic vector difference equa-tion:
x
k
+1
=
f
(
x
k
−
1
,w
k
)
where
f
:
R
m
×
R
m
→
R
n
is the system transition functionand
w
k
∈
R
m
is a zero-mean noise process independent of the system states. The PDF of
w
k
is assumed to be knownas
p
w
(
w
k
)
. At discrete time, measurements are denoted by
y
k
∈
R
m
, which are related to the state vector via the ob-servation equation:
y
k
=
h
(
x
k
,ν
k
)
(12)
where
h
:
R
n
×
R
r
→
R
p
is the measurement function,and
ν
k
∈
R
r
is the observation noise, assumed to be an-other zero mean random sequence independent of both statevariable
x
k
and the system noise
w
k
.The PDF of
ν
k
is assumed to be known as
p
v
(
ν
k
)
. Theset of measurements from initial time step to step
k
is ex-pressedas
Y
k
=
{
y
i
}
ki
=1
= 1
. Thedistribution oftheinitialcondition
x
0
is assumed to be given by
p
(
x
0
/Y
0
) =
p
(
x
0
)
.The recursive Bayesian ﬁlter based on the Bayes rule canbeorganizedinto thetimeupdatestateand themeasurementupdate stage. The time update state can be constructed as:
p
(
x
k
/Y
k
−
1
) =
�
p
(
x
k
/x
k
−
1
)
×
p
(
x
k
−
1
/Y
k
−
1
)
dx
k
−
1
(13)
where
p
(
x
k
/Y
k
−
1
)
is determined by
f
(
x
k
−
1
,w
k
−
1
)
andthe known PDF
p
w
(
w
k
−
1
)
.Then at time step
k
, a measure-ment
y
k
becomes available and may be used to update theprior according to the Bayes’rule:
p
(
x
k
/Y
k
−
1
) =
p
(
y
k
/x
k
)
×
p
(
x
k
/Y
k
−
1
)
p
(
y
k
/x
k
)
×
p
(
x
k
/Y
k
−
1
)
(14)
where the conditional PDF
p
(
y
k
/x
k
)
is determined fromthe measurement model and the known PDF,
p
ν
(
ν
k
)
. The@ 2012, IJNS All Rights Reserved 20
Khalid TOUIL et al., International Journal of Networks and Systems, 1(1), August - September 2012, 18-25embodiment of the recursive Bayesian ﬁlter is very hard andlaborious work because closed form solutions of the equa-tions (
14
) and (
15
) do not exist, or are difﬁcult to ﬁnd, there-fore should be implemented numerically with high compu-tational load.
3.2 Bayesian Bootsrap ﬁlter
FromtherecursiveBayesianﬁltertheBBFalgorithmcanbe described as follow. The BBF is an algorithm for prop-agating and updating the random samples from the pdf of the state. Therefore, the BBF may be considered as an ap-proximation to be the recursive Bayesian ﬁlter. Suppose wehave a set of random samples
x
k
−
1
(
i
) :
i
= 1
,...,N
PDF
p
(
x
−
k
−
1
/Y
k
−
1
)
. Here,
N
is the number of bootstrapsamples.The ﬁlter procedure is as follows:1. Prediction: Each sample from PDF
p
(
x
k
−
1
/Y
k
−
1
)
ispassed through the system model to obtain samplesfrom the prior at time step
k
:
x
k
(
i
) =
f
(
x
k
−
1
(
i
)
,w
k
(
i
))
,
(15)
where
w
k
(
i
)
is a sample draw from PDF of the systemnoise
p
w
(
w
k
)
.2. Update: On receipt of the measurement
y
k
, evaluatethe likelihood of each prior sample and obtain the nor-malized weight for each sample:
q
i
=
p
(
y
k
/x
∗
k
(
i
))
N
j
=1
p
(
y
k
/x
∗
k
(
j
))
(16)
This deﬁne a discrete distribution over
x
k
(
i
) :
i
=1
...,N
, with probability mass
q
i
associated with ele-ment
i
. Now resample
N
times from the discrete dis-tribution to generate samples
{
x
k
(
i
) :
i
= 1
,...,N
}
,so that for any
j
,
Probx
k
(
j
) =
x
k
(
i
) =
q
i
. Theabove steps of prediction and update form a single it-eration of the recursive algorithm. To initiate the al-gorithm,
N
samples
x
1
(
i
)
are drawn from the knowninitial PDF
p
(
x
1
/Y
0
) =
p
(
x
1
)
. These samples feeddirectly into the update stage of the ﬁlter. We contendthat the samples
x
k
(
i
)
are approximately distributedas the required PDF
p
(
x
k
/Y
k
)
. Repeat this procedureuntil the desired number of time samples has been pro-cessed. The basic algorithm is very simple and easy toprogram. The re-sampling update stage is performedby drawing a random sample
u
i
from the uniform dis-tribution over
(0
,
1]
. The value
x
∗
k
(
M
)
correspondingto:
M
−
1
j
=0
q
j
< u
i
≤
M
j
=0
q
j
(17)
where
q
0
= 0
, is selected as a sample for the posterior.This procedure is repeated for
i
= 1
,...,N
. It wouldalso be straightforward to implement this algorithm onmassively parallel computers, raising the possibility of real time operation with very large sample sets. Thenumber
N
dependsonthedimensionofthestatespace,thetypicaloverlapbetweenthepriorandthelikelihood
p
(
y
k
/x
k
)
and the required number of time steps [
15
].
4 Simulation results
The analysis of some simulations will enable us to eval-uate the performance of the utilization of the BBF. Here, wepresent two examples which illustrate the operation of thebootstrap ﬁlter. Estimation performance is compared withthe standard EKF. The ﬁrst example is an univariate nonsta-tionary growth model taken from references [
28
,
29
]. Thesecond is a navigation problem using GPS and INS systems.
4.1 Example1
Consider the following nonlinear model [
28
]:
x
k
= 0
.
5
x
k
−
1
+ 25
x
k
−
1
/
(1 +
x
2
k
−
1
) + 8
cos
(1
.
2(
k
−
1)) +
w
k
(18)
y
k
=
x
2
k
/
20 +
ν
k
(19)
where
w
k
and
ν
k
are zero-mean Gaussian white noise withvariances
10
and
1
respectively. This example is severelynonlinear, both in the system and the measurement equa-tion. Note that the form of the likelihood
p
(
y
k
/x
k
)
adds aninteresting twist to the problem. For measurement
y
k
<
0
the likelihood is unimodal at zero and symmetric aboutzero. However, for positive measurements the likelihoodis symmetric about zero with modes
at
±
(20
y
k
)1
/
2
. Theinitial state was taken to be
x
0
= 0
.
1
and Fig.
1
shows a
50
step realization of Eq.
19
. The EKF and bootstrap ﬁlterswere both initialized with the prior PDF
p
(
x
0
) =
N
(0
,
2)
.Fig.
2
shows the result of applying the EKF to
50
measure-mentsgeneratedaccordingtoEq.
20
. Thetruestateisrepre-sented by a solid line, EKF mean is given as a dashed lines.Fig.
3
shows the result of directly applying the bootstrap al-gorithm of section
3
.
2
with a sample saze of
N
= 500
. Thedashed line gives the bootstrap estimate of posterior mean.Fig.
4
shows, the posterior can be bimodal in this exam-ple. At a couple of time steps, the actual state is just outsidethese percentile estimates, and quite often it is close to oneof the limits. However, most of the time the actual stateis very close to the posterior mean and performance is ob-viously greatly superior to the EKF. Running the bootstrapﬁlter with larger sample sets gave results indistinguishablefrom Fig.
3
, and this is taken as conﬁrmation that our sam-ple set size is sufﬁcient. The relatively high system noiseprobably accounts for the reasonable performance of thebasic algorithm using only
500
samples: the system noise@ 2012, IJNS All Rights Reserved 21

We Need Your Support

Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks