12th International Conference on Information Fusion Seattle, WA, USA, July 6-9, ISIF 126

Transcription

1 12th International Conference on Information Fusion Seattle, WA, USA, July 6-9, ISIF 126

2 with x s denoting the known satellite position. ρ e shall be used to model the errors occurring as described below. If all the errors in ρ e except the receiver s clock bias are neglected, at least four pseudorange measurements to different satellites are needed at the same time in order to solve the localization problem stated above. While this can often be guaranteed, it is e.g. still a problem in so called urban canyons, where large buildings can occlude a direct line of sight to several satellites. An integration of motion or inertial sensors measuring velocity and yaw rate of the vehicle can aid the localization in several ways. By using motion models [1], a feasible vehicle motion path can be determined by continuously integrating these measurements into the localization estimation. Therewith, localization accuracy can be increased and as the update rate of inertial sensors is usually high, interpolation between two consecutive GPS measurements can be done more precisely. Several methods of combining GPS and Inertial Navigations Systems (INS) are proposed in literature. Loosely coupled systems join a separately determined GPS position with the motion measurements, whereas tightly coupled systems use the raw GPS pseudorange measurements instead. Hence, even if there are less than four satellites visible they can be used to aid the localization. There are a lot of applications where an absolute positioning is not really necessary, but it is rather useful to know where traffic participants are located to each other. Relative Positioning addresses this task, while several approaches exist to solve it. A straightforward one could be that each vehicle determines its own absolute position by GPS and inertial sensors, provides this estimation to other vehicles so they can calculate a difference vector using their own position estimation. Another approach is to introduce ranging sensors which measure the distance between two vehicles by radio ranging technology. Therewith, a direct information link between two participants can be established, improving the localization estimation. Within this paper, a decentralized cooperative localization approach is presented, where several traffic participants provide their pseudorange measurements and ego motion via communication. Each vehicle estimates relative vectors to the other participants which are ego motion compensated using local and remote ego motion estimations. The main advantage arises from the fact that pseudorange measurements from common available satellites are tightly coupled with measurements from inertial sensors using a differential motion model. Additionally, as only raw measurement data are communicated, only low bandwidth is needed. 3 Related Work In [2], relative localization is shown using several receivers mounted on an airplane. Therewith, high accurate GPS carrier phase measurement differencing can be done, e.g. using an analog circuit. Sanguino proposed a centralized localization scheme in [3], where pedestrians are tracked using both GPS time of flight and carrier phase measurements (L 1/2 ). In [4], GPS and additional radio ranging sensors are used to improve the localization accuracy and reliability. A tightly coupled GPS/ INS integration has been proposed by [5], by which drift errors can be reduced significantly. A cooperative lane-level positioning approach using GPS and intervehicle communications is shown in [6]. In [7], the local estimates of all participants are exchanged as the only common information in a decentralized network. The raw measurements are not directly used for the cooperative localization. 4 Algorithm Framework In this chapter, the algorithmic framework used for estimating relative vectors from GPS pseudorange and inertial sensor measurements is shown. The main filter technology used is a Bayesian Particle Filter implementation which is described briefly. Hereafter, the ego motion model is depicted in short and pseudorange double differencing is introduced. 4.1 Filter Technology Particle Filters belong to the class of Bayes filters which recursively estimate the state x k of a certain system from one time step t k 1 to the next t k. While Kalman Filters represent the probability density function (pdf) by parameters (mean and covariance) Particle Filters are based on Sequential Monte Carlo Methods (SMCM) [8], [9] and therefore heavily rely on samples which are also called particles. In contrast to the Unscented Kalman Filter [10] which represents the pdf with a fixed number of deterministically drawn particles, Particle Filters use a large number of randomly generated samples to represent the pdf or density of the system. Due to the use of this non-parametric representation, Particle Filters are very suitable for non-linear and non-gaussian applications, but also for systems with a possibility of ambiguities. A typical particle filter algorithm consists of the following steps: sampling step: generation of new particles where each particle is drawn from an importance function π(x) update importance weights: weights ω (i) calculation of particles resampling step: draw M particles x (i) from set S k according to resampling algorithm First of all a representation of a system and measurement model x k = f(x k 1 ) + u k (4) z k = g(x k ) + v k (5) 127

