IMPROVED VEHICLE PARAMETER ESTIMATION USING SENSOR FUSION BY KALMAN FILTERING

Transcription

1 XIX IMEKO World Cogress Fudameal ad pplied Merology Sepember 6, 009, Lisbo, Porugal IMPROVED VEHICLE PRMETER ESTIMTION USING SENSOR FUSION Y KLMN FILTERING Eri Seimez, Rage Emardso, Per Jarlemar 3 SP Techical Research Isiue of Swede, orås, Swede, esei@ee.chalmers.se SP Techical Research Isiue of Swede, orås, Swede, rage.emardso@sp.se 3 SP Techical Research Isiue of Swede, orås, Swede, per.jarlemar@sp.se bsrac Wihi several applicaios cocerig he improveme of vehicle safey, accurae sysems for deermiaio of posiio, velociy ad acceleraio are useful. We prese a sysem for accurae deermiaio of hese parameers usig a sesor fusio echique. The mai focus is o how GPS carrier phase daa ad acceleromeer daa are modeled ad iegraed i a Kalma filer ha provides boh esimaes ad accompayig uceraiies. Keywords: Sesor fusio, Kalma filer, Global posiioig sysem (GPS). INTRODUCTION ccurae iformaio abou posiio, velociy ad acceleraio is useful for several applicaios wih he aim of improvig vehicle safey. I for example advaced oudoor crash ess, sudies of wid loads o cars ad driver behaviour owledge abou posiio, velociy ad acceleraio a high samplig raes are impora. We prese a sysem for accurae deermiaio of posiio, velociy ad acceleraio of a vehicle, by usig a sesor fusio echique for combiig GPS carrier phase echology wih acceleromeers o achieve a icreased ime resoluio. The mai focus i he paper is o how he GPS carrier phase daa ad acceleromeer daa are iegraed i a muli rae Kalma filer ad how he sysem is modeled as well as how process parameers are deermied.. STTE-OF-THE-RT GPS/INS INTEGRTION Kalma filer algorihms are widely used i he field of avigaio o iegrae differe avigaio sesors such as GPS ad Ierial Navigaio Sysems (INS) i order o exceed he performace of he idividual sesors. Smyh e al. [] shows by simulaios he advaages of combiig iformaio from displaceme sesors ad acceleromeers i dyamic sysem moiorig. The mehod provides improved esimaes of velociy ad displaceme, avoidig low frequecy oise amplificaio from he acceleromeers ad high frequecy oise amplificaio from he displaceme measuremes. Smyh e al. also brigs up he fac ha higher samplig raes are available wih acceleromeers ad ha his also ca be used o icrease he limied ime resoluio i GPS (Sae of he ar GPS samples wih a rae of 0 Hz). The maximum samplig rae of he measured acceleraio was se o 000 Hz i heir simulaios. Mos iegraed avigaio sysems are oday, as i [], loosely coupled [], which meas ha displaceme daa (i mos cases GPS posiio esimaes) are iegraed wih INS daa i he avigaio filer. However, ighly coupled sysems where raw GPS daa, i he form of pseudorages/dela-rages, isead of posiios, are direcly iegraed wih INS daa are ofe superior []. Such a algorihm is demosraed ad esed i [] ogeher wih a mehod where ime differeced GPS carrier phase measuremes are used o improve he accuracy of he velociy esimaes. Relaive GPS carrier phase measuremes ca be used o reach ceimeer accuracy i he esimaed posiios. Gao e al. [3, 4] preses a ceimeer level vehicular posiioig sysem, usig GPS/INS G-sesors/yaw rae sesors ad wheel speed sesors, wih focus o maiaiig accuracy durig GPS ouages. The sysem uses GPS carrier phase measuremes wih resolved ieger ambiguiies ad provides posiio, velociy ad aiude a a updae rae of 0 Hz. lso Kim e al. [5] preses simulaios from a complee GPS/INS iegraio algorihm wih GPS carrier phase measuremes. Ieger ambiguiy deermiaio ad cycle slip deecio ca be improved by usig INS iformaio. Kim e al. [5] suggess a INS aided ieger ambiguiy resoluio algorihm. Peovello e al. [6] also shows he advaages of usig INS iformaio. They prese a ulra-igh GPS/INS avigaio sraegy for ceimeer-level GPS carrier phase posiioig i wea sigal eviromes, by usig a sofware based receiver wih INS aided racig loops. This mehod provides a sesiiviy improveme i erms of posiio accuracy, ad hey sugges ha wih his echique

