Proceedings of the 5th Workshop on Positioning, Navigation and Communication 2008 (WPNC'08), pages 137-141.
PNaFF: a Modular Software Platform for Testing Hybrid Position Estimation Algorithms Matti Raitoharju, Niilo Sirola, Simo Ali-L¨oytty, Robert Pich´e Department of Mathematics Tampere University of Technology Tampere, Finland Email:
[email protected]
Abstract—PNaFF (Personal Navigation Filter Framework) is a comprehensive simulation and filtering test bench that is being developed within the Personal Positioning Research Group at the Department of Mathematics of Tampere University of Technology. Hybrid positioning is a process where measurements from different sources are used to obtain position estimate. PNaFF provides tools for comparison and visualization of the performance of hybrid positioning filters that estimate the current state from measurements and the previous state estimate. New filters can be added to PNaFF easily and the performance of the new filter can be assessed instantly. Different kinds of measurements can be used in estimation, and it is simple to add new measurement types.
I. I NTRODUCTION Current consumer grade GPS-based personal positioning devices are reasonably priced and small. They give an accurate position estimate when the visibility to the sky is good. However, due to the poor permeability of the GPS signals, these devices are often unable to give a satisfactory estimate when visibility is obscured by buildings, trees, etc. Hybrid positioning is an approach to dealing with such situations. Hybrid positioning is a general term for techniques that use diverse information sources to estimate position. These new measurement sources can be wireless networks such as the cellular network, WLAN and Bluetooth, or measurements of physical quantities by on-board sensors such as accelerometers, barometers or a digital compass. Although additional measurements can improve the estimate, the larger estimation problem is mathematically and computationally harder because of nonlinearities, non-Gaussian noise, larger number of parameters, etc. Positioning is a fast-growing field of industry. The need for new more accurate positioning methods causes a major interest in development of nonlinear filters for positioning. Over the past few years, we have studied and developed hybrid positioning algorithms at Personal Positioning Research Group1. As a part of this research, the group has developed a comprehensive simulation and filtering test bench called PNaFF (Personal Navigation Filter Framework). PNaFF provides tools for comparison and visualization of the performance of a wide variety of nonlinear positioning 1 http://math.tut.fi/posgroup/
filters that use different kinds of real or simulated measurements. PNaFF reduces the amount of overhead work, as not every researcher has to write their own simulation programs and test benches. PNaFF also gives a platform for comparing performance of different algorithms currently under development in our group. Currently, to our knowledge, there is no equivalent work done. There are some M ATLAB toolboxes for the general filtering problem [1] [2]. However, development of filters for personal positioning has specific needs in addition to general filter development: • Application-specific measurement equations • Input of real data • Calculation of the ground truth • Generation of simulated tracks and measurements • Augmenting real and simulated measurements These are implemented in PNaFF. We have written already over twenty filter variants for PNaFF, which include the most commonly used filters and some novel filters that we have developed in our group. In the next section, the nonlinear filtering problem is presented. Different coordinate systems used in PNaFF are discussed in Section III. Section IV covers PNaFF and the last section is the conclusions. II. N ONLINEAR F ILTERING P ROBLEM State estimation in PNaFF is based on state space models [3, p. 35]. In context of position estimation, state consists of at least the position and often some other variables such as the velocity. State space models can be written in form xn yn
= fn (xn−1 ) + wn = hn (xn ) + vn ,
(1) (2)
where xn is the current state and yn is the measurement vector at a discrete time tn . Function fn (xn−1 ) defines how the next state depends on the previous, and hn (xn ) is the measurement function. Process noise of the state transition is denoted wn and measurement noise is denoted vn . In our models, the state transition equation (1) is assumed linear and can be written as xn = Φn xn−1 + wn .
(3)
the ideal filter when the number of particles tends to infinity [8]. In many occasions, the required number of particles for good results is enormous and so the amount of needed resources is enormous as well. However, when the time is not a top priority, particle filters can be used to estimate the ideal filter, and this is how PNaFF calculates reference solutions for benchmarking.
filter A
ideal filter filter B
III. C OORDINATE S YSTEMS
true track Fig. 1. Hypothetical contour curves of distributions given by an ideal filter and two approximate filters for a bimodal distribution [5]
Filters try to estimate the current state when the initial state x0 and measurements y1 , . . . , yn are known. In this paper, by state estimate we mean the probability distribution of the state. If a point estimate is needed e.g. the mean, then we can calculate suitable value for that from the full distribution. For practical purposes, filter operation has to be recursive so that the state estimate xn can be calculated from current measurements yn and the initial state xn−1 . To achieve this we have to assume that vn , wn and xn−1 are independent of each other and errors are white noise. PNaFF does not constrain the use of measurements to only the last ones. Smoothers, for example, use several successive measurements. An ideal filter is the Bayesian filter that uses all the prior and measurement information in estimation [4, p. 4]. In most cases, practical filters are only approximations of the ideal filter. Even though we consider Bayesian filter as an ideal filter, the filters in PNaFF do not have to be based on Bayesian statistics. To appreciate some of the issues that can arise when comparing different filters, consider Fig. 1, which shows estimates from different hypothetical filters. Dashed line represents level curve of a probability distribution of the ideal filter. Filters A and B are different approximations of the ideal filter. In this case, filter A gives estimate closer to ideal filter and filter B gives estimate closer to the true position. As the estimate of filter B is more local than the ideal estimate, one can suspect that there is a risk that filter B may give inconsistent estimates in some other case. Because there is no clear single evaluation criterion, there is clearly a need for different alternative quality measures when benchmarking filters. Section IV-E presents the measures used in PNaFF. There is an ideal filter called the Kalman filter [6] for estimation problems where fn (xn−1 ) and hn (xn ) in equations (1) and (2) are linear, the initial state estimate x0 is Gaussian, wn and vn are Gaussian and independent, and the distribution of vn is nonsingular. The GPS measurements are on ground level almost linear and extensions of Kalman filter can solve problems with only GPS measurements quickly and with reasonable accuracy [7]. In hybrid positioning, measurements are usually nonlinear. For example, distance measurements made from nearby sources are nonlinear. The handling of nonlinear measurements is currently our main interest in filter design. It has been proven that filters, called particle filters, converge almost certainly to
PNaFF uses internally the Cartesian ECEF (Earth Centered Earth Fixed) coordinate system, where origin is located in the center of the Earth, z-axis points along Earths rotational axis, x-axis is parallel to line where latitude and longitude are both zero degrees and y-axis forms a right handed system with xand z-axes. WGS84 (World Geodetic System 1984) coordinates are commonly used in GPS receivers. WGS84 defines a LLH (Latitude Longitude Height) coordinate system. These coordinates are used in PNaFF for altitude calculations and for looking up positions on a map. ENU (East North Up) is a local coordinate system, where coordinate axes are directed towards east, north and up at the reference point. These coordinates are used in visualization and in direction measurements. IV. PNA FF PNaFF is a set of M ATLAB programs that have common interfaces for communicating with each other. It is designed to allow easy addition of new filter algorithms and measurement sensor models. Filters are added into PNaFF by simply writing a new filter function. New measurement sources are added by writing the corresponding equations into the measurement processing functions. These functions are presented in more detail in Section IV-C. A. Filters Following list describes the main types of filters that we have in PNaFF. Many of the filters have different variations implemented. It makes the total number of filters to be over twenty. References are given only to filters that have some novel variation developed in our group. For rest of filters refer to textbooks, e.g. [4] and [9]. EKF Extended Kalman Filter [10] EKF2 Second Order Extended Kalman Filter IEKF Iterative Extended Kalman Filter LKF Linearized Kalman Filter UKF Unscented Kalman Filter Chain Batch Least Squares Filter GMF Gaussian Mixture Filter [11] Grid Grid Filter [12] IMM Interactive Multiple Models Filter Point Point Mass Filter Smoother Smoother RKF Robust Kalman Filter [13] SMC Sequential Monte Carlo Filter, a kind of a particle filter
Main
Filter
MeasEq
Statetransition
Init Solv Measurements, Solv
Solv.state x Measurements, x y , R, H
Solv
Fig. 2. Sequence diagram of a basic single step estimation procedure with filter that uses measeq function TABLE I S TATE VARIABLES position velocity user clock bias user clock drift satellite clock bias satellite clock drift pressure at sea level
r v = r˙ β γ = β˙ βn γn = β˙n p0
Implementing a new filter does not require alterations of any old files, only the creation of a new file containing the function of the new filter. Due to the common interface, the performance of this new filter can be immediately assessed visually as well as compared to existing ones. Filters are initialized by giving them the command init and then they return structure called Solv. Solv consists of time, state, state covariance, options and solver specific fields. Options structure has function handles to state covariance and transition functions that need time step as input parameter. In the following time steps, filters alter the structure Solv to correspond to the current state. Basic estimation procedure is illustrated in Fig. 2 and source code of an Extended Kalman filter, without error handling, for PNaFF is in Listing 1. B. State Models A state x in PNaFF is constructed from multiple variables that are listed in Table IV-B. Different variables can be combined freely into state vector. Only limitations are that any variable can be used only once and position r has to be the first variable in the state. All variables can be estimated or declared as constants. It is also possible to assign multiple variables equal and then estimate this. Currently state models are linear. State model process noise is assumed Gaussian and its covariance matrix is a function of the time step. Most variables are independent, however two commonly used state pairs are dependent: position – velocity and bias – drift. In situations where dependent state pairs exist the calculation of error covariance considers this automatically and models the movement with a Brownian model. More about stochastic processes and system models can be found in [3].
f u n c t i o n S o l v = e k f s o l v e r ( cmd , v a r a r g i n ) s w i t c h cmd case ’ i n i t ’ % I n i t i a l i z e s olver I n i t = v a r a r g i n {1}; OPT = v a r a r g i n { 2 } ; S o l v = s t r u c t ( ’ t i m e ’ , I n i t . tim e , ’ s t a t e ’ , . . . I n i t . state , ’ covariance ’ , . . . I n i t . c o v a r i a n c e , ’ O p t i o n s ’ , OPT ) ; case ’ step ’ S o l v = v a r a r g i n { 1 } ; % d a t a fr o m l a s t s t e p Meas = v a r a r g i n { 2 } ; % c u r r e n t m e a s u r e m e n t s OPT = S o l v . O p t i o n s ; % o p t i o n s d t = Meas . t i m e − S o l v . t i m e ; % t i m e s t e p P h i = OPT . t r a n s M t x ( d t ) ; % s t a t e t r a n s i t i o n m a t r i x Q = OPT . s t a t e C o v ( d t ) ; % and c o v a r i a n c e x = Solv . s t a t e ( : ) ; % previous sta t e P = Solv . covar iance ; % and c o v a r i a n c e z = Meas . v a l u e ( : ) ; % c u r r e n t m ea s es xminus = P h i ∗x ; % state prediction % g e t t h e p r e d i c t e d m ea s u r em en ts , d e r i v a t i v e s and % t h e m ea s u r em en t c o v a r i a n c e [ zminus , H , R ] = measeq ( xminus , Meas , OPT ) ; % Kalman s t e p P = P h i ∗P∗ Phi ’ + Q; % p r i o r c o v a r i a n c e K = P∗H ’ / ( H∗P∗H’ +R ) ; % Kalman g a i n dx = K∗ ( z − zm in u s ) ; x = xminus + dx ; % s t a t e e s t i m a t e P = ( ey e ( l e n g t h ( x )) −K∗H) ∗ P ; %p o s t . c o v a r i a n c e % create output S o l v = s t r u c t ( ’ t i m e ’ , newt , ’ s t a t e ’ , . . . x ( : ) ’ , ’ c o v a r i a n c e ’ , P , ’ O p t i o n s ’ ,OPT ) ; end Listing 1.
EKF filter for PNaFF
TABLE II M EASUREMENT TYPES CURRENTLY IMPLEMENTED IN PN A FF
deltarange altitude ambiguous pseudorange
ks − rk + ε = m ks − rk + β + ε = m ”T “ s−r (˙s − v) + γ + ε = m ks−rk [llh(r)]3 + ε = m c ks − rk + β + ε = m + N 1000
sector maximum range motion detection speed barometric altitude
m − σ2 ≤ ∠(enu(r − s)1:2 ) ≤ m + ks − rk ≤ m (discrete motion state) kvk + ε = m p0 (1 − 0.00012[llh(r)]3 ) + ε = m
range pseudorange
2
σ2 2
C. Measurement Processing for Filters Measurement types that can be used in PNaFF at the moment are presented in Table II, used variables are defined in Table IV-B. PNaFF offers two functions measeq and likelihood for the processing of the measurements for filters. Filters use either one or both of these two functions and are independent of actual implementation of measurement equations. Function measeq corresponds to Equation (2). It takes measurements and the prior state x− as the input parameters and returns expected values of measurements y− in the prior state, measurement error covariance matrix R and Jacobian
matrix of measurements H. The prior state can be obtained for example using (3). Given measurements y and measurement error distributions, likelihood calculates the likelihood p(y|x) of the state x. D. Data Measurements can be imported to PNaFF from a measurement device. They can be also simulated, and real and simulated measurements can be merged. When using real measurements, a ground truth track is needed. First, a smoothed solution using all measurements is calculated with EKF or RKF. The first solution is plotted on a map and then user can define the actual track with line segments. Then the first solution is projected on line segments and the position at each time step is calculated. If local altitude data is available, it can be also used in this process. Position on true track on ith time step is denoted rTrue i . When using real measurements, the true positions are not exactly known. The covariance matrix of rTrue is denoted PTrue i i . Generation of simulated measurements begins with generation of the true track using (3). Actual measurements are generated using function measeq. Error characteristics of measurements can be defined and probability for measurements that are completely erroneous can be defined. This probability is called blunder probability. Simulator tries to arrange base stations, satellites etc. realistically. Minimum elevation for visible satellites is user definable as well as the maximum range of the base stations. When PNaFF uses simulated track, PTrue i is a zero matrix. When generating augmented tracks, the true track is defined with real measurements. The simulated measurements are generated using this track and combined with existing real measurements. PNaFF calculates for each track a reference track that represents the best possible estimate using the available measurements. This estimate is generated with a particle filter that uses a very large number of particles. Position on reference track on ith time step is denoted rRef i . Main part of data in PNaFF consists of measurement tracks and test banks consisting of these tracks. At the moment PNaFF includes 14 hours of real data in the test banks. A test bank consists of following components: • Initialization values • Measurements • Information about measurement sources • True track • Reference track • Options Measurements of one time step are processed into one structure that has following fields: • Types of the measurements • Time of the measurements • Values of the measurements • Variances of the measurement errors • Blunder probabilities of the measurements • Positions of the measurement sources • Velocities of the measurement sources
TABLE III A N EXAMPLE OF COMPARISON OF FILTERS
Filter EKF EKF2 RKF GMF IMM SMC (N = 103 ) SMC (N = 16 · 103 ) SMC (N = 128 · 103 )
Relative time 1 1 2 2 4 6 129 1089
Mean error (m) 23 23 20 20 21 33 29 25
Fractile 95% (m) 50 50 43 43 46 78 60 56
Inside 50m (%) 95 95 97 97 96 85 92 94
E. Testing The filter test bench compares performance of different filters in different situations. Results can be presented graphically and/or as a list of the following statistical quality measures: Relative time is the ratio of the used CPU time of the filter to that of the fastest filter on that run Mean error is the mean Pn of errors compared to the true position, i=1 kri − rTrue i k/n RMS error is the p root mean square of the errors of the Pn True 2 ) /n (r − r filter, i i i=1 Mean reference error is P the mean error compared to the n reference, i=1 kri − rRef i k|/n Fractile x% is the distance d such that x% of the filtered results are within d of the true position Inside x tells how often result is within distance x from the true position Efficiency loss is the loss of accuracy when compared to the of the P estimate True Pn referenceTrue filter, 2 2 1 − ni=1 (rRef − r ) / i i i=1 (ri − ri ) Yield tells how often filter is able to give a result Inconsistency is the amount of time steps when the filtered position is inconsistent by its covariance [7] Table III is an example of results of comparison of filters. In this case, the test bank is made out of GPS and step counter measurements that are augmented with simulated base station measurements. PNaFF has a data testing function that analyses test bank tracks to evaluate how ”hard” they are to filter. The measures of ”hardness” are: Sigma How does the distribution of the reference filter differ from a Gaussian distribution with the same covariance matrix Components How many Gaussian components are needed to present the distribution Static How often a static solution can be calculated from measurements of a time step Cumulative How many consecutive measurements are needed to get a static solution Inconsistency How often EKF is inconsistent F. Supporting functions Supporting functions include visualization, coordinate conversion, and other useful functions for positioning tasks.
V. C ONCLUSIONS We have presented in this paper PNaFF that is a test bench for nonlinear hybrid positioning filters. Main strengths of PNaFF are the possibility to combine real and simulated measurements, easy addition of new filters, measurement types and error models and instant assessment of effects of the new filter or of the new measurement to accuracy of estimate and to computational complexity. These are essential components in developing hybrid positioning algorithms for mobile devices having limited resources on computational capacity, sensors and battery power. PNaFF is under constant development and a future work includes making public release versions and a comprehensive documentation of PNaFF. ACKNOWLEDGEMENTS This study was partly funded by Nokia Corporation. Simo Ali-L¨oytty acknowledges the financial support of the Nokia Foundation and the Tampere Graduate School in Information Science and Engineering. R EFERENCES ˇ ˇ acha, “Nonlinear filtering [1] M. Simandl, M. Fl´ıdr, O. Straka, and J. Sv´ toolbox,” Department of Cybernetics, University of West Bohemia in Pilsen, Technical Report, 2003, last visited 8.2.2008. [Online]. Available: http://nft.kky.zcu.cz/document/nftools.pdf [2] R. van der Merwe and E. Wan, “Recursive bayesian estimation library,” last visited 8.2.2008. [Online]. Available: http://choosh.csee.ogi.edu/ rebel
[3] P. S. Maybeck, Stochastic Models, Estimation, and Control Volume 1, ser. Mathematics in Science and Engineering. Academic Press, 1979, vol. 141. [4] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter: Particle Filters for Tracking Applications. Artech House, 2004. [5] N. Sirola, “Mathematical methods in personal positioning and navigation,” Ph.D. dissertation, Tampere University of Technology, August 2007. [Online]. Available: http://webhotel.tut.fi/library/tutdiss/ pdf/sirola.pdf [6] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Transactions of the ASME–Journal of Basic Engineering, vol. 82, no. Series D, pp. 35–45, 1960. [7] S. Ali-L¨oytty, N. Sirola, and R. Pich´e, “Consistency of three Kalman filter extensions in hybrid navigation,” in Proceedings of the European Navigation Conference GNSS 2005, July 19-22, 2005, Munchen, 2005. [8] K. Heine, “On the stability of the discrete time filter and the uniform convergence of its approximations,” Ph.D. dissertation, Tampere University of Technology, 2007. [Online]. Available: http: //webhotel.tut.fi/library/tutdiss/pdf/heine.pdf [9] M. S. Grewal and A. P. Andrews, Kalman filtering: theory and practice. Upper Saddle River, NJ, USA: Prentice-Hall, Inc., 1993. [10] S. Ali-L¨oytty and N. Sirola, “A modified Kalman filter for hybrid positioning,” in Proceedings of ION GNSS 2006, September 2006. [11] S. Ali-L¨oytty and N. Sirola, “Gaussian mixture filter and hybrid positioning,” in Proceedings of ION GNSS 2007, Fort Worth, Texas, Fort Worth, September 2007, pp. 562–570. [12] N. Sirola and S. Ali-L¨oytty, “Moving grid filter in hybrid local positioning,” in Proceedings of the ENC 2006, Manchester, May 7-10, 2006. [13] T. Per¨al¨a and R. Pich´e, “Robust Extended Kalman filtering in hybrid positioning applications,” in Proceedings of the 4th Workshop on Positioning, Navigation and Communication (WPNC’07), March 22, 2007, pp. 55–64.