3 is assumed. Here, x k is the state vector of interest, while z k represents the vector of observations. u k and v k are both independent noise vectors with known distributions. f( ) and g( ) are known (maybe non-linear) functions. p(x k x k 1 ) represent the state transition probability and p(z k x k ) the measurement likelihood function. Each particle consists of a certain state x i and an importance weight ω i. The single particles which represent a concrete instance of the state space are combined to the set { } S k = x (i) k, ω(i) k i = 1,...,N p (6) which represents the pdf p(x k Z k ). Here, X k = {x i, i = 0,...,k} contains all states and Z k = {z i, i = 0,...,k} all measurements up to time step k. For a large number of particles N p the approximation of the pdf p(x k, Z k ) is then given by N p p(x k Z k ) ω (i) k δ(x k X (i) k ) (7) i=1 where the weights are normalized such that i w(i) k = 1. During a typical initialization phase the particles are drawn from an initial distribution p 0 (x) x (i) 0 p 0 (x) (8) while the weights ω (i) 0 are set to 1/N p. For a Bayesian Filter framework using first order Markov assumption the transitional density p(x k x k 1 ) corresponds to the system update equation (4). It is used to predict the particles to the next time step t k. When a new measurement z k arrives, all particles need to be judged based on the likelihood function p(z k x (i) k ): ω (i) k = p(z k x (i) w(i) k )p(x(i) k x(i) k 1 ) k 1 q(x (i) k x(i) k 1, z k) Here, q(x (i) k x(i) k 1, z k) is the proposal distribution where to sample from. The new posterior pdf p(x k Z k ) with incorporated measurement z k is then approximated by: N p i=1 (9) p(x k Z k ) ω (i) k δ(x k x (i) k ) (10) The described approach is problematic since the variance of the weights ω (i) will increase over time which leads to the so called degeneracy phenomenon. In consequence only a few particles will contribute to the approximation of the posteriori pdf while most have an importance weight near zero. To overcome this limitation, the resampling step is necessary. In short, resampling should duplicate particles having a high weight and eliminate particles containing weights near zero. After resampling the weights of all remaining particles are usually set to 1/N p. Typical resampling algorithms are multinomial sampling and systematic sampling [11]. In this paper KLD-sampling [12] is used since it offers quite equal performance and the additional benefit of an adaptive number of particles. KLD-sampling uses the Kullback-Leibler (KL) divergence which is a measure of difference between two probability distributions p and q: K(p, q) = x p(x)log p(x) q(x) (11) In contrast to [12], a slightly different algorithm is used instead. The prediction and resampling are separated into two independent steps. Hence, a constant number of particles is firstly predicted via the motion model, while the subsequent resampling step may adapt the number of particles. 4.2 Pseudorange Double Differences Consider two vehicles a and b each equipped with one GPS receiver measuring the pseudorange to one satellite S. As shown in [2], a pseudorange single difference can be calculated by ρ (s) = ˆρ (s) a ˆρ (s) b = d e (s), (12) where d denotes the difference vector between the vehicles and e S the unit vector pointing from the receivers to the satellite. As the vehicles are supposed to be close to each other compared to the satellite distance, the unit vector for both vehicles can be treated to be equal. Additionally, the height difference between the vehicles is assumed to be zero, which leads to two dimensional difference vectors d = ( x y ). The error ρ e from (2) can be written as ρ e = T c, (13) the time error can be split into T = T s + T r + T ion + T trop + T mp + T v, (14) where T s and T r denote the clock error of the satellite and receiver, T ion the ionospheric and T trop the tropospheric error, T mp errors due to multipath effects and T v other errors. As stated in [13], ionospheric and tropospheric errors between two close receivers are highly correlated. Therefore, by taking the single difference between two pseudorange measurements those errors can be eliminated. A pseudorange double difference (see figure 2) between two receivers a and b and two satellites s 1 and s 2 can be expressed as the difference between two single differences ρ (s1,s2) = ρ (s1) ρ (s2) = d e (s1,s2) (15) 128

4 129