2 he RTK capabiliy could be expaded i wea sigal applicaios, wih difficulies o rac he carrier phase. This paper preses high precisio (ceimeer level) esimaio of posiio, velociy ad acceleraio for a movig vehicle wih a updae rae of 000 Hz. This is achieved by combiig GPS ad acceleromeer daa i a ighly coupled muli-rae Kalma filer algorihm. Relaive GPS carrier phase measuremes are used o achieve he obaied precisio i he posiio, velociy ad acceleraio. ϕ = ρ + N + f δ o i + ε λ y muliplyig () ad (3) wih he sigal wavelegh ad subracig hem, we obai a phase differece measureme: λ Δϕ = Δρ + N + cδ + Δl + ε (4) D D (3) 3. MESUREMENTS The developed sysem cosiss of acceleromeers ad GPS receivers. Two acceleromeers are moued i he vehicle perpedicular o each oher so ha oe acceleromeer measures he acceleraio i he drivig direcio ad he oher oe measures he acceleraio sideways. ooh acceleromeers measure he acceleraio wih a samplig rae of Hz. Due o he desig of he acceleromeers frequecy compoes below 0. Hz are o capured. The acceleromeer measuremes are he rasformed o he GPS coordiae sysem by muliplyig hem wih a rasformaio marix T, where θ is he agel bewee he wo coordiae sysems. simplificaios is here made of he coordiae sysems ad he acceleromeer daa is oly projeced io he horizoal compoes of he GPS coordiae sysem. T = cosθ siθ siθ cosθ X GPS = T X Y GPS Y The GPS equipme cosiss of a referece aea ad receiver posiioed a a ow locaio ad a aea ad a receiver moued o he vehicle, referred o as he rover. oh GPS receivers measure he received sigal phase a he respecive aea a wo differe frequecies, f ad f. The observed phase is sampled a a frequecy of 0 Hz. The phase measuremes from he rover ad he referece receiver ca be described by () ad (3), where ϕ is he measured phase i fracio of cycles, ρ is he geomerical disace bewee he receiver ad he saellie, N is he ieger umber of cycles referred o as he ambiguiy parameer. The δ represes he combied saellie ad receiver cloc error, l o is he error i he repored saellie posiio, l is he sigal delay i he lower par of he amosphere referred o as he roposphere, l i is he sigal delay i he ioosphere par of he amosphere, ad ε is measureme error. λ is he sigal wavelegh ad f is he sigal frequecy [7]. ϕ = ρ + N + f δ o i + ε λ () () I (4), we have assumed ha he orbial errors ad he ioospheric delay are approximaely ideical for he wo receivers, because he separaio is relaively small, ad hus cacel each oher. The coribuio from he roposphere, however, depeds o he heigh differece bewee he rover ad he referece saio as he sigals a he wo aeas experieces differe amous of roposphere. The mai par of he ropospheric delay ha remais ca be approximaed usig heigh differece iformaio as Δ 0 l = Δz χ m (5) where Δz is he heigh differece bewee he rover ad he referece. The parameer χ 0 is he refraciviy coefficie a he surface of he earh [7], ad m is a mappig fucio used o relae observaios i he zeih direcio o he direcio of he saellie. To achieve he ecessary iformaio abou he heigh differece Δz a prelimiary esimaio of he referece ad rover posiios are performed usig he less precise code observables. Sar values for he ieger parameer, N, i (4) are foud from evaluaig cadidaes ad choosig oe se of ieger values ha opimizes he mach bewee he models ad he measuremes. We use he code daa o fid a priori values of N i his evaluaio. The ieger values are expeced o remai cosa i ime. There are, however, isas whe a receiver emporarily loses he coiuous racig of a cerai saellie sigal while laer resume i. Uder such circumsaces, he correc value for N afer he brea differs a ieger umber from he previously chose value for his saellie. I order o deec hese cycle slips, he phase observables ad L a he wo frequecies, f ad f are compared a wo adjace pois i ime. The chage measured i uis of legh is expeced o be approximaely equal a he wo frequecies. We form he es ( λ Δϕ ( ) λ Δϕ ( )) ( λ Δϕ ( ) λ Δϕ ( )) Ψ L L L L where λ is he wavelegh for sigals a he wo differe frequecies ad ϕ is he correspodig phase differece i (4), ad Ψ is he es limi. We use a value of 3 mm for Ψ. This is, wih some margi for oise, eough o deec he difficul cycle slip combiaio of 9 cycles o ad 7 (6)

