Simultaneous State Estimation of UAV Trajectory Using Probabilistic Graph Models

Transcription

1 Smultaneous State Estmaton of UAV Trajectory Usng Probablstc Graph Models erek Chen and Grace Xngxn Gao Unversty of Illnos at Urbana-Champagn BIOGRAPHY erek Chen s currently a Masters student n the epartment of Aerospace Engneerng at the Unversty of Illnos at Urbana-Champagn. He graduated wth hs B.S. degree n Aerospace Engneerng from the Unversty of Illnos at Urbana-Champagn n May 014. Hs research nterests are n autonomous systems and sensor fuson. Grace Xngxn Gao s an assstant professor n the Aerospace Engneerng epartment at Unversty of Illnos at Urbana-Champagn. She receved her B.S. degree n Mechancal Engneerng n 001 and her M.S. degree n Electrcal Engneerng n 003, both at Tsnghua Unversty, Chna. She obtaned her Ph.. degree n Electrcal Engneerng at Stanford Unversty n 008. Before jonng Illnos at Urbana-Champagn as an assstant professor n 01, Prof. Gao was a research assocate at Stanford Unversty. Prof. Gao has won a number of awards, ncludng RTCA Wllam E. Jackson Award, Insttute of Navgaton Early Achevement Award, 50 GNSS Leaders to Watch by GPS World Magazne, and multple best presentaton awards at ION GNSS conferences. ABSTRACT For many arborne sensor applcatons, knowledge of the aeral platform s poston and velocty asssts n constructon of the target mage and mappng the resultant model onto a surveyed map. Snce sensor data s processed durng post-processng, emphass s placed on an accurate trajectory estmate as opposed to a real-tme navgaton. In ths paper we present a smoothng approach for ntegraton of Inertal Measurement Unt (IMU) and Global Postonng System (GPS) pseudorange measurements n a tghtly coupled UAV system. We combne all avalable sensor nformaton n a probablstc graph connected by a moton model that relates each state to ts predecessor states. We then solve for a Maxmum a Posteror (MAP) estmate by fndng the parameters that maxmze the jont probablty model derved from the probablstc graph. An Iteratve Least Squares (ILS) algorthm s used to solve for the parameters of the MAP estmate. In order to valdate our algorthms, we set up an expermental test bed usng an Asctec Pelcan Quadrotor equpped a u-blox LEA-6T recever. Implementng our algorthm on collected GPS Pseudorange and accelerometer flght data, we show that we can combne GPS code measurements and nosy IMU sensor data for smoother, more precse state estmates of a UAV s trajectory. INTROUCTION Unmanned Ar Vehcles (UAVs) are commonly used n Intellgence, Survellance, and Reconnassance (ISR) mssons for ther abltes to survey vast areas quckly, safely and effcently. They can be equpped wth a varety of sensors ncludng magers, radar, and SIGINT sensors and can gather n-depth nformaton across a target regon. Because there are many applcatons where knowledge of a UAV s poston and velocty asssts n reconstructon of sensor data, GPS measurements are often taken alongsde sensor measurements. One example s ground feature reconstructon usng arborne Synthetc Aperture Radar (SAR) Interferometry. Radar measurements taken from multple postons are combned wth precse GPS postons through postprocessng to reconstruct geographcal features [1]. GPS postons and veloctes are also used n reconstructon of 3 models n geo-regstered locatons from moton vdeo []. Tradtonally, the goal of aeral platform state estmaton s to estmate ts current state and predct ts future state at the next tme step so that future state estmates can be corrected. However n order to complement the scenaros above, our goal s not only estmatng and predctng our current and next states, but also rectfyng our past estmates based on future measurements. Navgatonal sensors can be categorzed nto external sensors and dead-reckonng sensors. External sensors such as GPS provde absolute postonng based on measurements to external landmarks or beacons. eadreckonng sensors, such as Inertal Navgaton Systems, provde relatve postonng based on one s prevous poston. Although more robust than external sensors, dead reckonng sensors can be subject to drft, accumulatng large errors over tme. Generally these sensor types are combned n the form of a GPS/INS system. Integraton of these two sensors can be ether loosely coupled or tghtly coupled. Our method takes a tghtly coupled approach to combne GPS and Inertal