5 satellite error vehicle error RMSE std. deviation in m std. deviation in m in m Table 1: Influence of vehicle and satellite error to RMSE of estimation after T=250 time steps. RMSE [m] difference vector from absolute localization difference vector from relative localization satellite error σ s [m] Figure 4: RMSE of difference vector depending on satellite error σ s (const. vehicle error σ v = 3m) at time step T = 400 for differencing two absolute positions (cross line) and the proposed relative positioning approach (circle line). The likelihood for a measurement z is p( z x a ) N( z,g b,σ). (23) Σ is a diagonal covariance matrix as possible correlations due to ionospheric and tropospheric errors are removed by differencing in (12). 6 Results To evaluate the system presented in this paper, two vehicles are simulated using the CTRV model. Both vehicles are able to get the pseudoranges of four randomly chosen satellites. The components x and y of the difference vectors d b where initialized using a zero mean Gaussian with σ d = 50m, while the heading θ was set to uniform. As the unit vector differences e (s1,ss) are derived from the generally unknown absolute position of the vehicle, they where set using a very rough GPS fix with σ e = 1000m. Simulations have shown that the estimation is not very susceptible on that initial guess. In table 1 the ionospheric and tropospheric error (satellite error) compensation capability of the system is shown. While the upper part does not show a significant influence of the satellite error to the RMSE due to the error compensation by differencing, the lower part shows an increasing error due to increasing vehicle error (e.g. multi path). As another benchmark the presented cooperative approach has been compared to the straightforward method, which differences two absolute localization estimations obtained by tightly coupling GPS and INS [5]. To be comparable with the shown approach, only the expectations of the absolute estimations are used as the amount of data which is needed to communicate is similar to the raw pseudorange data. Figure 6 shows that the error of the system which differences two absolute positions is significantly higher than of the proposed relative positioning approach. An increasing satellite error amplifies this effect. References [1] R. Schubert, E. Richter, and G. Wanielik, Comparison and evaluation of advanced motion models for vehicle tracking, in Proceedings of the 11th International Conference on Information Fusion, June July [Online]. Available: arnumber= &isnumber= [2] F. Graas and M. Braasch, Gps interferometric attitude and heading determination: Initial flight test results, Navigation: Journal of the Institute of Navigation, vol. 38, No. 4, Winter , [3] J. E. Sanguino, Enhanced waw location-based service positioning, in The 11th International Symposium on Wireless Personal Multimedia Communications (WPMC 2008), [4] R. Parker and S. Valaee, Cooperative vehicle position estimation, in Proc. IEEE International Conference on Communications ICC 07, June 2007, pp [5] Z. Syed, P. Aggarwal, Y. Yang, and N. El-Sheimy, Improved vehicle navigation using aiding with tightly coupled integration, in Proc. IEEE Vehicular Technology Conference VTC Spring 2008, May 2008, pp [6] T.-S. Dao, K. Y. K. Leung, C. M. Clark, and J. P. Huissoon, Markov-based lane positioning using intervehicle communication, vol. 8, no. 4, pp , Dec

6 [7] L.-L. Ong, T. Bailey, H. Durrant-Whyte, and B. Upcroft, Decentralised particle filtering for multiple target tracking in wireless sensor networks, in Proc. 11th International Conference on Information Fusion, June July , pp [8] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman Filter Particle Filters for Tracking Applications. Artech House, [9] S. Thrun, Probabalistic Robotics, [10] S. J. Julier and J. K. Uhlmann, Unscented filtering and nonlinear estimation, Proceedings of the IEEE, vol. 92, no. 3, pp , [Online]. Available: murphyk/papers/ Julier Uhlmann mar04.pdf [11] J. Hol, T. Schön, and F. Gustafsson, On resampling algorithms for particle filters, in Nonlinear Statistical Signal Processing Workshop, Cambridge, United Kingdom, Sep [12] D. Fox, Adapting the sample size in particle filters through kld-sampling, International Journal of Robotics Research, vol. 22, p. 2003, [13] M. Grewal, L. Weill, A. Andrews, and J. Wiley, Global positioning systems, inertial navigation, and integration. Wiley New York, [Online]. Available: id=zm7mub8y35wc&printsec=frontcover&dq= grewal+mohinder+global+positioning [14] R. P. S. S. Blackman, Design and Analysis of Modern Tracking Systems. Artech House,