NonlinearProgrammingApproachtoFilter
Tuning
DylanM.Asmar andGreg J.Eslinger
May 15,2012
1
Introduction
State estimation is a problem that is often encountered across multiple disciplines. Filters are used to obtain state estimations based on measurements
whenoneorbothofthedynamicsandmeasurementsareinaccurateorsubject
tonoise. Various filtermethodscanbeapplied toobtain accuratestateestimations. SuchfiltersincludetheKalmanfilter[1],particlefilter,[2],unscented
KalmanfilterandextendedKalmanfilter[3],andaconstantratefilterbased
ontheFokker-Planckequation[4]justtonameafew. However,despitealarge
varietyinfilters,mostrequiretuningafterimplementation.
Filter tuning is the process of selecting measurement and dynamic noise
statisticstoobtainadesiredperformance. EvenoptimalfiltersliketheKalman
filter are sensitive to tuning parameters [5]. Often, filter tuning is done via
trialanderror. Thisprocessmayprovideinsightfortheengineering,butoften
consumesalargeamountoftime[6].
Aplethoraofresearchhasbeendevotedtodevelopalgorithmstooptimally
and/or automatically tune filters, especially the Kalman filter. These methods include a neural network based approach [7], using the downhill simplex
algorithm[6],reinforcementlearning[8],andmanymore[9,10]. However,gradientbasedmethodshavelargelybeenavoided. Thedi_cultyofanalytically
calculatingthegradient,approximatingthecostfunctionwithawell-behaved
function,andtheinstabilityofnumericaldi_erentiationofnoisyfunctionshave
allcontributedtothisavoidance[7,6].
Thispaperfocusesonapplyinggradient-basedmethodstotunefilterswhich
estimatetheverticalstateofaircraftbasedonlyonquantizedaltitudemeasurements. Di_erentstateestimationtechniqueshavebeendevelopedforquantized
measurements; however, only the Kalman filter and a modified Kalman filter
[11]willbediscussed.
Therestofthispaperisorganizedasfollows. Section2givesanoverviewof
theaircrafttrackingproblemandthemodelused. Section3outlinesthegeneral
mathematicalprogramdefinitionandthenformulatesaformalrepresentation
ofthetrackingproblem. Section4discribesthevariousapproachestogradient
1
basedoptimization. Section5showstheresultsofthedi_erentmethodsaswell
asaderivationofanimprovedsearchalgorithm.
2
ProblemOverview
Theproblemunderconsiderationisfindingthebestfilterparametersforvertical
state estimation of aircraft with quantized altitude measurements. Tracking
suchaircraftisanapplicablereal-worldproblem. TheTra_cAlertandCollision
AvoidanceSystem(TCAS)ismandatedonalllargeaircraftworldwide. TCAS
servesasalastminutedefenseagainstmidaircollisionsandissuescommandsto
changealtitudewhenneededintheformofresolutionadvisories. Themajority
oftheadvisoriesinthecurrentU.S.airspacearewithintrudersthatbroadcast
theiraltitudewith100ftquantization. Lastminutecollisionavoidancesystems
relyonprecisealtituderateestimationsandaccuratelyestimatingthevertical
ratefromsuchquantizedmeasurementsisdi_cult[12].
2.1
Model
Anaircraft’sverticalstateisdefinedintermsofaltitude,
h˙ . Thisisformallydefinedas
_hk _
xk = ˙
hk
h,andverticalrate,
A)
Thestateestimationisperformedbasedonobservationsof
zk =Δround
_1 0_x k + wk0 !
Δ
where k isadiscrete-timeindex,_isthesizeofthequantizationlevel,and
iszero-meanGaussianmeasurementnoisewithcovariance R 0.
Theaircraftverticalmotioncanbemodeledwithadiscrete-timelineardynamicsystemas:
x k +1 = Fx k + Gvk
where x k isthestatevectorpreviouslydefined,
noisewithcovariance Q,and
B)
wk0
C)
vk iszero-meanGaussianprocess
_1 dt _
0 1
_dt/2 2_
G=
dt
F =
D)
E)
where dt isthechangeintimefrom
k s 1 to k . Inthisformulation,thetracked
aircraftisassumedtomaintainconstantvelocityandmaneuversaremodeled
asaccelerationsduetoprocessnoise. Themeasurements zk aremodeledby
zk = Hx k + wk
2
F)
where wk iszero-meanGaussianobservationnoisewithcovariance
R ,and
H = _1 0_
2.2
G)
Filtering
ModelingtheverticaltrackingproblemintheformatdescribedinSection2.1,
allowsfordi_erentfilterstobeapplied. AKalmanfilterandamodifiedKalman
filter[11]werechosen. TheKalmanfilteristhebestlinearestimatorandthe
modifiedKalmanfilterprovidesasimilarnonlinearcounterparttoanalyze. The
detailsofthefilteringalgorithms arebeyondthescopeofthispaper, butare
providedinAppendixAforreference.
3
ProgramFormulation
Thedesiredperformanceofafilterdictateshowtheparametersshouldbeselected. AsdescribedinSection2,theverticalestimationproblemiscrucialto
last minute air tra_c collision avoidance systems since they rely on accurate
verticalrateestimates. Thisnaturallyleadstoadesiretominimizetheerrorin
theestimatedverticalrate. Theidealsituationwouldallowfortheminimizationoftheerrorduringestimationprocess. However,thereisnoaccesstothe
truedataduringestimation. Therefore,theparametershavetotunedapriori.
Stateestimationproblemsoftenhavehistoricaldataonthesystemforwhich
the filters were designed. Usingthis assumption ofattainable data, the filter
canbetunedo_inetooptimizeperformanceoverasubsetofdatathatisagood
representationoftheactualsystem. Applyingthisideainageneralsense,the
costfunctioncanbedefinedintermsoftherootmeansquarederror(RMSE)
as:
n
s
X X cj RMSEij
J ( p,p,
1 2 ∙∙∙ ,p l ) =
H)
i =1 j =1
where J isthecostafunctionof
l parameters, p, cj istheweightoftheRMSE
i
j of item i of the subset, n is the
of state j , RMSE jis the RMSE of state
numberofitemsinthesubset,and
s isthenumberofstatevariables. Forthe
KalmanfilterandmodifiedKalmanfilter,theonlychangeinresultswouldbe
subjecttotheprocessnoisecovariance, Q,andtheobservationnoisecovariance,
R . Thereforetheformaldefinitionofourprobleminthegeneralsensewould
be:
n
s
min J ( Q,R ) = X X cj RMSEij
Q,R
i =1 j =1
s.t. Q,R _ 0
Therefore,theprogramformulationfortheverticaltrackingproblembecomes:
n
min J ( Q,R ) = X Ych
j
Q,R
i =1
3
i
RMSE
+ ch˙ h˙ iRMSE _
Figure1: ContourPlotofanExampleDataSet
s.t. Q,R ≥ 0
isthealtitude RMSEfor track
ch˙ arepredefined weights,
i , h˙ iRMSE is
i ,and n isthenumberoftracksinthesetfor
theverticalrateRMSEfortrack
whichtheoptimizationisoccurring.
Theparticularproblemposedisadvantageousfromanintuitivedesignperspectiveinthatanyoptimizationwilloccurbychangingtwoscalarvalues. This
allowscontourandmeshplotstobeusedtoaidtheKalmanfilterandmodifiedKalmanfiltertuning. Inalternativeproblemswithhigherdimensionsthis
wouldnotbepossible;however,thisproblemwillallowforademonstrationof
theperformanceofthesearchalgorithmsthroughtheaidofplots. Fig.1shows
anexampleofwhatacontourplotmightlooklikeforthisproblem,whileFig.2
isathreedimensionalmeshofthesameexamplecase.
Noticeinthecontourandmeshplotthattheredidnotappeartobeaunique
globalminimum;rather,thereexistedatroughofQandRvaluesthatproduced
similarcostsforallQandRinthetrough. Uponcloserinspectionthetrough
didcontainaglobaloptima.
hiRMSE
4
Methods
This section discusses various gradient-based methods used to solve unconstrainednonlinearoptimizationproblems.
4.1
GradientDerivation
Sincethecostfunctionwasnotexplicit,thereweretwoapproachesconsidered
fordeterminingthegradient: adjointmethodsandfinitedi_erenceequations.
4
Figure2: 3DimensionalMeshofanExampleDataSet
4.1.1
AdjointMethods
Adjoint methods find the derivative of a function imbeded in a program by
trackingthederivitivesofallvariablesintheprogram. Themethodisbasedon
thefactthatallprogramsarecomprisedofasetofequations. Therefore,any
programcanbewrittenas
y = F n ( F n k 1 ( ...F 2 ( F 1 ( x )) ...))
I)
where Fi ( • ) is the i th line of code in the program [13]. Using this logic, the
derivitiveof y withrespectto x isfoundusingthechainrule:
∂y
∂F n ∂F n k 1 ∂F 2 ∂F 1
...
=
∂x
∂z n k 1 ∂z n k 2 ∂z 1 ∂x
_)
where zi isthevectorofallinternalvariablesatline
i ofthecode. Theadjoint
canbecalculatedintheforwardorbackwarddirection;however,if
n =dim( x )
and m =dim( z), calculatingtheadjointintheforwarddirectionrequirestrackingan m × n matrixwhilecalculatingtheadjointinthereversedirectionrequires
trackinga1 × m matrix.
Adjoint methods will produce the analytic gradient of the function with
respecttotheinputvariables; however,adjointmethodsrequirethreetofour
linesofcodeforeveryoriginallineofcodeexecutedintheforwarddirection.
Adjoint methods also require every value for every variable be saved for the
reversedirection,whichcanfillupcomputermemoryquickly.
5
4.1.2
Finite-Di_erenceEquations
Finite-di_erence equationsapproximatethederivativeofthefunctionbyperturbingthefunctionbyasmallamountandtrackingthechangeintheobjective
function. Assumingthefunctionisdi_erentiable,thegradientisapproximated
byfindingtheslopeofthelinearizationattheperturbationpointusingtheperturbation data. Because the finite-di_erence equations only approximate the
gradient,thegradientproducedbythefinite-di_erenceequationswillnotbeas
accurateasthoseproducedbyadjointmethods. However, thecomputational
overheadoftheerrorcalculationwasverysmallascomparedtothefulladjoint
method. Forthisreasonfinite-di_erenceequationswerechosentoapproximate
thegradient.
4.2
Algorithms
SincetheHessianoftheobjectivefunctionisnotaccessable,theavailableoptimizationalgorithmsare:
• SteepestDescent
• Quasi-NewtonMethods
• FirstOrderMethods
ThesealgorithmswillbediscussedindetailinSection4.2.1,Section4.2.2,and
Section4.2.3,respectivly.
4.2.1
SteepestDescentMethods
TheSteepestDescentAlgorithm,aspresentedbyProfRobertFreund,isseen
inAlgorithm1:
Algorithm1 SteepestDescent
Given x 0
0
k
repeat
dk := sr f ( x k )
α =argmin α f ( x k + αdk )
x k +1
x k + αdk
k
k+ 1
until dk =0
Thesteepestdescentmethodhastheadvantageofbeingoneofthesimplist
optimizationmethods. Itisalsorequireslittleoverhead,astheonlyinformation
x k . However,thesteepest
thatissavedfromaninterationisthecurrentvalueof
descentmethoddoesusealinesearch, whichcanbecostlydependingonthe
function. Thesteepestdescentmethodcanalsobreakdownwhentheprobrlem
isill-conditioned,thatis,whenthesearchpathfallsintoavalleythatissteep
inonedirectionbutshallowinanother.
6
4.2.2
Quasi-NewtonMethods
Quasi-NewtonMethodsattempttobuildupanestimateoftheHessianasthe
searchprogressesbyusinginformationonthefunctionvalueandthegradient.
TheearliestQuasi-NewtonMethodistheDavidon,Fletcher,Powell,orDFP,
method. ThemostpopularmethodnowadaysistheBroyden,Fletcher,Goldfarb, Shanno, orBFGS,method[13]. TheBFGSAlgorithm, aspresentedby
ProfStevenHall,isseeninAlgorithm2:
Algorithm2
Quasi-Newton(BFGS)
0
Given x
B 0 := I
0
k
repeat
dk := s B k r f ( x k )
α =argmin α f ( x k + αdk )
x k +1
x k + αdk
sk = αd k = x k +1 s x k
yk = r f ( x k +1 ) s f ( x k )
T
_ I s yskT Tk _ B k _ I s
B k +1
ys k
k
ysk
y kTs
T
k
k
_+
ssk
y kTs
T
k
k
k
k+ 1
until dk =0
TheadvantageofQuasi-Newtonmethodsisthattheydonotstrugglewith
ill-conditionedproblemsasmuchassteepestdescentmethodsdo. Thedownsideisthatadditionalcalculationsarerequiredateachiteration. Inaddition,
B k +1 ,mustbestoredateveryiteration.
additionalinformation,intheformof
4.2.3
FirstOrderMethods
TheSimpleGradientSchemeoftheFirstOrderMethod,aspresentedbyProf
RobertFreund,isseeninAlgorithm3:
Algorithm3 FirstOrder
Given x 0 , L
k
0
repeat
dk := sr f ( x k )
1
x k +1
x k s←r
f (x k )
L
k
k+ 1
until dk =0
This method is very similar to the steepest descent method in that the
descentdirectionissimplythenegativeofthegradientofthefunctionevaluated
7
at x k . However,oneofthemajorassumptionsofthefirstordermethodisthat
f ( • ) hasa Lipschitz gradient. Thatis,thereisascaler
L forwhich
kr f ( y) sr f ( x ) k≤ L ky s x k8 x,y 2 Rn
)
L willbeinitializedat
L =1. At
Since L isunknownforthisapplication,
everyiteration,ifEq.)holds,thenthecurrentvalueof
L issu_centforthe
searchalgorithmtoconverge.
f ( y) ≤ f ( x ) + r f ( x ) T ( y s x ) +
L
ky s x k2
2
)
Should the current iteration fail this test, the algorithm is set to double the
currentguessof L andrecomputethestep. Therefore,thetruealgorithmthat
willbeimplimentedforthisprojectisseeninAlgorithm4
Algorithm4
FirstOrder(Modified)
Given x 0
L = L0
. Theusermustprovideaguessfor
k
0
repeat
dk := sr f ( x k )
1
x k +1
x k s←r
f (x k )
L
k +1
k
if f ( x
) >f ( x ) + r f ( x kT) ( x k +1 s x k +1 ) + L2 ky s x k2 then
L =2 L
x k +1
xk
. Thisrerunstheiterationwiththenewvaluefor
endif
k
k+ 1
until dk =0
5
Results
ThissectiondiscussestheresultsofeachalgorithmdiscussedinSection4. Multipledatasetswereusedtosimulatevarioustruedatasets.
5.1
DataSetGeneration/Evaluation
Threetypesofdatasetswereusedwhentestingthealgorithms.
• Synthetictracksfromanassumedmodel
• Synthetictracksgeneratedformahighfidelityencountermodel
• Actualradardataofaircraftcollisionavoidanceadvisoryevents
8
L0
L
Figure3: ConvergencePathsofDi_erentSearchAlgorithms
Thefirstsetofdatawasusedasastartingpointandacontrol. Trackswere
generatedfromthedynamicmodelgivenbyEqs.B)toE). Thealtitudewas
quantizedat100ftlevelsaftertheobservationnoisewasaddedtogeneratedthe
measurements. The process and observation noise covariances were varied to
generatevariousdatasets. Thestartingaltitudewassampledfromanormal
distributionwithameanof15000ftandastandarddeviationof5000ft. The
initialaltituderatewassamplefromanormaldistributionwithameanof0ft/s
andastandarddeviationof50ft/s.
Thesecondtypeofdatasetweregeneratedfromahighfidelityencounter
model[14]. Thetracksaregeneratedfromacorrelatedmodelbasedonrecorded
advisoryeventsintheU.S.airspace. Thethirdtypeofdatasetwasactualradar
dataofencounterswithaircraftof100ftquantizationlevels.
Each scenario resulted in the convergence of at least one algorithm to an
optimum. Theparametersettingsfoundusingsubsetsoftruedata(theradar
data) were applied to the larger set. In each case, the mean RMSE over all
tracks was lower than multiple other settings tested. Including settings from
anexperiencedengineeringthathadbeenworkingwiththeKalmanfilterand
modifiedKalmanfilterforthisproblem.
5.2
AlgorithmPerformance
Thethreedescribedalgorithmswererunonaseveraldi_erentsetsofdata. The
results forallthree methodsfrom aradar datasetareshowninFig. 3. The
purposeofthissectionisnottopresenttheresultofonedataset;rather,itis
tocompareandcontrasttheperformanceofthedi_erentsearchalgorithmson
thisparticulartypeofoptimization.
9
Figure4: LinearConvergenceofSteepestDescentAlgorithm
5.3
SteepestDescentAnalysis
Thesteepestdescentalgorithmwasabletoconvergeinaregionthate_ectively
minimizedtheobjectivefunction. Fig.3alsoshowsthathem-stitchingoccured
during the search. The steepest descent mehtod also converged linearly. An
algorithmissaidtoconvergelinearlyifthereisaconstant
δ< 1 suchthatfor
all k su_centlylarge,theiterates x k satisfy:
f ( x k +1 ) s f ( x _ )
≤ δ
f (xk ) s f (x _ )
_)
where x _ issomeoptimalvalueoftheproblem[15]. Theplotof
δ ateachiteration
canbeseeninFig.4. Inthisparticularexamplethealgorithmdidnotrunfor
enoughinterationstomakeadefinitvestatementonwhethertheconvergence
waslinear. ThereappearstobealinearsectionbetweenIteration2and8as
wellasasuperlinear section between Iteration 8and 12; however, this isnot
enoughiterationstoclearlyproveordisprovelinearconvergence.
r g( x k +1 ) •r g( x k ) 6
Itisalsointerestingtonotethat
=0. Whenanexactline
searchisusedand theexact gradientisprovided, everyiteration travelsperpediculartothepreviousiteration. Sincethegradientofthefunctionwasonlya
numericalapproximationandthelinesearchusedwasnotanexactlinesearch,
thestepsinthesteepestdescentalgoirthmwerenotperfectlyperpedicular.
10
5.4
Quasi-NewtonMethods
TheQuasi-NewtonMethodconvergedinthefewestiterationsascomparedtothe
othertwomethods. Theteamalsocheckedtoseeifthisalgorithmdemonstrated
quadraticconvergence. Analgorithmissaidtoconvergequadtraticallyifthere
existssome δ< ∞ where:
lim
k →∞
f ( x k +1 ) s f ( x _ )
≤ δ
2
( f ( x k ) s f ( x _ ))
_)
Although Newton and Quasi-Newton methods often times converge quadratically, this algorithm did not. This is partially because the Hessian was approximatedusingnumericalapproximationsofthegradients. Thisintroduces
errorintotheHessian,whichwasalreadyanapproximation. Nevertheless,itis
significanttonoticethattheBFGSconvergedquicklyonasolution.
5.5
FirstOrderMethods
Outofallthreealgorithms,theFirstOrderMethodconvergedonthegreatest
function value. Fig. 3clearly shows that the First Order Method could have
continued to decrease the function value. The algorithm’s termination was
caused by the decreasing gradient. Since L is fixed, as the magnitude of the
gradientdecreases,sodoesthestepsize. Asthestepsizesdecrease,thechanges
betweenfunctionvaluesdecrease. Eventually,thechangesinthefunctionvalue
become so small the algorithm deems the path converged. Steepest Descent
Methodsdonothavethisissuebecausethestepsizeisvariable.
On the other hand, the First Order Method is the most stable method.
TheFirstOrderMethodhasguaranteedconvergeceinaconvexregion. This
isevidincedbythefactthattheconvergencepathoftheFirstOrderMethod
washeading directly towards the minimum. SteepestDescent algorithms, althoughtypicallyrobust,cannotfullyboastguaranteedconvergencewhenusing
anin-exactlinesearch. SinceNewtonandQuasi-Newtonmethodsalsouseline
searches,thesamecanbesaidaboutthesemethods.
Althoughtherearetoofewdatapointstomakeadefinitiveasserstion,Fig.5
suggeststhattheFirstOrderalgorithmhasanearlinearconvergence. Itisalso
interestingthattheFirstOrderpathseemedtofindandfollowaneigenvector
ofthesystem. Thisissignificantbecauselinesearchalgorithmsperformvery
well when aligned with the eigenvector. This lead to the development of the
AugmentedFirstOrderMethod
5.6
AugmentedFirstOrderMethod
In order to create a more e_cent algorithm for solving the posed problem,
a hybrid algorithm was developed that combined First Order Methods with
SteepestDescentMethods. SincetheFirstOrderMethodwase_ectiveatfinding
aneigenvector,thealgorithmstartsthesamewayastheFirstOrderMethod.
WhentheFirstOrderMethodhasconvergedonatrough,thealgorithmswitchs
11
Figure5: LinearConvergenceofFirstOrderMethod
toalinesearch,whichrapidlyfollowsthetroughdowntowardstheminimum.
Theswtichingcriteriaforthealgorithmwasderivedfromthedotproduct:
u• v
cosθ =
kukkvk
)
Thedotproductisusedinthealgorithmtomeasuretheanglebetweenthelast
stepandthecurrentdescentdirection. WhenEq._)holdstrue,thedescent
directionise_ectivelylinedupwiththepreviousdirectionoftravel,indicating
alinesearchwouldbeappropriate:
k
k
dk • ( x k s x k 1 ) >c kdk kkx k s x k 1 k
_)
where 0 <c<
1. The value of c determines the range of θ that allows the
algorithmtousealinesearchinsteadoftheFirstOrderMethod. Inpractice,
c =0 .97 producedsatisfactoryresults. Thisvalueof c makestheallowableangle
deviationapproximitely15degrees. AcomparisonoftheAugmentedFirstOrder
Algorithm, the Steepest Descent Algorithm, and the First Order Method are
seeninFig.6. Aftermultipletests,theAugmentedFirstOrderMethodtypically
convergedonasolutionin30%lessiterationsthanthesteepestdescentorfirst
ordermethod. TheaglorithmcanbeseeninAlgorithm5.
6
Remarks
• Thecasesanalyzedinthispaperweresimplecasesofamuchmoregeneral
problem set. However, the results suggest that gradient-based methods
12
Figure6: ComparisonoftheDi_erentSearchAlgorithms
Algorithm5 AugmentedFirstOrderMethod
Given x 0
L = L0
. Theusermustprovideaguessfor
L0
k
0
repeat
dk := sr f ( x k )
if dk • ( x k s x k k 1 ) >c kdk kkx k s x k k 1 k then
. 0 <c< 1
α =argmin α f ( x k + αd k )
x k +1
x k + αd k
else
1
x k +1
x k s←r
f (xk )
L
k +1
k
if f ( x
) >f ( x ) + r f ( x kT) ( x k +1 s x k +1 ) + L2 ky s x k2 then
L =2 L
x k +1
xk
. Thisrerunstheiterationwiththenewvaluefor
L
endif
endif
k
k+ 1
until dk =0
13
shouldbeconsideredwhenforoptimalfiltertuning.
• Thecostfunctionconsideredisnotvalidforalltuningsituations. Certain
situationsmightrequirethefiltertobemorerobusttoverynoisyobservations. Therefore, minimizing over the RMSE would not be the ideal
case.
• When optimizing over a data set, generally a bound on the converged
covariancefromtheKalmanfilterwouldbeset. Thiswouldprovideadditionalconstraintstoconsider.
• TheanalysisinthispaperinvolvedtherapidsimulationofaKalmanfilter
andamodifiedKalmanfilterovernumeroustracksatvariouslengths. For
slowerfilters(i.e. theparticlefilter)orforextremelylargedatasets,this
processwouldnotbetractable.
• Despitemultipleevaluationsofthefiltersperalgorithm,theoptimization
methodwasmuchfasterthanagriddingscheme.
7
Conclusion
It is important to note that the goal of this publication was not to tune a
specificKalmanfilterormodifiedKalmanfilter. Instead,theobjectivewasto
proveitispossibletousegradientmethodsonafunctionthatisnotnecessary
convex in order to optimize a given filter. The steepest descent method, the
BFGSalgorithm,andthefirstordermethodswereallabletofindanoptimum
ofthegivenfunction. Throughaconvexcombination ofthesteepestdescent
algorithmandfirstorderalgorithm,itwasevenpossibletodecreasethenumber
ofiterationsrequiredtooptimizethefilter,whichbecomessignificantifcalling
the function to return a value is time consuming.
The application used in
thispublicationwasparticularlyuniqueinthatthesearchspacewasonlytwo
dimensional. Thisallowedtheplottingofthesearchspacetoensurethesearch
space had the proper curvature to allow optimization. In higher dimensions
itwouldnotbepossibletographtheprobleminthismanner, meaningthere
is no guaruntee algorithm convergence or global minima. Nevertheless, this
publication provides the proof of cocept for gradient-based optimization of a
filter.
Appendices
A
Filters
ThissectionoutlinestheKalmanfilterandthemodifiedKalmanfilterandhow
theyareappliedtotheproblemdefinedinSection2.
14
A.1
KalmanFilter
IfthestatedynamicsandmeasurementsareproperlymodeledbyEqs.C)toG),
thenaKalmanfilterprovidesanexactsolutionforestimatingtheposteriorstate
distribution[16]. Theupdatedstateestimatewouldbeadistributionwithmean
andcovariance:
xˆ k = F xˆ k k 1 + K k ( zk s HF xˆ k k 1 )
Pˆ k =( I s KH
)( FPˆ k k 1 F T + Q)
k
_)
“)
where K k istheKalmangaingivenby
K k =( FPˆ k k 1 F T + Q) H TS
k1
k
”)
and Sk istheinnovationcovariancegivenby
Sk = H ( FPˆ k k 1 F T + Q) H T + R
“)
Theinitialmeanandcovariance,^ x 0 and Pˆ 0 ,canbecalculatedbasedonthefirst
measurementorsetapriori.
Without quantization, the observation noise covariance would be set to a
valuebasedonknowledgeoferrorinaircrafttransponders;however,sincethe
observationsarequantized, thecovarianceneedstobehandleddi_erently. A
common approach to handling measurement quantization is to use the Sheppardcorrection[3]. Thisapproachtreatsthemeasurementsasbeinguniformly
distributedovertheinterval( s Δ / 2, Δ / 2),whichresultsin R =Δ 2 / 12.
A.2
KalmanFilterModifiedforQuantizedMeasurements
Curryetal. presentanapproachforquantizedmeasurements[11]. Insteadof
computingE[ x |z]usingEq._)liketheKF,theycompute
E[x |z 2 A ]=E[E[ x |z]|z 2 A ]
”)
where A representsaquantizationinterval. Themeanandcovariancearegiven
by
E[x k |zk 2 A k ]=ˉ x k + K k (E[ zk |zk 2 A k ] s H xˉ k )
cov[x k |zk 2 A k ]= Pˆ k + K k cov[zk |zk 2 A k ]K kT
_)
_)
where x_k = F xˆ k k 1 . Equation _) is equivalent to Eq. _) and Eq. _) is
equivalenttoEq.“)ifthemeasurementsarenotquantized. Eq._)shows
that the quantization is treated as if it were uncertainty added after a linear
measurementhadbeenprocessed. Theuncertaintyinducedbythequantization
isaddedto Pˆ k ,whichistheminimumcovarianceifthemeasurementsarenot
quantized.
Duan,Jilkov,andLipresentane_cientmethodofnumericallycalculating
E[z|z 2 A ]andcov[ z|z 2 A ][17]. Theirapproachusesagridtoapproximatethe
15
P ( H x k | z 0: k )
A
zk
H xˉ k
Hx
k
Figure7: IllustrationofthenumericalprocessusedtocalculateE[
z|z 2 A ].
H k xˉk , as adiscrete
probabilitydensityfunction of thepredicted observation,
probabilitymass function. In the problem of vertical state estimation,
z is a
scalar. Applyingthistechniquetothisproblem,theequationsbecome
L
E[zk |zk 2 A k ] ≈ X pg
i i
_)
i =1
L
2
2
cov[zk |zk 2 A k ] ≈ X pg
i i s (E[ zk |zk 2 A k ])
–)
i =1
where pi istheprobability associated with the gridpoint
gi usedinapproximatingthedensityfunctionasthemassfunction,and
L isthenumberofgrid
points. AnillustrationoftheprocessisshowninFig.7. Theinitialdistribution
isGaussianwithmean H xˉ k andcovariance Sk . Thedensityfunctionisthen
L discrete probabilities are caltruncated by the quantization bin. A set of
culated,transformingtheprobabilitydensityfunctionintoaprobabilitymass
function.
References
[1] R. E. Kalman, “A new approach to linear filtering and prediction problems,” TransactionsoftheASME.SeriesD,JournalofBasicEngineering
vol.82D,pp.35e,1960.
[2] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian track16
,
ing,” IEEE Trans. Signal Processing , vol. 50, no. 2, pp. 174ˆ, 2002.
doi:10.1109/78.978374.
[3] T.Soderstrom, Discrete-time Stochastic Systems
. Springer-Verlag,2002.
[4] E. Sviestins and T. Wigren,
“Nonlinear techniques for Mode C
climb/descentrateestimationinATCsystems,”
IEEE Trans. Contr. Syst.
Technol.,vol.9,no.1,pp.163|,2001.doi:10.1109/87.896757.
[5] A.Gelb, Applied Optimal Estimation .
MITPress,1974.
[6] T.D.Powell, “Automatedtuningofanextendedkalmanfilterusingthe
downhillsimplexalgorithm,” JournalofGuidance,Control,andDynamics
vol.25,no.5,pp.901H,September-October2002.
,
[7] O.V.Korniyenko,M.S.Sharawi,andD.N.Aloi,“Neuralnetworkbased
approachfortuningkalmanfilter,”vol.25,no.5,2005,pp.1-.
[8] C.GoodallandN.El-Sheimy,“Intellegienttuningofakalmanfilterusing
low-costmemsinertialsensors.”
[9] B.M.Akesson,J.B.Jorgensen,N.K.Poulsen, andS.B.Jorgensen, “A
toolforkalmanfiltertuning,”in
17th European Symposium on Computer
Aided Process Engineering - ESCAPE17,2007.
[10] M.Saha,B.Goswami,andR.Ghosh, TwoNovelCostsforDeterminingthe
Tuning Parameters of the Kalman Filter ,DepartmentofInstrumentation
andElectronicsEngineeringStd.
[11] R.E.Curry,W.E.V.Velde,andJ.E.Potter,“Nonlinearestimationwith
quantizedmeasurements-PCM,predictivequantization,anddatacompression,” IEEE Transactions on Information Theory ,vol.16B),pp.152q,
1970.
[12] J.W.Andrews,“Animprovedtechniqueforaltitudetrackingofaircraft,”
MassachusettsInstituteofTechnology,LincolnLaboratory,ProjectReport
ATC-105,Mar.1981.
[13] S.R.Hall,“Principlesofoptimalcontrol,”Slides,February2012.
[14] M.J.Kochenderfer,M.W.M.Edwards,L.P.Espindle,J.K.Kuchar,and
J. D. Gri_th, “Airspace encounter models for estimating collision risk,”
Journal of Guidance, Control, and Dynamics ,vol.33,no.2,2010.
[15] R.M.Freund,“Thesteepestdescentalgorithmforunconstrainedoptimizationandabisectionline-searchmethod,”February2012.
[16] G.M.Siouris, AnEngineeringApproachtoOptimalControlandEstimation
Theory. JohnWiley&Sons,Inc.,1996.
17
[17] Z.Duan,V.P.Jilkov,andX.R.Li,“Stateestimationwithquantizedmeasurements: ApproximateMMSEapproach,”in Proc. 11th Int Information
Fusion Conf,2008,pp.1®.
18