Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models

12 Pages·2008·0.52 MB·English

Linköping University Postprint

Marginalized Particle Filters for Mixed
Linear/Nonlinear State-Space Models

Thomas Schön, Fredrik Gustafsson and Per-Johan Nordlund

N.B.: When citing this work, cite the original article.

Original publication:

Thomas Schön, Fredrik Gustafsson and Per-Johan Nordlund, Marginalized Particle Filters
for Mixed Linear/Nonlinear State-Space Models, 2005, IEEE Transactions on Signal
Processing, (53), 7, 2279-2289.
http://dx.doi.org/10.1109/TSP.2005.849151.
Copyright: IEEE, http://www.ieee.org

Postprint available free at:
Linköping University E-Press: http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-11749
IEEETRANSACTIONSONSIGNALPROCESSING,VOL.53,NO.7,JULY2005 2279
Marginalized Particle Filters for Mixed
Linear/Nonlinear State-Space Models
ThomasSchön,FredrikGustafsson,Member,IEEE,andPer-JohanNordlund
Abstract—The particle filter offers a general numerical tool andthefollowingtimerecursion:
to approximate the posterior density function for the state in
nonlinearandnon-Gaussianfilteringproblems.Whiletheparticle
filter is fairly easy to implement and tune, its main drawback is (2c)
that it is quite computer intensive, with the computational com-
plexity increasing quickly with the state dimension. One remedy
tothisproblemistomarginalizeoutthestatesappearinglinearly initiated by [20]. For linear Gaussian
inthedynamics.TheresultisthatoneKalmanfilterisassociated models, the integrals can be solved analytically with a finite
with each particle. The main contribution in this paper is the dimensional representation. This leads to the Kalman filter
derivation of the details for the marginalized particle filter for a
recursions, where the mean and the covariance matrix of the
general nonlinear state-space model. Several important special
statearepropagated[1].Moregenerally,nofinitedimensional
casesoccurringintypicalsignalprocessingapplicationswillalso
be discussed. The marginalized particle filter is applied to an representation of the posterior density exists. Thus, several
integratednavigationsystemforaircraft.Itisdemonstratedthat numerical approximations of the integrals (2) have been pro-
thecompletehigh-dimensionalsystemcanbebasedonaparticle posed. A recent important contribution is to use simulation
filter using marginalization for all but three states. Excellent
based methods from mathematical statistics, sequential Monte
performanceonrealflightdataisreported.
Carlo methods, commonly referred to as particle filters [11],
Index Terms—Kalman filter, marginalization, navigation sys- [12],[16].
tems,nonlinearsystems,particlefilter,stateestimation.
Integrated navigation is used as a motivation and applica-
tion example. Briefly, the integrated navigation system in the
I. INTRODUCTION SwedishfighteraircraftGripenconsistsofaninertialnavigation
system(INS),aterrain-aidedpositioning(TAP)systemandan
THE nonlinear non-Gaussian filtering problem considered
integrationfilter.ThisfilterfusestheinformationfromINSwith
hereconsistsofrecursivelycomputingtheposteriorprob-
theinformationfromTAP.Foramorethoroughdescriptionof
abilitydensityfunctionofthestatevectorinageneraldiscrete-
thissystem,see[32]and[33].TAPiscurrentlybasedonapoint-
timestate-spacemodel,giventheobservedmeasurements.Such
massfilteraspresentedin[6],whereitisalsodemonstratedthat
ageneralmodelcanbeformulatedas
theperformanceisquitegood,closetotheCramér–Raolower
(1a) bound.FieldtestsconductedbytheSwedishairforcehavecon-
firmed the good precision. Alternatives based on the extended
(1b)
Kalmanfilterhavebeeninvestigated[5]buthavebeenshownto
Here, isthemeasurementattime isthestatevariable, beinferiorparticularlyinthetransientphase(theEKFrequires
is the process noise, isthe measurementnoise,and are the gradient of the terrain profile, which is unambiguous only
twoarbitrary nonlinearfunctions.Thetwonoise densities verylocally).Thepoint-massfilter,asdescribedin[6],islikely
and areindependentandareassumedtobeknown. tobechangedtoamarginalized particlefilterinthe futurefor
Gripen.
Theposteriordensity ,where ,isgiven
TAP and INS are the primary sensors. Secondary sensors
bythefollowinggeneralmeasurementrecursion:
(GPS and so on) are used only when available and reliable.
Thecurrentterrain-aidedpositioningfilterhasthreestates(hor-
(2a)
izontal position and heading), while the integrated navigation
system estimates the accelerometer and gyroscope errors and
(2b)
someotherstates.Theintegrationfilteriscurrentlybasedona
Kalman filter with 27 states, taking INS and TAP as primary
inputsignals.
ManuscriptreceivedOctober9,2003;revisedAugust21,2004.Thiswork
wassupportedbythecompetencecenterISISatLinköpingUniversityandthe The Kalman filter that is used for integrated navigation re-
SwedishResearchCouncil(VR).Theassociateeditorcoordinatingthereview quires Gaussian variables. However, TAP gives a multi-modal
ofthismanuscriptandapprovingitforpublicationwasDr.PaulD.Fiore.
un-symmetric distribution in the Kalman filter measurement
T. Schön and F. Gustafsson are with the Department of Electrical Engi-
neering,LinköpingUniversity,Linköping,Sweden(e-mail:[email protected]; equation and it has to be approximated with a Gaussian dis-
[email protected]). tribution before being used in the Kalman filter. This results
P.-J.NordlundiswiththeDepartmentforFlightDataandNavigation,Saab
in severe performance degradation in many cases, and is a
Aerospace,Linköping,Sweden(e-mail:[email protected]).
DigitalObjectIdentifier10.1109/TSP.2005.849151 commoncauseforfilterdivergenceandsystemreinitialization.
1053-587X/$20.00©2005IEEE
2280 IEEETRANSACTIONSONSIGNALPROCESSING,VOL.53,NO.7,JULY2005
Theappealingnewstrategyistomergethetwostatevectors A. StandardParticleFilter
intoone,andsolveintegratednavigationandterrain-aidedposi-
Theparticlefilterisusedtogetanapproximationofthepos-
tioninginonefilter.Thisfiltershouldincludeall27states,which
teriordensity inthegeneral model(1).Thisapproxi-
effectivelywouldpreventapplicationoftheparticlefilter.How-
mationcanthenbeusedtoobtainanestimateofsomeinference
ever, the state equation is almost linear, and only three states
function, ,accordingto
enterthemeasurementequationnonlinearly,namelyhorizontal
positionandheading.Oncelinearization(andtheuseofEKF)
(4)
isabsolutelyruledout,marginalizationwouldbetheonlyway
toovercomethecomputationalcomplexity.Moregenerally,as
soon as there is a linear sub-structure available in the general Inthefollowing,theparticlefilter,asitwasintroducedin[16],
model(1)thiscanbeutilizedinordertoobtainbetterestimates willbereferredtoasthestandardparticlefilter.Forathorough
andpossiblyreducethecomputationaldemand.Thebasicidea introductiontothestandardparticlefilter,see[11]and[12].The
istopartitionthestatevectoras marginalizedandthestandardparticlefilterarecloselyrelated.
ThemarginalizedparticlefilterisgiveninAlgorithm1andne-
glecting steps 4a and 4c results in the standard particle filter
(3) algorithm.
where denotesthestatevariablewithconditionallylineardy- ALGORITHM1:Themarginalizedparticlefilter
namics and denotes the nonlinear state variable [14], [32]. 1)Initialization:Fori=1;...;N,initializetheparticles
Using Bayes’ theorem we can then marginalize out the linear xn0;j((cid:0)i)1 (cid:24)px (x0n),andsetfx0l;(ji(cid:0))1;P0(ij)(cid:0)1g=fx(cid:22)l0;P(cid:22)0g:
state variables from (1) and estimate them using the Kalman 2)Fori=1;...;N,evaluatetheimportanceweights
filter[22],whichistheoptimalfilterforthiscase.Thenonlinear qt(i) =p(ytjXtn;(i);Yt(cid:0)1)andnormalize
statevariablesareestimatedusingtheparticlefilter.Thistech-
q(i)
nique is sometimes referred to as Rao-Blackwellization [14]. q~(i) = t :
The idea has been around for quite some time; see, e.g., [2], t Nj=1qt(j)
[7], [8], [12], [14], and [31]. The contribution of this article
3)Particlefiltermeasurementupdate(resampling):
is the derivation of the details for a general nonlinear state-
ResampleN particleswithreplacement
spacemodelwithalinearsub-structure.Modelsofthistypeare
commonandimportant inengineering applications,e.g.,posi-
tioning, target tracking and collision avoidance [4], [18]. The Pr xntj;(ti) =xntj;(tj(cid:0))1 =q~t(j):
marginalized particle filter has been successfully used in sev-
eral applications, for instance, in aircraft navigation [32], un- 4)ParticlefiltertimeupdateandKalmanfilter:
derwater navigation [24], communications [9], [37], nonlinear a)Kalmanfiltermeasurementupdate:
systemidentification[28],[37],andaudiosourceseparation[3]. Model1:(10),
SectionII explainsthe idea behind usingmarginalizationin Model2:(10),
conjunction with general linear/nonlinear state-space models. Model3:(22).
Three nested models are used in order to make the presenta- b)Particlefiltertimeupdate(prediction):For
tioneasytofollow.Someimportantspecialcasesandgeneral- i= 1;...;N,predictnewparticles,
izations of the noise assumptions are discussed in Section III.
To illustrate the use of the marginalized particle filter, a syn- xnt+;(1i)jt (cid:24)p xnt+1jt Xtn;(i);Yt :
thetic example is given in Section IV. Finally, the application
exampleisgiveninSectionV,andtheconclusionsarestatedin c)Kalmanfiltertimeupdate:
SectionVI. Model1:(11),
Model2:(17),
Model3:(23).
II. MARGINALIZATION 5)Sett := t+1anditeratefromstep2.
Thevarianceoftheestimatesobtainedfromthestandardpar-
Theparticlefilteralgorithm1isquitegeneralandseveralim-
ticle filter can be decreased by exploiting linear substructures
provements are available in the literature. It is quite common
inthemodel.Thecorrespondingvariablesaremarginalizedout
tointroduceartificialnoiseinstep3inordertocounteractthe
andestimatedusinganoptimallinearfilter.Thisisthemainidea
degeneracyproblem.Dependingonthecontextvariousimpor-
behindthemarginalizedparticlefilter.Thegoalofthissection
tance functions can be used in step 4b. In [11], several refine-
istoexplainhowthemarginalizedparticlefilterworksbyusing
mentstotheparticlefilteralgorithmarediscussed.
threenestedmodels.Themodelsarenestedinthesensethatthe
firstmodelisincludedinthesecond,whichinturnisincluded
B. DiagonalModel
in the third. The reason for presenting it in this fashion is to
facilitatereaderunderstanding,byincrementallyextendingthe Theexplanationofhowthemarginalizedparticlefilterworks
standardparticlefilter. isstartedbyconsideringthefollowingmodel.
SCHÖNetal.:MARGINALIZEDPARTICLEFILTERSFORMIXEDLINEAR/NONLINEARSTATE-SPACEMODELS 2281
Model1: where .Aformalproofof(8)isprovidedin[13]and
[14].Forthesakeofnotationalbrevity,thedependenceof in
(5a) ,and aresuppressedbelow.
(5b) Lemma2.1: GivenModel1,theconditionalprobabilityden-
sityfunctionsfor and aregivenby
(5c)
Thegapsintheequationsaboveareplacedthereintentionally,in (9a)
ordertomakethecomparisontothegeneralmodel(18)easier.
The state noise is assumed white and Gaussian distributed ac- (9b)
cording to
where
(6a)
(10a)
The measurement noise is assumed white and Gaussian dis- (10b)
tributedaccordingto (10c)
(10d)
(6b)
and
Furthermore, isGaussian (11a)
(11b)
(6c)
Thedensityof canbearbitrary,butitisassumedknown.The Therecursionsareinitiatedwith and .
and matricesarearbitrary. Proof: WeusestraightforwardapplicationoftheKalman
Model 1 is called the “diagonal model” due to the diagonal filter[21],[22].
structureofthestateequation(5a)and(5b).Theaimofrecur- The second density in (7) will be approximated
sivelyestimatingtheposteriordensity canbeaccom- using the standard particle filter. Bayes’ theorem and the
plishedusingthestandardparticlefilter.However,conditioned Markovpropertyinherentinthestate-spacemodelcanbeused
on the nonlinear state variable, , there is a linear sub-struc- towrite as
turein(5),givenby(5b).Thisfactcanbeusedtoobtainbetter
estimatesofthelinearstates.Analyticallymarginalizingoutthe
linear state variables from and using Bayes’ theorem
gives (12)
(7)
where an approximation of is provided by the
previous iteration of the particle filter. In order to perform
the update (12) analytical expressions for
where is analytically tractable. It is givenby the and are needed. They are provided by the
Kalmanfilter(KF); seeLemma2.1belowforthe details.Fur- followinglemma.
thermore, can be estimated using the particle filter Lemma 2.2: For Model 1 and
(PF). If the same number of particles are used in the standard aregivenby
particlefilterandthemarginalizedparticlefilter,thelatterwill,
intuitively,providebetterestimates. Thereasonfor this is that
the dimension of is smaller than the dimension of
,implyingthattheparticlesoccupyalowerdimen- (13a)
sionalspace.Anotherreasonisthatoptimalalgorithmsareused (13b)
inordertoestimatethelinearstatevariables.Let de-
note the estimate of (4) using the standard particle filter with Proof: We utilize basic facts about conditionally linear
particles. When the marginalized particle filter is used the models;see,e.g.,[19]and[36].
correspondingestimateisdenotedby .Undercertain Thelinearsystem(5b)–(5c)cannowbeformedforeachpar-
assumptionsthefollowingcentrallimittheoremholds: ticle ,andthelinearstatescanbeestimatedusingthe
Kalman filter. This requires one Kalman filter associated with
eachparticle.Theoverallalgorithmforestimatingthestatesin
Model1isgiveninAlgorithm1.Fromthisalgorithm,itshould
(8a)
beclearthattheonlydifferencefromthestandardparticlefilter
isthatthetimeupdate(prediction)stagehasbeenchanged.In
(8b) the standard particle filter, the prediction stage is given solely
2282 IEEETRANSACTIONSONSIGNALPROCESSING,VOL.53,NO.7,JULY2005
bystep4binAlgorithm1.Step4aisreferredtoasthemeasure- secondmeasurementupdatewiththetimeupdatetoobtainthe
mentupdateintheKalmanfilter[21].Furthermore,thepredic- predictedstates.Thisresultsin
tionofthenonlinearstatevariables isobtainedinstep4b.
Accordingto(5a),thepredictionofthenonlinearstatevariables (17a)
doesnotcontainanyinformationaboutthelinearstatevariables.
Thisimpliesthat cannotbeusedtoimprovethequality (17b)
oftheestimatesofthelinearstatevariables.However,ifModel (17c)
1is generalizedbyimposinga dependencebetween thelinear
(17d)
and the nonlinear state variables in (5a), the prediction of the
nonlinear state variables can be used to improve the estimates
For a formal proof of this, see the Appendix. To make
ofthelinearstatevariables.Inthesubsequentsection,itwillbe
Algorithm 1 valid for the more general Model 2, the time
elaboratedonhowthisaffectsthestateestimation.
updateequationintheKalmanfilter(11)hastobereplacedby
(17).
C. TriangularModel The second measurement update is called measurement
update due to the fact that the mathematical structure is the
Model1isnowextendedbyincludingtheterm in
same as a measurement update in the Kalman filter. However,
thenonlinearstateequation.Thisresultsina“triangularmodel”
strictly speaking, it is not really a measurement update, since
defined below.
theredoesnotexistanynewmeasurement.Itisbettertothink
Model2:
of this second update as a correction to the real measurement
updateusingtheinformationinthepredictionofthenonlinear
(14a)
statevariables.
(14b)
(14c) D. GeneralCase
Intheprevioustwosections,themechanismsunderlyingthe
withthesameassumptionsasinModel1. marginalizedparticlefilterhavebeenillustrated.Itisnowtime
Now, from (14a), it is clear that does indeed contain to apply the marginalized particle filter to the most general
information about the linear state variables. This implies that model.
there will be information about the linear state variable in Model3:
the predictionofthe nonlinearstate variable .Tounder-
standhowthisaffectsthederivation,itisassumedthatstep4b (18a)
in Algorithm 1 has just been completed. This means that the
(18b)
predictions are available, and the model can be written
(18c)
(the information inthe measurement has already been used
instep 4a)
wherethestatenoiseisassumedwhiteandGaussiandistributed
with
(15a)
(15b) (19a)
where
The measurement noise is assumed white and Gaussian dis-
tributedaccordingto
(15c)
(19b)
It is possible to interpret as a measurement and as the
correspondingmeasurementnoise.Since(15)isalinearstate- Furthermore, isGaussian
spacemodelwithGaussiannoise,theoptimalstateestimateis
givenbytheKalmanfilteraccordingto (19c)
Thedensityof canbearbitrary,butitisassumedknown.
(16a)
Incertaincases,someoftheassumptionscanberelaxed.This
(16b) willbediscussedinthesubsequentsection.Beforemovingon
it is worthwhile to explain how models used in some applica-
(16c)
tionsofmarginalizationrelatetoModel3.In[23],themarginal-
(16d) izedparticlefilterwasappliedtounderwaternavigationusinga
modelcorrespondingto(18),savethefactthat
where“ ”hasbeenusedtodistinguishthissecondmeasurement . In [18], a model corresponding to linear
update fromthe first one.Furthermore, and aregiven stateequationsandanonlinearmeasurementequationisapplied
by(10a)and(10b),respectively.Thefinalstepistomergethis tovariousproblems,suchascarpositioning,terrainnavigation,
SCHÖNetal.:MARGINALIZEDPARTICLEFILTERSFORMIXEDLINEAR/NONLINEARSTATE-SPACEMODELS 2283
andtargettracking.Duetoitsrelevance,thismodelwillbedis- work,theseconddensity in(20)istakencareofac-
cussed in more detail in Section III. Another special case of cordingto(12).Theanalyticalexpressionsfor
Model3hasbeenappliedtoproblemsincommunicationtheory and areprovidedbythefollowingtheorem.
in [9] and [37]. The model used there is linear. However, de- Theorem 2.2: For Model 3, and
pendingonanindicatorvariablethemodelchanges.Hence,this aregivenby
indicatorvariablecanbethoughtofasthenonlinearstatevari-
ableinModel3.Agoodanddetailedexplanationofhowtouse
themarginalizedparticlefilterforthiscasecanbefoundin[14].
(25a)
TheyrefertothemodelasajumpMarkovlinearsystem.
Analogouslytowhathasbeendonein(7),thefilteringdistri-
bution issplitusingBayes’theorem
(20) (25b)
ThelinearstatevariablesareestimatedusingtheKalmanfilter
Proof: For the basic facts about conditionally linear
in a slightly more general setting than which was previously
models, see [19]. The details for this particular case can be
discussed. However,it is stillthe same three stepsthatare ex-
foundin[36].
ecuted in order to estimate the linear state variables. The first
ThedetailsforestimatingthestatesinModel3havenowbeen
stepisameasurementupdateusingtheinformationavailablein
derived,andthecompletealgorithmisAlgorithm1.Aspointed
. The second step is a measurement update using the infor-
out before, the only difference between this algorithm and the
mation available in , and finally, there is a time update.
standardparticlefilteringalgorithmisthatthepredictionstage
The following theorem explains how the linear state variables
is different.Ifsteps 4aand 4c are removedfrom Algorithm 1,
are estimated.
thestandardparticlefilteralgorithmisobtained.
Theorem 2.1: Using Model 3 the conditional probability
Inthispaper,themostbasicformoftheparticlefilterhasbeen
densityfunctionsfor and aregivenby
used. Several more refined variants exist, which in certain ap-
plicationscangivebetterperformance.However,sincetheaim
(21a)
ofthisarticleistocommunicatetheideaofmarginalizationin
(21b) a general linear/nonlinear state-space model, the standard par-
ticle filter has been used. It is straightforward to adjust the al-
where gorithmgiveninthispapertoaccommodatee.g.,theauxiliary
particlefilter[34]andtheGaussianparticlefilter[26],[27].Sev-
(22a)
eralideasarealsogiveninthearticlescollectedin[11].
(22b) Theestimatesasexpectedmeansofthelinearstatevariables
andtheircovariancesaregivenby[32]
(22c)
(22d)
(26a)
and
(23a)
(26b)
(26c)
(23b)
(23c)
where arethenormalizedimportanceweights,providedby
(23d)
step2inAlgorithm1.
where
III. IMPORTANTSPECIALCASESANDEXTENSIONS
(24a)
Model 3 is quite general indeed, and in most applications,
(24b) specialcasesofitareused.Thisfact,togetherwithsomeexten-
sions,willbethetopicofthissection.
(24c)
The special cases are just reductions of the general results
Proof: SeetheAppendix. presented in the previous section. However, they still deserve
It is worth noting that if the cross-covariance between some attention in order to highlight some important mecha-
the two noise sources and is zero, then and nisms.Itisworthmentioningthatlinearsubstructurescanenter
. The first density on the right-hand the model more implicitly as well, for example, by modeling
sidein(20)isnowtakencareof.Inorderfortheestimationto colorednoiseandbysensoroffsetsandtrends.Thesemodeling
2284 IEEETRANSACTIONSONSIGNALPROCESSING,VOL.53,NO.7,JULY2005
issues are treated in several introductory texts on Kalman fil- usually corresponds to the position, whereas the linear state
tering,seee.g.,[17,Sec.8.2.4].Inthesubsequentsection,some variable corresponds to velocity, acceleration, and bias
noisemodelingaspectsarediscussed.Thisisfollowedbyadis- terms.
cussion ofa modelwith linearstate equations and a nonlinear If Model 4 is compared to Model 3, it can be seen that the
measurementequation. matrices ,and areindependentof inModel4,
whichimpliesthat
A. GeneralizedNoiseAssumptions
(28)
TheGaussiannoiseassumptioncanberelaxedintwospecial
cases.First,ifthemeasurementequation(18c)doesnotdepend
This follows from (23b)–(23d) in Theorem 2.1. According to
onthelinearstatevariables, ,i.e., ,themeasure-
(28)onlyoneinsteadof Riccatirecursionsisneeded,which
mentnoisecanbearbitrarilydistributed.Inthiscase,(18c)does
leads to a substantial reduction in computational complexity.
notcontainanyinformationaboutthelinearstatevariablesand,
This is, of course, very important in real-time implementa-
hence,cannotbeusedintheKalmanfilter.Itissolelyusedinthe
tions. A further study of the computational complexity of the
particlefilterpartofthealgorithm,whichcanhandleallproba-
marginalizedparticlefiltercanbefoundin[25].
bilitydensityfunctions.
If the dynamics in (18a)–(18b) are almost linear, it can be
Second,if in(18a),thenonlinearstateequation
linearized to obtain a model described by (27a)–(27b). Then,
will be independent of the linear states and, hence, cannot be
the extended Kalman filter can be used instead of the Kalman
used in the Kalman filter. This means that the state noise
filter. As is explained in [29] and [30], it is common that the
canbearbitrarilydistributed.
systemmodelisalmostlinear,whereasthemeasurementmodel
Thenoisecovariancescandependonthenonlinearstatevari-
is severely nonlinear. In these cases, use the particle filter for
ables,i.e., and .Thisisusefulfor
theseverenonlinearitiesandtheextendedKalmanfilterforthe
instanceinterrainnavigation,wherethenonlinearstatevariable
mildnonlinearities.
includes information about the position. Using the horizontal
position and a geographic information system (GIS) onboard
IV. ILLUSTRATINGEXAMPLE
theaircraft,noisecovariancesdependingonthecharacteristics
oftheterrainatthecurrenthorizontalpositioncanbemotivated. Inordertomakethingsassimpleaspossible,thefollowing
WewillelaborateonthisissueinSectionV. two-dimensionalmodelwillbeused:
B. ImportantModelClass (29a)
A quite important special case of Model 3 is a model with (29b)
linearstateequationsandanonlinearmeasurementequation.In
Model4below,suchamodelisdefined. where the state vector is . Hence, the state con-
Model4: sistsofaphysicalvariableanditsderivative.Modelsofthiskind
areverycommoninapplications.Oneexampleisbearingsonly
(27a) tracking, where the objective is to estimate the angle and an-
gular velocity and the nonlinear measurement depends on the
(27b)
antennadiagram.Anothercommonapplicationisstateestima-
(27c)
tion in a DC-motor, where the angular position is assumed to
bemeasurednonlinearly.Asafinalapplicationterrainnaviga-
with and .Thedistributionfor
tioninonedimensionismentioned,wherethemeasurementis
canbearbitrary,butitisassumedknown.
givenbyamap.Amorerealisticterrainnavigationexampleis
Themeasurementequation(27c)doesnotcontainanyinfor-
discussedinSectionV.
mation about the linear state variable . Hence, as far as the
Model(29)islinearin andnonlinearin .Thestatevector
Kalmanfilterisconcerned,(27c)cannotbeusedinestimating
can thus be partitioned as , which implies that
thelinearstates.Instead,allinformationfromthemeasurements
(29)canbewrittenas
enter the Kalman filter implicitly via the second measurement
updateusingthenonlinearstateequation(27a)andthepredic-
(30a)
tionofthenonlinearstate ,asameasurement.Thismeans
(30b)
that in Algorithm 1, step 4a can be left out. In this case, the
second measurement update is much more than just a correc- (30c)
tiontothefirstmeasurementupdate.Itistheonlywayinwhich
This corresponds to the triangular model given in Model 2.
theinformationin entersthealgorithm.
Hence,theKalmanfilterforthelinearstatevariableisgivenby
Model4isgivenspecialattentionasseveralimportantstate
(22)–(24),wherethenonlinearstateisprovidedbytheparticle
estimation problems can be modeled in this way. Examples
filter.Theestimateofthelinearstatevariableisgivenby(23a),
include positioning, target tracking, and collision avoidance
which,forthisexample,is
[4],[18].Formoreinformationonpracticalmattersconcerning
modeling issues, see, e.g., [4], [29], [30], and [32]. In the
applications mentioned above, the nonlinear state variable (31)
SCHÖNetal.:MARGINALIZEDPARTICLEFILTERSFORMIXEDLINEAR/NONLINEARSTATE-SPACEMODELS 2285
Fig. 2. Using the marginalized particle filter for navigation. The terrain
Fig.1. Integratednavigationsystemconsistsofaninertialnavigationsystem
informationisnowincorporateddirectlyinthemarginalizedparticlefilter.The
(INS),aterrain-aidedpositioning(TAP)system,andanintegrationfilter.The
radaraltimeterdeliversthehightmeasurementy .
integrationfilterfusetheinformationfromINSwiththeinformationfromTAP.
A. DynamicModel
where
Inordertoapplythemarginalizedparticlefiltertothenaviga-
(32) tionproblemadynamicmodeloftheaircraftisneeded.Inthis
paper, the overall structure of this model is discussed. For de-
Intuitively,(31)makessense,sincethevelocityestimateisgiven tails,see[32]andthereferencestherein.Theerrorsinthestates
asaweightedaverageofthecurrentvelocityandtheestimated are estimated instead of the absolute states. The reason is that
momentaryvelocity,wheretheweightsarecomputedfromthe the dynamics of the errors are typically much slower than the
Kalman filter quantities. In cases where (29a) is motivated by dynamics of the absolute states. The model has the following
Newtons’forcelaw,theunknownforceismodeledasadistur- structure:
bance,and .Thisimpliesthat(31)isreducedto
(35a)
(33) (35b)
(35c)
Again, this can intuitively be understood, since, because it is
conditioned on the knowledge of the nonlinear state variable,
(30a)canbewrittenas Therearesevenlinearstatesandtwononlinearstates.Thelinear
statesconsistoftwovelocitystatesandthreestatesfortheair-
(34) craftintermsofheading,roll,andpitch.Finally,therearetwo
states for the accelerometer bias. The nonlinear states corre-
Thus,(30b)doesnotaddanyinformationfortheKalmanfilter spondtotheerrorinthehorizontalposition,whichisexpressed
since isadeterministicfunctionoftheknownnonlinearstate inlatitude andlongitude .
variable. Thetotaldimensionofthestatevectoristhus9,whichistoo
large to be handled by the particle filter. The highly nonlinear
natureofmeasurementequation(35c),duetotheterraineleva-
V. INTEGRATEDAIRCRAFTNAVIGATION
tiondatabase,impliesthatanextendedKalmanfiltercannotbe
Aswasexplainedintheintroduction,theintegratednaviga-
used.However,themodeldescribedby(35)clearlyfitsintothe
tion system in the Swedish fighter aircraft Gripen consists of
frameworkofthemarginalizedparticlefilter.
aninertialnavigationsystem(INS),aterrain-aidedpositioning
Themeasurementnoisein(35c)deservessomespecialatten-
(TAP)system,andanintegrationfilter.Thisfilterfusesthein-
tion.Theradaraltimeter,whichisusedtomeasuretheground
formationfromINSwiththeinformationfromTAP;seeFig.1.
clearance,interpretsanyechoastheground.Thisisaproblem
Thecurrentlyusedintegrationfilterislikelytobechangedtoa
whenflyingovertrees.Thetreetopswillbeinterpretedasthe
marginalizedparticlefilterinthefutureforGripen;seeFig.2.A
ground, with a false measurement as a result. One simple, but
firststepinthisdirectionwastakenin[18],whereasix-dimen-
effective,solutiontothisproblemistomodelthemeasurement
sionalmodelwasusedforintegratednavigation.Insixdimen-
noise as
sions,theparticlefilterispossibletouse,butbetterperformance
canbeobtained.Asdemonstratedin[18],4000particlesinthe (36)
marginalized filter outperform 60000 particles in the standard
particle filter. where istheprobabilityofobtaininganechofromtheground,
The feasibility study presented here applies marginalization and istheprobabilityofobtaininganechofromthetree
toamorerealisticnine-dimensionalsubmodelofthetotalinte- tops. The probability density function (36) is shown in Fig. 3.
gratednavigationsystem.Alreadyhere,thedimensionalityhas Experimentshaveshownthatthis,inspiteofitssimplicity,isa
proventobetoolargefortheparticlefiltertobeapplieddirectly. quiteaccuratemodel[10].Furthermore, ,and
Theexamplecontainsallingredientsofthetotalsystem,andthe in(36)canbeallowedtodependonthecurrenthorizontalpo-
principleisscalabletothefull27-dimensionalstatevector.The sition .Inthisway,informationfromtheterraindatabase
model can be simulated and evaluated in a controlled fashion; canbeinferredonthemeasurementnoiseinthemodel.Using
see[32]formoredetails.Inthesubsequentsections,theresults this information,itis possibletomodel whetherthe aircraftis
fromfieldtrialsarepresented. flyingoveropenwateroroveraforest.
2286 IEEETRANSACTIONSONSIGNALPROCESSING,VOL.53,NO.7,JULY2005
Fig. 3. Typical histogram of the errorin the data fromthe radar altimeter.
Thefirstpeakcorrespondstotheerrorinthegroundreading,andthesecond
peakcorrespondstotheerrorinthereadingsfromthetreetops.
B. Result
The flight that has been used is shown in Fig. 4. This is a
fairly tough flight for the algorithm, in the sense that during
some intervals data are missing, and sometimes, the radar al-
timeter readings become unreliable. This happens at high alti-
tudesandduringsharpturns(largerollangle),respectively.In
ordertogetafairunderstandingofthealgorithm’sperformance,
100MonteCarlosimulationswiththesamedatahavebeenper-
formed, where only the noise realizations have been changed
from one simulation to the other. Many parameters have to be
chosen, but only the number of particles used are commented
here (see [15] for more details). In Fig. 5, a plot of the error
in horizontal position as a function of time is presented for a
different numberof particles.Thetrueposition is providedby
thedifferentialGPS(DGPS).Fromthisfigure,itisobviousthat
theestimateimprovesasmoreparticlesareused.Thisisnatural
sincethetheorystatesthatthedensitiesareapproximatedbetter
whenmoreparticlesareused.Thedifferenceinperformanceis
Fig.4. Flightpathusedfortestingthealgorithm.Theflightpathisclockwise,
mainly during the transient, where it can be motivated to use andthedarkregionsinthefigureareopenwater.
more particles.By increasingthe numberof particles the con-
vergencetimeissignificantlyreduced,andabetterestimateis
thelinearstates.Thesecondstepwastousethepredictionofthe
obtained.Thisistrueupto5000particles.Hence,5000particles
nonlinearstateasanadditionalmeasurement.Thiswasusedto
wereusedinthisstudy.Thealgorithmcanbefurtherimproved,
obtain better estimates of the linear state variables. The com-
andin[15],severalsuggestionsaregiven.
pletedetailsforthemarginalizedparticlefilterwerederivedfor
Theconclusionfromthisstudyisthatthemarginalizedpar-
a general nonlinear and non-Gaussian state-space model. Sev-
ticle filter performs well and provides an interesting and pow-
eralimportantspecialcaseswerealsodescribed.Conditionsim-
erfulalternativetomethodscurrentlyusedinintegratedaircraft
plyingthatalltheKalmanfilterswillobeythesameRiccatire-
navigationsystems.
cursionweregiven.
Finally, a terrain navigation application with real data from
VI. CONCLUSION
theSwedishfighteraircraftGripenwaspresented.Theparticle
Themarginalizationtechniqueshavesystematicallybeenap- filter is not a feasible algorithm for the full nine-state model
pliedtogeneralnonlinearandnon-Gaussianstate-spacemodels, since a huge number of particles would be needed. However,
with linear substructures. This has been done in several steps, since only two states (the aircrafts horizontal position) appear
where eachstepimpliesa certainmodification ofthe standard nonlinearlyinthemeasurementequation,aspecialcaseofthe
particlefilter.ThefirststepwastoassociateoneKalmanfilter generalmarginalizationalgorithmcanbeapplied.Averygood
witheachparticle.TheseKalmanfilterswereusedtoestimate resultcanbeobtainedwithonly5000particles,whichisreadily
SCHÖNetal.:MARGINALIZEDPARTICLEFILTERSFORMIXEDLINEAR/NONLINEARSTATE-SPACEMODELS 2287
Using(37b)and(38),(37a)canberewrittenaccordingto(
isassumedinvertible.Thecaseofanoninvertible istreated
in [5])
(40)
(41)
where
(42)
Thede-correlatedsystemis
(43a)
(43b)
Fig. 5. Horizontal position error as a function of time units for different
(43c)
numbersofparticles.ThemarginalizedparticlefiltergiveninAlgorithm1has
beenused.
which is a linearsystemwith Gaussiannoise. Moreover,from
(37d)and(37e),itcanbeseenthat and areknownif
possible to implement in the computer currently used in the
and are known. The actual proof, using induction, of the
aircraft.
theorem can now be started. At time zero,
.Now,assumethat is
APPENDIX Gaussianatanarbitrarytime .
PROOFFORTHEOREM2.1 Therecursionsaredividedintothreeparts.First,theinforma-
tionavailableintheactualmeasurement ,i.e., isused.Once
Theproofof(16)and(17)isprovidedasaspecialcaseofthe
themeasurementupdatehasbeenperformed,theestimates
proof below.
and areavailable.Thesecannowbeusedtocalculatethe
Proof: Forthesakeofnotationalbrevity,thedependence
predictionsofthenonlinearstate .Thesepredictionswill
on in(18)issuppressedinthisproof.Write(18)as
providenewinformationaboutthesystem.Second,thisnewin-
formationisincorporatedbyperformingasecondmeasurement
(37a)
updateusing theartificialmeasurement .Finally,atimeup-
(37b) date,usingtheresultfromthesecondstep,isperformed.
(37c) Part 1: Assume that both
and areavailable.Thismeansthat can
where and aredefinedas becomputedas
(37d)
(37e)
(44)
Inspectionoftheaboveequationsgivesthat and canboth
Using the fact that the measurement noise and, thereby,
bethoughtofasmeasurements,sincemathematically,(37b)and
is Gaussian,asis the Kalmanfilter [1].itcanbe
(37c)possessthestructureofmeasurementequations.Thefact
seenthat ,where
thatthereisacross-correlationbetweenthetwonoiseprocesses
and , since in (19a), has to be taken care of.
Thiscanbe accomplishedusing theGram–Schmidt procedure (45a)
todecorrelatethenoise[17],[21].Insteadof ,thefollowing
(45b)
can beused
(45c)
(45d)
(38) Part2:Atthisstage, becomesavailable.Use
resultingin ,and
(39) (46)