Model-Based Trajecory Reconsrucion using IMM Soohing and Moion Paern Idenificaion Jesús García Copuer Science Deparen-GIAA Universidad Carlos III de Madrid Colenarejo, Spain jgherrer@inf.uc3.es José M. Molina Copuer Science Deparen-GIAA Universidad Carlos III de Madrid Colenarejo, Spain olina@ia.uc3.es Juan A. Besada Signal Syses and Radio-counicaions Deparen-GPDS Universidad Poliécnica de Madrid, Spain besada@grpss.ssr.up.es Gonzalo de Miguel Signal Syses and Radio-counicaions Deparen-GPDS Universidad Poliécnica de Madrid, Spain gonzalo@grpss.ssr.up.es Absrac - This wor addresses off-line accurae rajecory reconsrucion for Air Traffic Conrol. We propose he use of specific dynaic odels afer idenificaion of regular oion paerns. Daases recorded fro opporuniy raffic are firs segened in oion segens, based on he ode probabiliies of an IMM filer. Then, reconsrucion is applied wih an opial soohing filer operaing forward and bacward. The paraeers describing he specific odes are esiaed and hen used as eernal inpu for soohing filers. The perforance of his approach is copared wih a ehod based on inerpolaion B- splines. Coparaive resuls on siulaed and real daa are discussed a he end. Keywords: Trajecory reconsrucion, daa soohing, air raffic conrol, real-daa validaion. Inroducion Daa-processing syses operaing in criical applicaions such as Air Traffic Conrol (ATC) [,] require fro validaion wih real daa. Perforance assessen of ATC cenres by eans of recorded daases (also naed opporuniy raffic) requires as a previous sep he reconsrucion of rajecories. This reconsrucion process provides he unnown ground ruh o be used as reference in he evaluaion. However, boh daa processing and rajecory reconsrucion can be considered as esiaion probles. The only difference is ha ATC processors operae in real ie (daa is processed in a single forward sequence as easureens becoe available), while he reconsrucion, or soohing, is a bach process which aes use of all available daa. Therefore, he reconsrucion perfored off-line over recorded daa files can be forulaed as a special ulisensor fusion process. The paricular aspec is he advanage of nowledge abou boh pas and fuure arge posiion repors o iprove he perforance of classificaion and esiaion algorihs. A heoreically opial approach consiss in a double racing loop in he forward and bacward direcions for soohing [3,4]. The use of uliple-odel o his end has been also proposed in [5]. Furherore, daa soohing has been acled fro ohers poins of view. Basically, one ay eiher choose o creae a paraeric odel or a nonparaeric odel for fiing rajecory daa. In non-paraeric odels, a free equaion such as a polynoial curve or a fied neural newor archiecure is seleced as basic shape of he odel curve. Then he curve is opiized for soohness, while iniizing error agains he sapled daa poins in boh posiion and velociy. However, if a paraeric represenaion is a priori nown for he oion being odelled (parabolic oion for a falling objec, coordinaed urn for regular fligh oion, ec.), hen well esablished esiaion ehods ay be used. Splines are he os used ehods for non-paraeric daa fiing, in cases were accurae odels of he oion are no available, paricularly when odelling huan or errain robos oveen. Following he second forulaion, in his wor we propose he use of IMM racing filers o his end, Model-Based Reconsrucion (MBR), eploiing physical oion odels usually perfored by aircraf flying in conrolled airspace. I is based on he segenaion of rajecory in regular oion segens : Unifor oion, Transversal anoeuvre, Longiudinal anoeuvre, Cobined anoeuvre. Then, an accurae reconsrucion can be perfored now using rajecory inerpolaion accordingly o he odels idenified in he fligh. In his sudy we develop a reconsrucion archiecure based on his ideas and copare wih an nonparaeeric ehod as previously used in ATC [,]. Coparaive resuls on siulaed and real daa are discussed o copare boh approaches in his specific doain. Ne secion reviews basis of spline approiaion and secion 3 presens he proposed srucure o process available daa and generae reconsrucion. Secion 4 presens he resuls obained wih siulaed and real Funded by projecs CICYT TSI5-7344, CICYT TEC5-786 and CAM MADRINET S- 55/TIC/55
rajecories wih boh approaches, and soe conclusions are suarized a he end. Splines for fiing daa Splines have nuerous applicaions in he doain of processing daa recorded fro he oion of physical syses [,6,7] and in pah planning for roboics fields [8,9]. One specially proising applicaion of daa soohing is he esiaion of a bes-fi or os-liely perforance fro uliple eaples of a given as or oion, specially for characerizing huan oion. They are basically piecewise-polynoial curves for daa fiing. An eac fi would involve inerpolaion while an approiae fi involve leas-squares approiaion or soohing. The usual forulaion is a variaional approach, he funcion wih salles -h derivaive aong all hose aching prescribed funcion values a cerain sies. The soohing B-spline aes he given daa se ( i, i ), wih i in inerval [a,b], and posiive weighs for each daa w i, and a general soohing paraeer p, o iniize: b d f () p w i i f ( i ) + ( p) d () a i d The soohing spline f( i ) is a spline of order. The soohing paraeer, p, is adjused wih a anual process o achieve an appropriae balance beween low residual error easure, w i i f ( i ) and low roughness easure, b d f () d a d In rajecory reconsrucion applicaions, he independen variable of daase is usually ie, wih nos disribued along he easureen ies i{,,,, N }. 3 Syse Srucure for Model-based Reconsrucion As a firs sep for reconsrucion, all easureens are epressed in he sae coordinaes frae and iner-sensor syseaic errors correced, providing ie-space aligned easureens and covariance arices. Then, sensor daa is associaed in rajecories, aing use of inforaion available in ATC cone: SSR/ModeS/ICAO code, ieposiion copaibiliy, velociy copaibiliy, ec. Deails are no included here abou he associaion and regisraion processes, hey naurally also ae advanage of he bach processing condiions o aiize heir perforance. 3. Archiecure The reconsrucion is based on rajecory segenaion in differen ypes of oion. Figure suarizes he process. Once he oion segens are idenified and validaed, reconsruced rajecory is inerpolaed in each segen using filering odels ached o he oion segens using he paraeers describing each segen. Boh he idenificaion of segens and inerpolaion are i based on double racing loops in he forward and bacward direcions wih appropriae dynaic odels for predefined siuaions. Moion Segenaion Preprocessed Daa Cl assif ied Segen s Paraeers Esiaion & Validaion Forward- Bacward IMM Reconsrucion Segen s Parae ers Reconsruced Saples Figure. Bloc diagra of reconsrucion process The idenificaion of oion ype generaes ie inervals conaining uli-sensor daa corresponding o differen ypes of oion: unifor oion, urns, and longiudinal aneuvers. I is based on a se of ached Kalan filers o esiae he probabiliy of arge flying accordingly o he differen odes defined. This classificaion proble has been previously sudied by auhors using alernaive saisical and achine-learning approaches [, ]. Afer oion-paern idenificaion, differen racing odels specialized in recilinear segens and nonunifor oion provide he esiaion of rajecories. The soohing is achieved hrough forward and bacward runs, aing advanage of available pas and fuure easureens. They inerpolae, using IMM forwardbacward filers, he sae vecors corresponding o available easureens, aing also ino accoun he aneuvering paraeers describing he ean values along he segen, bu adaped o he specific condiions of he daa segen. This soluion iposes coninuiy condiions in speed and posiion wih adjacen segens, allows a high soohing o filer ou noise, and also adapaion o dynaic condiions of aneuvers (for insance, he ransversal acceleraion or longiudinal acceleraion ay change along a urn rajecory). The reconsruced rajecory is buil wih a se of reconsrucion saples, conaining posiion cineaic esiaes, ogeher wih he qualiy descripion hrough covariance arices (error ellipses and rajecory envelop). The nuber of saples foring he reconsrucion is chosen will inerpolae all 3D posiions wih ie sapling o aain he accuracy requireen, using a sapling of arge repors chosen wih a crierion
of liied linear inerpolaion error. Any inerediae ie is reconsruced wih a linear inerpolaion. 3. IMM Soohing Filer The IMM soohing filer for his applicaion is coposed of hree Kalan filers ached o unifor oion, ransversal and longiudinal anoeuvres (Figure ). The hree Kalan filers operae in parallel, each one ached o a specific ype of oion (paraeers in plan noise characerisics). Mode ransiions Repors [], y [] z - ˆ P [ ] ˆ [ ] ˆ 3 [ ] [ ] P [ ] P [ ] ˆ P Unif. Moion Ineracion/Cobinaion [ ] [ ] ˆ [ ] ˆ 3[ ] P [ ] P [ ] Turn Moion 3 3 CA Moion µ µ µ 3 [ ], [ ], [ ] z - Turn paras CA paras Λ [ ] ˆ P [ ] [ ] Λ [] Λ [ ] [] ˆ 3 [ ] [ ] P [ ] ˆ P 3 3 µ [] [] Mode probabiliy copuaion µ 3 Mode Cobinaion for oupu ˆ [ ] P[ ] Figure. IMM paraeric reconsrucion filer All odes in his srucure share a coon sae represenaion and dynaic odel, siplified o have saes (posiion and velociy) in D sereographic plane and consan-velociy predicion. The sae vecor is arranged as: [] [ ˆ[] vˆ ŷ[] vˆ ] [] y[] and is corresponding 44 error covariance ari. The difference aong he hree odes is in he predicion odels. Besides he unifor-oion odel, he wo anoeuvring odes predic he aircraf sae aing use of he paraeers copued for heir segens. For insance, in he case of urn odel, a circular predicion odel is applied. The paraeers for circle radius and cenre are copued wih a leas square algorih and, if validaed, used for predicion (see Figure 3). Oherwise, i uses a linear erapolaion and a wide covariance odel o avoid degradaion. I also represens a D sae (posiion and velociy), copleened wih he aneuvers paraeers. Besides, he inforaion abou oion inervals is used in he srucure. The ransiion probabiliies are odified in he inervals close o edges, and he plan noises are also increased in he presence of close ransiions. Therefore, all odes eploi inforaion abou he paraeers used in reconsrucion and also if hey are applicable or no Y Circle radio R θ Circle cenre ( C, y C ) Prediced vecor ( p (), y p ()) (v p (), vy p ()) Las updae ( f (), y f ()) (v f (), vy f ()) Figure 3. Curvilinear predicion for urn reconsrucion 3.3 Paraeer esiaion in idenified odes A Bes Linear Unbiased Esiaor (BLUE), he Weighed Leas Squares, is used o esiae he paraeers describing aneuvering segens. In he case of consanacceleraion oion (CA) in Caresian coordinaes, he si X
paraeers (X and Y posiions, velociies, and acceleraions a iniial segen ie) are esiaed wih weighed leas squared. The relaion beween each easure and he segen paraeers is: () y n n y () () () H( () )θ + n() v a + y vy ay ( ) Where ( [], y []) is he -h easure, epressed in Caresian coordinae, is he associaed ie inerval beween easureen ie and segen iniiaion, (,y ) is he iniial segen posiion, (v,vy ) is he iniial velociy vecor, and (a,ay ) is he iniial acceleraion vecor epressed in Caresian coordinaes. Meanwhile, (n [], n y []), are he easureen noises for his easure, projeced ino Caresian coordinaes. n( ) associaed covariance is R. Wih his odel in ind, assuing he easures follow he described paern, he segen paraeers are opially esiaed using all seleced easures using he following epression: θ [ v a y vy ay ] H( ) T R H( ) H( ) T R [] ( 3) Regarding he urn anoeuvre, a linear regression of he circle is also perfored, wih he ransfored paraeers as follows. If we assue all easures follow he sae circle, we have: [] + y [] + a [] + by [] + c (4) And aing a inor change in he equaion, we have: ( [] y []) a [] + by [] + c + (5) Arranging all easure equaions in ari for, we would have: ( [] + y [] ) ( [] + y []) [] y [] a [] y [] b............ c ( ) [N] y [N] [N] + y [N] (6) If we renae he arices as: [ b c] Y X a So, hese paraeers could be esiaed perforing a regression, using he pseudoinverse of X, as: T - T [ a b c ] ( X X) X Y Wih hese hree esiaes we obain inerpolaing circle paraeers (cenre posiion Ĉ and radius Rˆ ): c Ĉ c a b Rˆ - c + + 4 4 Finally, he error of reconsruced segens wih available paraeers is copued o validae he segen generaed before applying reconsrucion, in order o avoid errors: esegen + X Y a - b - ˆ[] in erp[] ˆ[] in erp[] R ŷ[] y [] ŷ[] y [] in erp in erp (9) The soohing IMM generaes he saples wihin each segen, wih an adapable rae. Alhough he saples correspond o easureen ies, he sapling separaion is se depending on he ype of anoeuvre. In unifor oion, he reconsruced rajecory can be direcly inerpolaed wih a linear funcion beween saples. In order o save space in his special case of unifor oion (which i is he os frequen siuaion), a sub sapling of reconsruced ies is carried ou, aing a predefined iniu separaion. Regarding anoeuvres, an inerpolaion is done aing use of available paraeers, wih a aiu separaion o avoid error due o linear inerpolaion. 4 Resuls The reconsrucion process presened in previous secion was copared wih he sooher available in MATLAB oolbo for B-splines. The coparisons were done in four siuaions, wo siulaed rajecories and wo real rajecories. 4. Siulaed Trajecories Siulaed rajecories were generaed o copare he approaches in represenaive siuaions agains he ideal values. Represenaive speed, acceleraion agniudes and disances o radar were used o siulae hypoheical siuaions. Two rajecories were siulaed in a scenario wih four radars wih scan period of seconds and noises in easured range, aziuh, given by zero-ean Gaussian variables wih sandard deviaions: σ r, σ θ.9º: (7) (8)
A race rac wih urns of acceleraion of.5 /s, speed of 5 /s and disance K o closes radar. In he raceracs, segens of unifor oion las seconds, and urns 3 seconds, approiaely. A hree-segen rajecory wih speed 5 /s, urn of 9º wih acceleraions of 4 /s, a K fro closes radar The resuls for siulaed rajecories appear in figures 4-. The rajecories are depiced firs, deailing soe segen o illusrae he easureens and inerpolaed curves. We can noice an apparen B-spline inerpolaion effec in soe segens, Figs. 5,7, where reconsruced curve deviae fro real sraigh oion. Besides, he effec of anoeuvres over b-splines is a bias since i goes hrough close easureens insead following he real urn. So, he long-er reconsrucion based on odels describing coplee inervals allows a higher accuracy boh in unifor segens and deailed reconsrucion during aneuvers. This is clear apparen looing o figures 9,, wih he horizonal error wih respec o ideal siulaed rajecory. Y (eers) 5 6.85 6.845 6.84 6.835 6.83 6.85 6.8 6.85 6.8 6.85 6.7 6.8 6.9 6. 6. 6. 6.3 X (eers) 5 Figure 6. Siulaed racerac (deail). (o plos, -- b- spline, +- MBR,.- ideal).5 5 7. 7 5 Y (eers) 9.5 9 Y (eers) 6.8 6.6 6.4 8.5 8.5..5..5 X (eers) 6 Figure 7. Siulaed siple urn wih 4 /s acceleraion 5 6. 9.63 5.8 6 6. 6.4 6.6 X (eers) 5 Figure 4. Siulaed racerac (.5 /s acceleraion) 5 6.4 Y (eers) 9.6 9.6 9.6 6.38 9.59 6.36 9.58 Y (eers) 6.34 6.3....3.4.5.6.7 X (eers) 6 Figure 8. Siulaed siple urn (deail) (o plos, -- b- spline, +- MBR,.- ideal) 6.3 6.8 5.76 5.78 5.8 5.8 5.84 5.86 5.88 X (eers) 5 Figure 5. Siulaed racerac (deail). (o plos, -- b- spline, +- MBR,.- ideal)
Y (eers).48.475.47.465.46.455.45.445.44 6.5.3.35.4.45.5.55.6.65.7 X (eers) 6 Figure 9: Siulaed siple urn (deail) (o plos, -- b- spline, +- MBR,.- ideal) 35 3 achieve his end, a rando selecion of 5% of original recorded daa was perfored o divide he original se ino he raining and validaion ses. Training se was used o reconsruc he inerpolaion rajecory, and validaion se o assess he deviaions. Figures -5 presen he rajecories corresponding o real daases, deailing he wo ses of daa used for raining and esing. The evaluaion hrough noralized residual (averaged in a 5s window) is presened in figures 6, 7. I can be noiced a sligh advanage of MBR in firs rajecory, and ore significan in he second one. The firs case had noisier daa, wih presence of uliple unclassified oion segens, reducing herefore he soohing capabiliy of MBR. -3-3.5 5 reconsrucion error (eers) 5 5 5 Y (eers) -4-4.5-5 -5.5-6 -6.5 44 46 48 5 5 54 56 58 6 6 ie (seconds) Figure 9: Horizonal error wih siulaed racerac (- MBR, --spline) 5-6.5-6.5-6 -5.5-5 -4.5-4 -3.5-3 -.5 - -.5 X (eers) 5 Figure. Real Trajecory 5 45 4-6.5-6.54-6.56 reconsrucion error (eers) 35 3 5 5 Y (eers) -6.58-6.6-6.6-6.64-6.66 5 3 4 5 6 7 8 9 ie (seconds) Figure : Horizonal error wih siulaed siple urn (- MBR, --spline) 4. Real rajecories The evaluaion of rajecories reconsruced o fi real daases has he proble ha no reference is available o copare he errors. To solve his proble, boh odels were evaluaed on wo real scenarios, applying wo ypes of daa ses for each one. In he firs place, he sae daa se used for reconsrucion was used o copare easureens and reconsrucion (residuals). This naurally produced an opiisic assessen, since he sae daa used for reconsrucion was used for esing. Secondly, an independen daa se, no used for reconsrucion, was used o asses he residuals. In order o -.4 -. - -.98 -.96 -.94 -.9 -.9 -.88 -.86 -.84 X (eers) 5 Figure. Real rajecory (deail of racerac)
5 6 Y (eers) -6.5-6.55-6.5-6.55 noralized reconsrucion residual 4 8 6 4-6.5 -.985 -.98 -.975 -.97 -.965 -.96 -.955 X (eers) 5 Figure 3. Real rajecory (deail) (o raining plos, validaion plos, -- b-spline, +- MBR) 5 5 5 3 35 4 45 ie (seconds) Figure 6. Noralized residual wih rajecory (--spline, -MBR) 4 Y (eers) 4 - -4-6 -8 - - -4-6 noralized reconsrucion residual 4.5 4 3.5 3.5.5 Y (eers) 4-3.45-3.5-3.55-3.6-3.65-3.5-3 -.5 - -.5 - X (eers) 5 Figure 4. Real rajecory 3.36 3.38 3.4 3.4 3.44 3.46 3.48 3.5 ie (seconds) 4 Figure 7. Noralized residual wih rajecory (--spline, -MBR) The resuls of evaluaion wih real daa are suarized in able. The reduced residual is apparen (boh wih absolue and noralized values). The effec of overfiing is also slighly iproved wih MBR, wih an inferior degradaion when reconsruced curve is esed agains validaion daa. Finally, he nuber of reconsrucion saples is reduced, aing advanage of he presence of unifor segens. -.765 -.76 -.755 -.75 -.745 -.74 X (eers) 5 Figure 5. Real rajecory (deail) (o raining plos, validaion plos, -- b-spline, +- MBR) Table. Resuls wih real rajecories
MBR in Scenario (raining se) MBR in Scenario (validaion se) Spline in Scenario (raining se) Spline in Scenario (validaion se) MBR in Scenario (raining se) MBR in Scenario (validaion se) Spline in Scenario (raining se) Spline in Scenario (validaion se) Nor. Residual Average Nor. Residual σ Absolue Residual () Average Absolue Residual () σ Nuber of Reconsrucion Saples 4.739 5.675 3.9 3.84 83 5.863 6.89 38.3589.73 97 5.457 6.83 8.9645 4.746 635 7.957 8.359 4.9457.8897 36.79 3.794 94.5 9.8534 74.9 3.9649 8.348 47.49 583.8538 3.9 9.5866 86.578 69.7647 4.569 5.7685 5.34 853 5 Conclusions An accurae approach for rajecory reconsrucion has been proposed in his wor aing advanage of he presence of regular oion paerns in ATC flighs. Is perforance was copared wih a convenional daa fiing process based on splines. Superior resuls were obained boh in siulaed and real represenaive scenarios. Furher research will eplore he opiizaion of paraeers configuraion of boh approaches and oher realisic effecs (uncalibraion, ouliers, irregular segens, ec.) in operaing condiions. References [] Kirubarajan T., Bar-Shalo, Y. Precision large scale air raffic surveillance using IMM/assignen esiaors. IEEE T. on AES. Jan. 999. Vol: 35, Iss.. [] Desond-Kennedy, A. Gardner, B. Tools for analysing he perforance of ATC surveillance radars. Specifying and Measuring Perforance of Modern Radar Syses (Ref. No. 998/), IEE Colloquiu on 6 March 998. [3] Arhur Gelb. Applied Opial Esiaion. The MIT Press, 974. [4] Ogle, T.L., Blair, W.D. Fied-lag alpha-bea filer for arge rajecory soohing. IEEE T. on AES. Oc. 4 Vol. 4, Iss. 4. [6] Lee, C.; Yangsheng Xu; Trajecory fiing wih soohing splines using velociy inforaion. ICRA '. IEEE Inernaional Conference on Roboics and Auoaion,. Volue 3, 4-8 April. [7] Lee, C.H.; A phase space spline sooher for fiing rajecories IEEE Transacions on Syses, Man and Cyberneics, Par B. Volue 34, Issue, Feb. 4. [8] Qiang Huang; Kajia, S.; Koyachi, N.; Kaneo, K.; Yooi, K.; Arai, H.; Kooriya, K.; Tanie, K.; A high sabiliy, sooh waling paern for a biped robo. IEEE Inernaional Conference on Roboics and Auoaion, 999. Proceedings. 999. [9] Taahashi, S.; Marin, C.F.; Opial conrol heoreic splines and is applicaion o obile robo. Proceedings of he 4 IEEE Inernaional Conference on Conrol Applicaions, 4. Sep. 4. [] Garcia, J.; Perez Concha, O.; Molina, J.M.; de Miguel, G.; Trajecory classificaion based on achinelearning echniques over racing daa. 9h Inernaional Conference on Inforaion Fusion. Florence, Ialy. July 6. [] Perez, O.; Garcia, J.; Molina, J.M.; "Neuro-fuzzy Learning Applied o Iprove he Trajecory Reconsrucion Proble". Inernaional Conference con Copuaional Inelligence for Modelling, Conrol and Auoaion, Nov. 6. [5] R. E. Helic, W. D. Blair, and S. A. Hoffan, Fied-Inerval Soohing for Marovian Swiching Syses, IEEE. Trans. Inforaion Theory, IT- 4(6):845 855, Nov. 995.