2 Measurement Unt (IMU) measurements. Tghtly coupled approaches allow for a more robust state estmate by ncorporatng IMU estmates drectly nto the GPS navgaton soluton. Kalman type flters are commonly used for GPS/IMU sensor fuson, provdng an teratve method to predct and update UAV state estmates for navgatonal purposes [3][4]. Snce the focus of collected sensor data s n postprocessed mappng and reconstructon of target mages and not real-tme navgaton, we place an emphass on estmatng the entre trajectory of the UAV as opposed to a current state. We can leverage future state estmates and sensor measurement nformaton to rectfy predecessor state estmates. Thus a smoothng approach s preferred rather than a flterng approach towards localzaton. In pror work, aellert and Kaess ntroduced a smoothng approach as a vable alternatve to extended Kalman flters for postonng a robot s path through landmark measurements and a moton model [5]. Bryson et al. presented a smoothng approach to IMU, GPS, and monocular vson fuson for large-scale terran reconstructons [6]. In ths paper we present a smoothng approach for ntegratng measurements from low-cost GPS and IMU sensors wth a smple UAV dynamcs model. APPROACH The goal of our state estmaton algorthm s to fnd the optmal estmated trajectory traversed by a UAV. By takng a smoothng approach, we allow sensor nformaton from future states to propagate backwards and rectfy prevous state estmates. Our system ncorporates GPS pseudorange code measurements tghtly coupled wth an Inertal Navgaton System (INS) threeaxs accelerometer. The UAV state beng estmated at tme s denoted x and conssts of UAV poston, velocty and acceleraton. We defne the UAV dynamc model as a pont partcle movng n Earth-Centered Earth- Fxed (ECEF) component drectons, x, y, and z at an estmated velocty and acceleraton. As opposed to loosely coupled systems where INS measurements are ntegrated wth poston estmates output from a GPS recever, our tghtly coupled estmator combnes accelerometer measurements n our poston and clock bas calculaton from the GPS pseudorange measurements [7]. The GPS and state calculatons then drectly complement the IMU measurements n estmatng the IMU drft and bas n order to obtan more accurate acceleraton measurements throughout the trajectory of the UAV. To accomplsh ths smoothng and batch estmaton of the UAV trajectory states, we frst set up a probablstc graph model to map the condtonal dependences between each state and measurement varable. Ths allows us to characterze the nfluence that measurements of a partcular state have on ts predecessor states. We then transform the probablstc model nto a maxmum a posteror probablty estmaton problem where we seek to fnd the most lkely state estmates gven specfc sensor measurements from the GPS recever and IMU accelerometer. We then solve the batch estmaton problem through Iteratve Least Squares (ILS) by mnmzng each source of error and transtonng from one teratve state estmate to another through a the systems Jacoban functons. Probablstc Graph Model We use a probablstc graph, specfcally a Bayes network to model random varables and ther condtonal dependences n our system. A Bayes network s a drected acyclc graph where the set of nodes are connected by edges and each edge has a drecton mplyng a condtonal dependence. It s acyclc mplyng that there are no loops. Startng at any chosen node, t s mpossble to follow a sequence of drected edges to eventually loop back to the ntally chosen node. Wthn our probablstc graph for a GPS system, each node represents a sensor measurement or an object state. We desgnate four types of nodes, UAV states, satellte postons, GPS measurements, and IMU measurements. The edges wthn the GPS probablstc graph represent the effect of a measurement upon an estmate of a state or moton between states. We model the UAV state estmates of each tme step as nodes, connected throughout the trajectory tmespan. Each node s lnked to ts predecessor through a sngle edge representng the propagated UAV state through ts dynamcs complemented by the IMU Accelerometer measurements. Ths apples a constrant to the probable postons of a state relatve to ts predecessor. Next, each GPS satellte vsble across the tmespan s desgnated as a dfferent node. Each UAV state node then takes a GPS pseudorange measurement to each vsble satellte at that partcular measurement epoch. Fgure 1: Vsualzaton of GPS Probabalstc Graph