3 cycles o L. If he requireme i (6) is fulfilled for a saellie, his saellie is emporarily excluded from he calculaios, ad a soluio is formed from he remaiig saellies. We use his soluio o deermie a N value for he excluded saellie. y subracig he derived Δl ad N from he measures i (4) we obai a se of correced phase differece measuremes ha are used i he esimaio procedure. 4. SENSOR FUSION We esimae he discree saes of he sough parameers, posiio, velociy, ad acceleraio usig a Kalma filer [8]. The measureme model of he filer is he assumed liear relaioship bewee he ipu quaiy, i.e., he measuremes, z, ad he oupu quaiy, x, ha we wa o esimae. This relaioship is described by he observaios marix, H, coaiig he parial derivaives z = Hx + v (7) where v is he measureme oise. The ipu quaiy, z, coais he correced phase differece measuremes from he wo GPS receivers ad he acceleraio measuremes i wo direcios from he acceleromeers. The oupu quaiy, x, coais he variables x { r, r, r,&& r,&& r,&& r, a, a, τ} LF LF T = e v e v e v e (8) where r e, r ad r v are he compoes of he baselie bewee he referece ad rover aeas e v are he velociy compoes of he rover, & r& e, & r&, & r& v are he correspodig acceleraio compoes, a LF e,a LF are he low frequecy compoes of he rover acceleraio o capured by he acceleromeers, ad τ is he differece bewee he local clocs i he wo GPS receivers. r r r Δ r Γ + = Γ + Δv + a () We model he acceleraio ad he cloc differece as radom wal processes. The low frequecy pars of he acceleraios are modeled as Gauss Marov processes. Radom wal: We defie a discree radom wal process, μ, as a sampled Wieer process [8] μ + () + = μ where is a zero mea whie oise sequece. Hece he bes predicio of a radom wal process value is he previous value of he process ad hus he represeig eleme i Φ is equal o. The process oise covariace marix ca be wrie as Q = α Δ (3) where α is a cosa characerizig he process ad Δ is he ime bewee he samples ad +. I order o fid represeaive values for our acceleraio parameer α, we esimae Q for differe Δ. Qˆ ( ) = ( am ( i) am ( i + )) (4) N where a m is measured acceleraio. Fig. shows Q for a 7 s log daa secio. The gree curve is based o measuremes from he acceleromeer moued i he drivig direcio of he car ad he red curve is based o a prelimiary GPS acceleraio esimaes. We use a marix Φ o describe he relaioship bewee he curre sae ad he ex sae + of he oupu quaiy, x. x + = Φx + w (9) where w is process oise. Hece, he covariace marix of he process oise w. is T [ ] Q = E w w (0) Usig he sae rasiio marix Φ, we predic he ex discree sae + of he posiio Γ={r e, r, r v } as a liear fucio of he previous posiio, velociy ad acceleraio: Fig. Example of a esimae of Q based o measuremes from he acceleromeer (gree) ad GPS (red).

4 fi o he daa i he figure gives a value for α ha we use i our processig. Gauss Marov: We model he low frequecy variaios, o capured by he acceleromeers, as a Gauss Marov processes which is a saioary Gaussia radom process wih a expoeial auocorrelaio fucio. The modelig is performed by describig he rue acceleraio, & r&, as he measured acceleraio plus a slowly varyig Gauss Marov process a GM: & r = a m + a GM (5) The sae of he Gauss Marov acceleraio is i he Kalma filer modelled as. a Δ, + = e agm, (6) GM + where e -Δ, which describes he expoeially decayig correlaio of he Gauss Marov, is he sae rasiio facor used i he Φ marix o describe he Gauss Marov process ad is he process oise of he Gauss Marov process. I order o characerize his Gauss Marov process, he acceleromeer measuremes are compared o GPS-oly esimaes of he acceleraio. The differece bewee hese wo ses of measuremes represes he low frequecy process, a GM ha he acceleromeers do o produce. We esimae he ime cosa / of he Gauss Marov process by aalyzig he auocorrelaio of his differece. s described above he esimaed sae variables ad he correspodig uceraiy i he Kalma filer is depede o he models ad parameers used i he Kalma filer. The measureme uceraiy used as ipu o he Kalma filer jus propagaes rough he filer, hece good raceabiliy is achieved. Wih proper modellig ad accuraely esimaed oise parameers good corol of he uceraiy esimaes is possible. 5. RESULTS We evaluaed he sysem by a ope-sy field experime wih he measureme sysem moued i a car a a es locaio wih miimal obsrucio of he GPS sigal. The experime was performed UTC o November 7, 008. Measuremes were colleced durig 5 miues. The drivig dyamics varied wih velociies bewee 0 ad 00 m/h ad wih boh rapid acceleraios ad deceleraios. Fig. shows he speed esimae durig a 85 secod log secio of he experime. The iiial par shows he speed of he car while drivig a a freeway, he rapid decrease i speed a he ed of he period correspods o a rapid deceleraio a a freeway exi. MESUREMENT UNCERTINTY We evaluae he measureme uceraiies usig he Kalma filer error covariace marix P. The diagoal elemes of he marix P coai he sadard measureme uceraiies squared for each oupu quaiy. P is deermied for each epoch as P ( ) (7) = I K H P Fig. Speed esimae durig a period of 85 s (gree). 00 ms log secio (blac) of his period is preseed i Fig.. where H is he observaio marix from he measureme model i (7), K is he Kalma gai, see [8], ad P is he a priori error covariace. The a priori error covariace is a fucio of he sae rasiio marix Φ, process oise covariace marix Q ad error covariace marix P from he previous epoch, -. P (8) T = Φ P Φ + Q Fig. 3 shows a 00 ms log secio of he speed esimae. The secio is a shor sapsho durig he deceleraio show i Fig.. I he figure is also show he measureme uceraiies associaed wih each speed esimae. s ca be see, he expaded uceraiy is abou ± 0. m/h usig a coverage facor =. sligh icrease i he measureme uceraiy ca be see durig he periods wih oly acceleromeer daa. The process oise parameers, i he oise covariace marix Q, are as described i he sochasic modellig saisically deermied by srucure fucios from he drivig dyamics of he car.