3 Model From the modeled dependence structure, we form a jont probablty model. The jont probablty of the UAV trajectory s denoted as: where P(X, Z, A, dt, β) s the jont probablty of the trajectory for the set of all states X, set of all pseudorange measurements Z, and set of all accelerometer measurements A. P(x o ) s a pror on the ntalzaton state, statng certanty at the UAVs takeoff poston. P(x x 1 ) s the moton model propagated from prevous states. P(z k x, ρ k, dt ) s the GPS measurement model where the pseudorange to satellte k at tme, z k, s parameterzed by x, the observed pseudorange to satellte k at tme, ρ k, and the recever clock bas dt. P(x α, β ) s the IMU measurement model where x, the acceleraton state s parameterzed by the accelerometer measured acceleratons a and the accelerometer bases β. T s the set of all tme steps and K s the set of all vsble satelltes. We defne our target estmate trajectory states as the states and observatons wth maxmum lkelhood. By assumng that the system process and measurement models are Gaussan, we defne the followng x f ( x ) w 1 k k k z r( x,, dt) x v where f s the process model that defnes the UAV s system dynamcs, w s normally dstrbuted zero-mean process nose wth covarance Σ, r s the pseudorange measurement equaton and ε k s the pseudorange error wth covarance Σ ρ, α s the measured acceleraton, β s the accelerometer bas, and v s a zero-mean measurement nose wth covarance Σ α. We then defne the Gaussan probablty dstrbutons of the jont probablty model as 1 Px x 1 exp f x 1 x where 1 P z x dt r x dt z,, exp,, k k k k Px, exp 1 b x v e s defned as the Mahalanobs dstance of vector e wth a covarance Σ. We defne as the collecton of parameter varables (X, Z, A, dt, β) such that X, Z, A, dt,. We search for the parameters ˆ that wll maxmze our jont probablty through a Maxmum a Posteror (MAP). P X Z A dt ˆ arg max P X, Z, A, dt, arg mn log,,,, By takng the negatve log functon of the probablty maxmzatons, we transform the maxmzaton problem nto a mnmzaton problem where the jont probablty products become a seres of sums. T T K k k f x 1 x r x,, dt z ˆ 1 1 k 1 argmn T x v 1 Iteratve least squares s then used to numercally solve for the optmal varable estmate ˆ. Numercal Soluton va Iteratve Least Squares (ILS) We rewrte the log lkelhood of the jont probablty as a sum of errors of ther respectve measurement and process models. ˆ arg mn C C C ynamcs PR IMU C s the sum of all costs c attrbuted to the dynamcs propagatons of velocty and poston from the predecessor state x 1 to the estmated postons at the current state x. It has two components, the poston propagaton error and the velocty propagaton error. c c c f x x Poston Velocty 1 ˆ 1 xˆ xˆ x t xˆ t xˆ xˆ xˆ t We denote the Jacoban for a sngle tme step for the dynamcs costs as P and V respectvely. x 1 x 1 x 1 x t t P t t t t x 1 x 1 x x t V t t where t s the calculaton tme step. P s the dervatve of the poston component of the dynamcs process model

4 and correlates the poston of the th state wth the -1 th state s poston, velocty, and acceleraton as defned n the dynamcs model. V accomplshes the same except for the velocty. C PR s the sum of all costs c PR attrbuted to the pseudorange measurement versus the geometrc range between the estmated poston and each satellte modfed by the recever clock bas. k k k k k k,, c r x dt z x x y y z z cdt PR As per the standard ILS method for code-based navgaton soluton calculaton, we denote the Jacoban for a sngle tme step for the Pseudorange costs as G, the geometry matrx each satellte-n-vew k. G J 1 P1 G T P V1 V x x y y z z x x y y z z 1 G k k k x x y y z z 1 k k k The resultant geometry matrx nfluences the postonal estmate as a gven state. C IMU s the sum of all costs c mu attrbuted to the error between the IMU accelerometer measurements and the estmated acceleratons. ˆ c x v x IMU We denote the Jacoban for a sngle tme step for the IMU error as correlatng the acceleraton estmates at each tme step to tself as well as the accelerometer bases. x β The errors have a covarance of, the concatenated covarance matrc of each error component. PR IMU The resultant Jacoban matrx J that relates the costs to each state varable s then the concatenaton of each of the component Jacobans P, V, G, at all tme steps t=1 to T. For a 3-dmensonal system of T tme steps, we estmate 10 state varables at each tme step, poston, velocty, and acceleratons x, x, x along wth the clock bas dt. We also estmate the system accelerometer bas n a sngle 3- dmensonal varable β. As a result, the total number of estmate varables s 10 x T + 3. From the dynamcs process cost functons, we obtan cost equatons, poston and velocty, for each 3- dmensonal varable x, x startng from the nd tme step. Ths results n a x 3 x (T-1) or 6 x (T-1) cost functons for the total system. The IMU error functons provde 3 cost functons per tme step resultng n 3 x T functons. From the GPS pseudorange measurements, we obtan k cost functons dependng on the satelltes n vew at each tme step. Assumng 4 satelltes are n vew at each epoch, we obtan 4 x T cost functons. The dmensons of the resultant Jacoban matrx are [13T 6] x [10T + 3] resultng n an overdetermned system when 4 satelltes are n vew per tme step. Ths also provdes robustness avalable for state estmaton at gap perods when satellte vsblty drops below 4. Although a vast Jacoban matrx s created wth components from each tme step, the resultant matrx s largely a sparse matrx. We can leverage the sparseness of the Jacoban matrx n reducng the computaton load and obtanng computatonally effectve batch estmaton. Afterwards ILS steps are used untl the system converges towards optmal estmate for the states n the trajectory so far. The matrx H s the transton matrx that represents the ILS step towards optmzaton and mnmzaton of the system costs. ter 1 T 1 H J J J ter 1 T 1 x x Hb As each measurement s added on, past measurements are re-estmated and rectfed. Through teraton, the optmal

5 state estmates are acheved at the end of a trajectory where all pseudoranges and IMU data collected throughout the flght s used for a batch estmate for all states smultaneously. code-based navgaton soluton from the trajectory of the flght. EXPERIMENTAL RESULTS To valdate our algorthm, we used the Asctec Pelcan Quadrotor as an expermental platform. Fgure 4: GPS only postonng of a UAV s trajectory. Fgure : Expermental setup of an Asctec Pelcan. It s equpped wth an onboard IMU and an addtonal u-blox LEA-6T GPS recever. The Asctec Pelcan has two on-board processors, a low level processor and a hgh level processor. The low level processor conssts of an onboard mcrocontroller that s n charge of the flght controller of the Pelcan. It contans an onboard INS consstng of IMU rate gyros and accelerometers. It also processes the dfferent nner and outer loop controls for operatng the quadrotor. The hgh level processor s an Asctec Mastermnd board wth an Intel Core 7 processor. The Mastermnd board serves as an on-board navgatonal computer for processng navgatonal algorthms. Connected to the Mastermnd s a u-blox LEA-6T GPS recever. For ths experment, flght sensor data from the IMU and the GPS recever was collected and logged on the navgatonal computer for post-processng. In the test segment, GPS data was collected at a rate of 1 Hz and fused wth fltered IMU data of the same rate. A 0 elevaton angle mask constrant was placed on the avalable satelltes and as a result each epoch had between 4-6 satelltes vsble. From the dfferent process and measurement models the resultant Jacoban of the flght s a 981 x 613 sparse matrx. The dfferent segments of the Jacoban are marked as follows n Fgure 5. Fgure 5: The nonzero elements of the resultant sparse Jacoban matrx. Through the smoothng approach presented wthn ths paper, we acheved a much more accurate UAV trajectory and postonng spread of less than meters. The result s shown n Fgure 6. Fgure 3: Command and ata Handlng agram for the Asctec Pelcan equpped wth navgaton equpment. We mplemented our algorthm on data collected from a sxty-one second flght. Fgure 4 shows the resultant