5 uilize he gyro iformaio he Kalma filer mus be augmeed wih sae variables correspodig o he gyro iformaio. Furhermore o avoid he Gauss Marov modellig ad o miimize he umber of parameers ha have o be esimaed i he Kalma filer i is of ieres o fid acceleromeers ha provides DC iformaio. Though whe usig acceleromeers ha provides DC iformaio a sudy of how he graviy filed effecs he resul mus be carried ou o be able o compesae for his. Fig. 3 Speed esimae durig a 00 ms log period. The error bars i blue show he expaded measureme uceraiy wih a coverage facor =. REFERENCES [] Smyh., M. Wu, Muli-rae Kalma filerig for he daa fusio of displaceme ad acceleraio respose measuremes i dyamic sysem moiorig, Mechaical sysems ad Sigal Processig, 007, vol. [] Wedel J., G. F. Trommer, Tighly coupled GPS/INS iegraio for missile applicaios, erospace Sciece ad Techology, 004, vol DISCUSSION The process oise parameers esimaed i his experime were esimaed based o a 7 secod log secio of daa from he 5 miue log experime. The drivig codiios durig he experime were as described varied wih periods of very calm drivig mixed wih periods of drivig wih high dyamics. s a cosequece, he process oise parameers may have bee overesimaed for applicaios wih oly low drivig dyamics ad slighly uderesimaed for very dyamical codiios. To improve he accuracy we could, for example, use process oise parameers ha are deermied from a idepede daa se wih drivig dyamics represeaive for he drivig codiios of he specific applicaio. For example he drivig dyamics for a crash es applicaio should be esimaed from daa colleced from previous crash ess. 7. CONCLUSIONS We have developed a sysem ad mehodology ha provides posiio, velociy ad acceleraio esimaes of high accuracy i he horizoal compoes wih a ime resoluio of 000 Hz. The mehod provides good corol over he measureme uceraiies hrough he Kalma filer algorihm. However, idepede evaluaio should be performed i order o assess he resuls. [3] Gao J., M. G. Peovello ad M. E. Cao, Developme of Precise GPS/INS/Wheel Speed Sesor/Yaw Rae Sesor Iegraed Vehicular Posiioig Sysem, ION NTM, Moerey C, 8-0 Jauary 006 [4] Gao J., GPS/INS/G Sesors/Yaw Rae Sesors/Wheel Speed Sesors Iegraed Vehicular Posiioig Sysem, ION GNSS, Forh Worh TX, 6-9 Sepember 006 [5] Kim J., G. Jee, J. G. Lee, Complee GPS/INS Iegraio Techique Usig GPS Carrier Phase Measuremes, Posiio Locaio ad Navigaio Symposium, IEEE 998 Volume, Issue, 0-3 pr 998 Page(s):56 533, DOI: 0.09/PLNS [6] Peovello, M.G., C. O'Driscoll ad G. Lachapelle (007) Ulra-Tigh GPS/INS for Carrier Phase Posiioig i Wea Sigal Evirome. NTO RTO SET-04 Symposium o Miliary Capabiliies Eabled by dvaces i Navigaio Sesors, alya, Turey, - Ocober 007, 8 pages. [7] Hoffma-Wellehof., H. Licheegger ad J. Collis, GPS Theory ad Pracice, Spriger, New Yor, 99. [8] row R. G., P. Y. C. Hwag, Iroducio o Radom Sigals ad pplied Kalma Filerig, Wiley, New Yor, 99. For fuure experimes we will exed he umber of sesors e.g., use combied ri-axial acceleromeers ad gyros. y doig his all hree acceleraio compoes ca be measured o improve he resul, ad he gyro iformaio could be used for deermiig he direcio of he car. To