6 Fgure 6: GPS wth PGM estmated postonng of a UAV s Trajectory. We then tested the effects of navgaton under low satellte vsblty. urng aperod where only four satelltes were n vew, we nvestgated the result of removng satellte PRN on the navgaton soluton. As a result, the UAV poston calculatons durng the perod of tme were degraded severely as shown n Fgure 7. Postonng errors were upwards of 10 m durng ths perod of low satellte vsblty. Fgure 8: PGM mproved GPS navgaton soluton wth a 10 s gap n satellte vsblty CONCLUSION AN FUTURE WORK In ths paper we propose a smoothng approach for smultaneous estmaton of a UAV s trajectory states. Our algorthm tghtly ntegrates IMU accelerometer measurements wth GPS pseudorange measurements usng a probablstc graph. We combne all sensor data taken throughout the flght n a sngle batch estmaton process n order to estmate the full trajectory. We demonstrate that wth our approach we are able to acheve a more accurate estmate of a UAV s trajectory even durng stuatons where satellte vsblty s momentarly lost. Future work wll examne modfyng the algorthm nto an teratve verson for real-tme control and navgaton of a UAV. We wll also examne ncorporaton of dfferent types of navgaton sensors nto our sensor fuson approach and the effects of leveragng payload sensor data on the navgaton soluton. ACKNOWLEGMENTS Fgure 7: GPS navgaton soluton wth a 10 s gap n satellte vsblty After applyng our graph-based smoothng algorthm, we were able to contan the trajectory estmate wthn a 3 meter error as shown n Fgure 8. We would lke to acknowledge Professor Tmothy W. Bretl and hs graduate students Xnke eng, avd Hanley, and Joseph M. egol for ther assstance wth operaton of the Asctec Pelcan Quadrotor. We would also lke to thank Yuhao Lu for hs assstance n UAV flght data collecton. REFERENCES [1] Hensley S, Zebker H, Jones C, et al. Use of arborne SAR nterferometry for montorng deformaton of largescale man-made features, Internatonal Workshop Spatal Informaton Technologes for Montorng the

7 eformaton of Large-Scale Man-made Lnear Features, Jan 010 Hong Kong, Chna. [] M. Pollefeys et al. etaled real-tme urban 3d reconstructon from vdeo. IJCV, 78(-3), 008. [3] A. Fakharan, T. Gustafsson, M. Mehrfam, "Adaptve Kalman Flterng Based Navgaton: An IMU/GPS Integraton Approach," 011 Internatonal Conference on Networkng, Sensng and Control,ICNSC 011, art. no , pp , 011. [4] B. P. Zhang, J. Gu, E. Mlos, and P. Huynh, "Navgaton wth IMU/GPS/dgtal compass wth unscented Kalman flter," n Proc. IEEE Int. Conf. Mechatron. Autom., Jul. 005, pp [5] F. ellaert, Square root SAM: Smultaneous locaton and mappng va square root nformaton smoothng, n Proc. Robotcs: Scence and Systems, 005. [6] M. Bryson, M. Johnson-Roberson, and S. Sukkareh, "Arborne smoothng and mappng usng vson and nertal sensors," n Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 009. [7] J. Carlson, Mappng Large, Urban Envronments wth GPS Aded SLAM, Carnege Mellon Unversty, 010