Multisensor Poisson Multi-Bernoulli Filter for Joint Target-Sensor State Tracking

1 Multisensor Poisson Multi-Bernoulli Filter for Joint T ar get-Sensor State T racking Markus Fr ¨ ohle, Christopher Lindberg, Karl Granstr ¨ om, Henk W ymeersch Abstract —In a typical multitarget tracking (MTT) scenario, the sensor state is either assumed known, or tracking is per - formed in the sensor’ s (r elative) coordinate frame. This assump- tion does not hold when the sensor , e.g., an automotiv e radar , is mounted on a vehicle, and the target state should be represented in a global (absolute) coordinate frame. Then it is important to consider the uncertain location of the vehicle on which the sensor is mounted f or MTT. In this paper , we present a multisensor low complexity Poisson multi-Bernoulli MTT filter , which jointly tracks the uncertain vehicle state and target states. Measurements collected by different sensors mounted on multiple v ehicles with varying location uncertainty are incorporated sequentially based on the arrival of new sensor measur ements. In doing so, targets observed from a sensor mounted on a well-localized vehicle reduce the state uncertainty of other poorly localized vehicles, provided that a common non-empty subset of targets is observed. A low complexity filter is obtained by appr oximations of the joint sensor-featur e state density minimizing the Kullback- Leibler divergence (KLD). Results from synthetic as well as experimental measurement data, collected in a vehicle driving scenario, demonstrate the performance benefits of joint vehicle- target state tracking. I . I N T RO D U C T I O N Intelligent transportation systems (ITSs) in general, and autonomous driving (AD) in particular, require accurate po- sition information [1]. Measurements provided by various on- board sensors allow to infer the vehicle state, e.g., position and velocity , as well as information about the surrounding en vironment. For instance, a global navigation satellite system (GNSS) receiv er provides absolute position, whereas a radar sensor provides relativ e position of a feature with respect to (w .r .t.) the sensor origin. Furthermore, vehicles hav e access to a pre-recorded local dynamic map (LDM) containing static features such as, e.g., landmarks [2]. Dynamic features such as pedestrians, cyclists, etc., are not part of the pre-recorded map. For an AD system to be fully aware of the surrounding en vironment, dynamic features need to be estimated from measurements and tracked over time, using the vehicle’ s on- board sensors, thus allowing to enrich the vehicle’ s LDM. In order to update the LDM by new features (static, dy- namic) described in a global coordinate frame, location uncer- tainty of the platform, where the sensors are mounted (e.g., M. Fr ¨ ohle, K. Granstr ¨ om, and H. W ymeersch are with the De- partment of Electrical Engineering, Chalmers University of T echnol- ogy , Gothenbur g, Sweden. C. Lindberg is with DENSO Sweden AB, Gothenbur g, Sweden. E-mail: { frohle, karl.granstrom, henkw, chrlin } @chalmers.se . This work was supported, in part, by the EU- H2020 project HIGHTS (High Precision Positioning for Cooperative ITS Applications) under grant no. MG-3.5a-2014-636537 and COPPLAR (campus shuttle cooperati ve perception and planning platform) project, funded under grant no. 2015-04849 from V innov a. a vehicle), needs to be considered. Furthermore, information from one vehicle can be utilized to increase location accurac y of other vehicles, and vice versa [3], [4]. In the literature, three different research tracks can be discerned: (i) MTT when both features and sensors are mobile, but the sensor states are known; (ii) simultaneous localization and mapping (SLAM) when the sensor state and feature state are unknown, but features are static; (iii) simultaneous localization and tracking (SLA T) combines MTT and SLAM by considering unknown mobile feature and mobile sensor states. All three tracks include measurements due to clutter; missed detections; unknown measurement-to-target correspondence; and target appearance and disappearance. MTT filters based on random finite set (RFS) and finite-set statistics (FISST), see, e.g., [5], [6] for recent works and [7], [8] for earlier works, hav e gained much attention. F or example, the probability hypothesis density (PHD) filter propagates the first moment of the RFS density over time [9]–[11]. The Poisson multi-Bernoulli mixture (PMBM) filter models unknown, i.e., nev er detected, features by a Poisson process and detected features by a multi-Bernoulli mixture (MBM) [12]. Based on this, the track-oriented marginal multiple tar- get multi-Bernoulli/Poisson (TOMB/P) filter approximates the global joint data association (DA) by the product of marginal D As, similar to the joint probability data association (JPDA) filter , which allows a computationally ef ficient implementation [12]. In [13], a deriv ation of the PMBM filter based on standard single target measurement models, without using probability generating functionals (p.g.fl.s) and functional deriv ati ves, is presented. Furthermore, connections to the δ - generalized labeled multi-Bernoulli ( δ -GLMB) filter [14], [15] are discussed. In contrast to the above FISST approaches, [16], [17] proposed a factor graph (FG) based approach for a variant of the JPD A filter . A multi-scan scenario was considered, and the filter was realized by running loopy belief propagation on the FG containing cycles. Additionally , similarities between the FG approach and the TOMB/P filter are discussed. For static features (called landmarks) observed by a sensor with unknown state, SLAM based methods can be employed to jointly estimate the sensor state and the landmarks (see, e.g., [2], [18]). In [19] and [20], an RFS based approach to the SLAM problem was proposed. The landmark state is conditioned on the sensor location and then tracked through a PHD filter following a Rao-Blackwellization (RB). Joint estimation of the unknown sensor and mobile feature states, also termed SLA T, was for instance addressed in [21], where a combination of static and mobile features are estimated through MTT. In doing so, a particle based repre- 2 sentation of the sensor state is combined with a parametric formulation of the PHD filter . Related to this, [22], [23] considers the problem of sensor state estimation through target tracking in the RFS framework by combining local PHD filters with the help of belief propagation. T o achiev e distributed processing, approximations in terms of separable likelihoods are taken. In [24], a particle based MTT filter is presented for SLA T in wireless sensor networks. Here, the measurement- to-target D A is known, but corrupted by noise leading to false alarms. In [25], a message passing based distributed multisensor MTT filter modeling target and sensor states by Gaussian probability density functions (PDFs) is presented. Measurement-to-target D A is known and targets are always present and no false alarm measurements occur . Similar to [17], a FG based approach was considered in [26] for an urban ITS scenario, where the number of features is assumed a priori known, and in [4] where the D A assumed to be known as well. In this paper, we consider the problem of multisensor SLA T for joint estimation of the unknown sensor and feature states, enabling accurate feature tracking w .r .t. the sensor uncertainty . Our proposed MTT filter b uilds upon the Bayesian RFS based PMBM filter [12] with proper feature birth and death processes, but explicitly models the sensor state uncertainty . A low complexity filter is obtained by approximations of the joint sensor-feature state density minimizing the KLD. A tractable implementation is achiev ed through approximations similar to the TOMB/P filter [12]. The proposed MTT filter allows to track the state of the sensor platform not only by (direct) measurements of the platform itself, but also through feature tracking in a multi-sensor setup. The main contributions are: • A low-comple xity asynchronous multisensor MTT filter with uncertain sensor state information, • Sensor state tracking by fusion of multisensor MTT information with local sensor information, • V alidation of the filter with real sensor data, as well as in-depth analysis of performance with synthetic data. The remainder of this paper is organized as follo ws; Section II giv es some background knowledge on RFS, and Section III introduces the problem formulation and system models. Section IV details the proposed MTT filter with uncertain sensor state, including the multisensor generalization of the proposed MTT filter . Results with synthetic data are giv en in Section V and with experimental data in Section VI, respectiv ely . Conclusions are drawn in Section VII. Notation Scalars are described by non-bold letters r , vectors by lower -case bold letters x ; matrices and sets by upper-case bold letters X . The cardinality of set X is denoted | X | . The set operator ] denotes the disjoint set union, i.e., X u ] X d = X means X u ∪ X d = X and X u ∩ X d = ∅ . The vehicle/sensor state is reserved by letter s , the feature state by letter x , and measurements by letter z . The identity matrix of size n × n is denoted I n . The ` 2 -norm of vector x is k x k 2 . I I . B A C K G RO U N D O N R F S In this section, we describe some useful properties of an RFS. If not stated otherwise, the source of all these is [12]. A. Random F inite Set F ormulation According to [12], RFS based methods have been de veloped in [5] to conduct statistical inference in problems in which the variables of interest and/or observations form finite sets. In tracking, they address two major challenges of interest: (i) the number of targets present in the scene is unknown, (ii) measurements are in variant to ordering (measurement-to- target correspondence is unknown). An RFS X is a finite-set valued random variable, which can be described by a discrete probability distribution p ( n ) , where n ≥ 0 denotes the number of elements x i ∈ X for i = 0 , . . . , n and a family of joint PDFs p n ( x 1 , . . . , x n ) yielding [12] f ( { x 1 , . . . , x n } ) = p ( n ) X π p n ( x π (1) , . . . , x π ( n ) ) , (1) where the sum spans over the n ! permutation functions π ( · ) , such that its RFS density f ( X ) is permutation inv ariant. The set integral of a real-valued function g ( X ) of a finite-set variable X is defined as [5, p. 361], [12] Z g ( X ) δ X , g ( ∅ ) + ∞ X n =1 1 n ! Z g ( { x 1 , . . . , x n } )d x 1 · · · d x n . (2) T wo important examples of RFSs are Bernoulli processes (and their generalization, multi-Bernoulli (MB) processes) and Poisson processes. A Bernoulli process X with probability of existence r and existence-conditioned PDF p ( x ) has RFS density f ( X ) =      1 − r, if X = ∅ , r · p ( x ) , if X = x , 0 , otherwise . (3) The RFS density of a multi-Bernoulli (MB) process X can be expressed as f ( X ) = X ] i ∈ I X i = X Y i ∈ I f i ( X i ) (4) for | X | ≤ | I | , and f ( X ) = 0 otherwise. Here, I is the index set of the MB with components { r i , p i ( x ) } i ∈ I . A MBM is a linear combination of MBs with density expressed by the Bernoulli components f ( X ) = X j ∈ J w j f j ( X ) , (5) where w j is the weight of the j -th MB with density f j ( X ) (such that P j w j = 1 ), and J is the index set of the MBM. A Poisson point process (PPP) with intensity function λ c ( y ) has RFS density [12] f ( Y ) = e −h λ c , 1 i Y y ∈ Y λ c ( y ) (6) with inner product h λ c , h i , R λ c ( y ) h ( y )d y . Remark 1: If X and Y are independent RFSs such that Z = X ] Y , then f Z ( Z ) = X X ] Y = Z f X ( X ) f Y ( Y ) . (7) 3 Note, when X follo ws an MBM process and Y an PPP (7) is called a PMBM density and Poisson multi-Bernoulli (PMB) density for | J | = 1 . Remark 2: A common way to estimate the set states from a Bernoulli process with RFS density f ( X ) is by comparing the probability of existence r to an existence threshold r th . For r > r th , the target is said to e xist and has PDF p ( x ) (c.f. (3)). Its state can then be estimated by the mean of p ( x ) , i.e., ˆ x = R x p ( x )d x . See [13, Sec. VI], for an extended discussion on state estimation. B. Bayesian F ilter F ormulation Similar to the random v ector (R V) case, an RFS based filter can be described, conceptually at least, within the Bayesian framew ork with alternating prediction and update steps oper- ating on the state RFS X with [5, Ch. 14] f + ( X ) = Z f ( X | X 0 ) f − ( X 0 ) δ X 0 (8) and f ( X | Z ) ∝ ` ( Z | X ) f + ( X ) , (9) where f − ( X 0 ) is the prior RFS density , f ( X | X 0 ) is the RFS transition density , f + ( X ) is the predicted RFS density , and ` ( Z | X ) is the RFS measurement likelihood for measurement set Z . C. T wo Useful Lemmas Lemma 1: A joint density f ( X , Y ) is approximated in the minimum KLD sense by [27, Ch. 10] f ( X , Y ) ≈ Z f ( X , Y ) δ X Z f ( X , Y ) δ Y . (10) Lemma 2: The set integral can be expressed as [28, Lemma 5] Z X X ] Y = X f ( X ) g ( Y ) δ X = Z f ( X ) δ X Z g ( Y ) δ Y . (11) I I I . M OT I V A T I O N , P R O B L E M F O R M U L A T I O N A N D S Y S T E M M O D E L S Here, we first present the motiv ation and problem formu- lation, as well as the vehicle and feature dynamics. This is followed by the GNSS and vehicle-to-feature (V2F) measure- ment models, and the communication assumption. A. Motivation W e consider an urban ITS scenario, consisting of cooper- ating vehicles (illustrated in Fig. 1). Each vehicle is equipped with a GNSS-type receiv er to determine its absolute position and radar-type sensor to retriev e relative positions of mobile features present in the en vironment through V2F measure- ments. Our goal is to develop a filter, which runs on an road side unit (RSU), to track the features and the states of all vehicles in e very discrete time step t through incorporation of all measurements provided by the vehicle’ s on-board GNSS and V2F sensors up until time step t . Fig. 1. Urban ITS scenario with two vehicles (red color) cooperating through the RSU (green color) and fiv e mobile features (blue color). B. V ehicle and F eatur e Dynamics V ehicle state motion follows independent identically dis- tributed (IID) Markovian processes, where the time-varying vehicle state s v ,t of each vehicle v ∈ V at time step t is statistically modeled as p ( s v ,t | s v ,t − 1 ) . Each feature k ∈ K with state x k,t − 1 surviv es to the next time step t following an IID Markovian process with survi v al probability p S ( x k,t ) . The feature state motion follows IID Markovian processes and is statistically modeled as p ( x k,t | x k,t − 1 ) . Note that vehicle and feature state motion are independent. 1 In the following, we will drop the subscript indexing on states and measurements w .r .t. vehicle/feature/time whenev er the context allows. C. Measur ement Models At time step t , vehicle v ∈ V obtains two different kinds of measurements: (i) measurements of the vehicle state s w .r .t. the reference frame, i.e., GNSS-like measurements z G modeled by p ( z G | s ) ; and (ii) measurements w .r .t. to features, i.e., from a radar-like (on-board) V2F sensor . W ithout loss of generality , we assume that the on-board sensors’ state is equal to the vehicle state. Furthermore, the measurement-to- feature state correspondence is not known and we assume that the standard MTT measurement assumptions for point targets apply , i.e., each feature generates at most one measurement per measurement scan and sensor , and each measurement has a unique source (feature or clutter). Let Z be a set of V2F measurements taken by a vehicle at a certain time. This set can be expressed as Z = Z F A ] Z D , where Z F A denotes the set of false alarm measurements due to clutter (modeled by a PPP with intensity λ c ( z ) ) and Z D denotes the set of feature measurements. Let a V2F mea- surement z ∈ Z D be obtained through the V2F sensor with 1 Note that the proposed filter only predicts over small time-horizons in the order of a few tens of milliseconds. Then the assumption on vehicle and feature states evolving independently is reasonable, because there is very little interaction among them within the prediction horizon. 4 likelihood function g ( z | s , x ) . The likelihood for measurement set Z = { z 1 , . . . , z m } is defined as in [13, Eqn. 13] by l ( Z | X , s ) = e −h λ c ;1 i X U ] X 1 ... ] X m = X [1 − p D ( s , · )] U × m Y i =1 ˜ l ( z i | X i , s ) (12) with ˜ l ( z | X , s ) =      p D ( s , x ) g ( z | s , x ) X = { x } , λ c ( z ) X = ∅ , 0 | X | > 1 . (13) Here, the feature set has been decomposed into all possible sets U ] X 1 . . . ] = X , where the set U represents unkno wn features and sets X i represent the origin of the i th measure- ment, which can either be a singleton containing the state of the feature that gave rise to the measurement or an empty set if the measurement is clutter, i.e., | X i | ≤ 1 ∀ i . Above, p D ( s , x ) denotes the probability of detection, which depends on the vehicle state s as well as on the feature state x . For instance, a limited sensor field-of-view (FoV) af fects the probability of feature detection based on the distance between vehicle and feature. An alternativ e, but equiv alent form to represent the likelihood (12), is [13, Eqns. 25, 26] l o ( Z | U , X 1 , . . . , X n , s ) = X Z 1 ... ] Z n ] Z u = Z l ( Z u | U , s ) × n Y i =1 t ( Z i | X i , s ) , (14) where | X i | ≤ 1 for i = 1 , . . . , n . Here, l ( Z u | U , s ) was defined in (12) and t ( Z i | X i , s ) =          p D ( s , x ) g ( z | s , x ) Z i = { z } , X i = { x } , 1 − p D ( s , x ) Z i = ∅ , X i = { x } , 1 Z i = ∅ , X i = ∅ , 0 otherwise . (15) D. Communication W e assume that e very vehicle is able to communicate all obtained measurements (V2F and GNSS) with the RSU instantaneously and without errors. This implies that at any time step t the number of vehicles communicating with the RSU can vary . The incorporation of a realistic vehicle-to- infrastructure (V2I) channel model and its performance impact is outside the scope of this work. I V . P RO P O S E D F I L T E R In this section, we formulate the proposed Bayesian filter with uncertain vehicle (sensor) state. For complexity reasons, we aim for a recursiv e formulation with a factorized joint density ov er the vehicle and feature states. A. Prior Joint State Density The vehicle state PDF at time step t − 1 is indicated by subscript ’ − ’, i.e., p − ( s ) , the PDF predicted to the current time step t (before updating by a measurement) is indicated by subscript ’ + ’, i.e., p + ( s ) , and the posterior PDF is stated without subscript. Similar notation holds w .r .t. the feature RFS density . W ith a single vehicle, the prior joint vehicle-feature density is of form f − ( s , X ) = p − ( s ) f − ( X ) , (16) where p − ( s ) is the prior PDF on the vehicle state, and f − ( X ) is the prior PMBM density . The latter density can be written in terms of a PPP density of unknown features f u − ( X u ) , i.e., features which are hypothesized to exist but have nev er been detected [12, Def. I], and the prior MBM RFS density of detected features, as [5, p. 484], [12] f − ( X ) ∝ X X u ] X 1 ] ... ] X n = X f u − ( X u ) X j ∈ J Y i ∈ I j w j i f j i − ( X i ) , (17) where the w j i and f j i − ( · ) are the weight and Bernoulli density of potentially detected feature i under global hypothesis j . Note that the weight of a global hypothesis j is proportional to the single hypothesis weights by Q i ∈ I j w j i . In (17), the PPP density of unknown features is f u − ( X u ) = e − h D u − , 1 i Y x ∈ X u D u − ( x ) , (18) where D u − ( x ) is the intensity of unknown features. B. Pr ediction Step Due to the independent mobility of vehicle and features, the predicted joint vehicle-feature density is f + ( s , X ) = p + ( s ) f + ( X ) , (19) where p + ( s ) is given by the Chapman-K olmogorov equation p + ( s ) = R p ( s | s 0 ) p − ( s 0 )d s 0 , where p ( s | s 0 ) is the state transition PDF and p − ( s 0 ) is the prior PDF [29]. Similarly , the predicted feature state PMB density is calculated by [5, Ch. 14.3] f + ( X ) = Z f ( X | X 0 ) f − ( X 0 ) δ X 0 , (20) where f ( X | X 0 ) is the transition RFS density , and f − ( X 0 ) is the prior PMB density . It can be shown that (20) is a PMB density where the predicted intensity of unkno wn features D u + ( x ) is given by [12, Thm. I] D u + ( x ) = D b ( x ) + Z p S ( x 0 ) p ( x | x 0 ) D u − ( x 0 )d x 0 . (21) Here, the (known) birth intensity is denoted D b ( x ) , p ( x | x 0 ) denotes the feature transition PDF, and the feature surviv al probability is denoted p S ( x 0 ) . The Bernoulli components of the MBM are updated as follows [12, Eqn. (40)]: p j i + ( x ) = Z p S ( x 0 ) p ( x | x 0 ) p j i − ( x 0 )d x 0 (22) r j i + = r j i − Z p S ( x 0 ) p j i − ( x 0 )d x 0 . (23) 5 where p j i − ( x 0 ) denotes the prior PDF of the j i -th Bernoulli component. C. Measur ement Update Step Updating the joint vehicle-feature density (19) by any of the two types of different measurements, GNSS and V2F measurements, in volv es the application of Bayes’ theorem. In the follo wing, we describe the update calculations using the different type of measurements. 1) Update with vehicle state measurement: Let z G be a measurement related to the vehicle state s through p ( z G | s ) . Giv en a predicted vehicle-feature density (19), by Bayes’ theorem the updated density is f ( s , X | z G ) = p ( z G | s ) p + ( s ) R p ( z G | s 0 ) p + ( s 0 )d s 0 f + ( X ) . (24) In other words, the vehicle state density is updated with the measurement z G , the feature set density is unaf fected by the update, and the independent form is retained (c.f. (16)). 2) Update with clutter ed set of feature measur ements: Let Z be the V2F measurement subject to the model defined in Section III-C. Furthermore, we assume the probability of detection to be state-independent, i.e., p D ( s , x ) = p D . W ith the help of Bayes’ rule, the updated joint vehicle- feature density is f ( s , X | Z ) ∝ p + ( s ) f + ( X ) l ( Z | s , X ) . (25) Due to the dependency of the V2F measurements on X and s , the updated posterior density (25) cannot easily be decom- posed into the independent form of the prior joint density (16). In order to enable joint vehicle-feature state tracking with: (i) low complexity; and (ii) standard MTT framew orks such as, e.g., [12], we calculate (25) approximately . W ith the help of Lemma 1 the joint vehicle-feature density is approximated as f ( s , X | Z ) ≈ p ( s | Z ) f ( X | Z ) , (26) where p ( s | Z ) ∝ Z p + ( s ) f + ( X ) l ( Z | s , X ) δ X , (27) f ( X | Z ) ∝ Z p + ( s ) f + ( X ) l ( Z | s , X )d s . (28) In this way , the independent structure of the vehicle and feature density (16) is retained. Note that it has been observed in the SLAM context that a factorization such as (26) can generate optimistic estimates about the state uncertainty [18], [30]. An alternative to the approximation (26) is to perform a Rao-Blackwellization as done in SLAM [18], [21]. Although more accurate, the extension of the multi-vehicle scenario is not apparent and opponent to a low complexity implementation we seek. Now , only computation of the marginal posteriors remains. This is deri ved in the next two subsections. For con venience, let X Z ( z ) = ( 0 z 6∈ Z , 1 z ∈ Z , (29) and δ ∅ ( Y ) = ( 0 Y 6 = ∅ , 1 Y = ∅ . (30) D. Mar ginal F eature Set P osterior The feature posterior f ( X | Z ) of (26) is given by the following Theorem. Theor em 3: The feature posterior for measurement set Z = { z 1 , . . . , z m } can be approximated by a PMBM with density f ( X | Z ) ∝ ∼ X Y ] X 1 ... ] X n ] X 1 ... ] X m = X f ( Y ) X j × X Z 1 ... ] Z n ] Z u = Z m Y i =1 [ X Z u ( z i ) ρ ( z i ) f ( X i | z i ) +(1 − X Z u ( z i )) δ ∅ ( X i )] × n Y i =1 w j i ρ j i ( Z i ) f j i ( X i | Z i ) , (31) where ∝ ∼ means approximately equal to. Here, f ( Y ) represents the PPP component, and the functional forms of the densities f ( X i | z i ) and f j i ( X i | Z i ) , as well as the constants ρ ( z i ) , ρ j i ( Z i ) are provided in Appendix A. Pr oof: See Appendix A. The feature posterior (31) consists of undetected features ( Y ), hypotheses for newly detected features ( X i ) and for updating existing features ( X i ). This structure is of the same form 2 as for a known vehicle (sensor) state, as in [13], and is thus amenable for using a standard PMBM filter imple- mentation [12]. The weights of newly detected features are considered in the product ov er m components (i.e., one new feature per measurement). A pre viously detected feature has an updated weight w j i ρ j i ( Z i ) consisting of the pre vious single hypothesis weight and, depending on whether Z i contains a measurement, the hypotheses for a detection or a miss of the feature. Note that (31) is only approximately f ( X | Z ) due to the approximations performed in the deriv ations. Therefore, the weights are also only approximate weights for f ( X | Z ) . E. Mar ginal V ehicle P osterior W e now proceed with the vehicle state posterior, which is stated in the following Theorem. Theor em 4: The v ehicle state posterior for measurement set Z is p ( s | Z ) ∝ ∼ X j X Z 1 ... ] Z n ] Z u = Z Y z i ∈ Z u X Z u ( z i ) ρ ( z i ) × n Y i =1 w j i ρ j i ( Z i ) q j ( s | Z 1 , . . . , Z n , Z u ) (32) where q j ( s | Z 1 , . . . , Z n , Z u ) is a properly normalized density defined in Appendix B. Pr oof: See Appendix B. 2 For the linear Gaussian case, the vehicle state uncertainty is essentially mapped onto the V2F measurement uncertainty , which can be seen as an increased time-varying measurement variance. 6 From the vehicle posterior (32), we observe that the single hypothesis weights ρ ( z i ) and w j i ρ j i ( Z i ) of each term are the same as in the feature posterior (31), only the order of integration ov er the vehicle state s and the single feature state x is exchanged. Hence, both posteriors use the same approximate single hypothesis weights. Furthermore, we hav e obtained that a global hypothesis is approximately proportional to the product of the approximate single hypothesis weights. F . Implementation Aspects Here, we discuss approximations for a practical implemen- tation to perform the sensor state V2F measurement update when the probability of existence of detected features is high. Furthermore, we discuss the approximation of the joint D A and the reduction of the feature posterior density to contain only a single global hypothesis. Using this approximation, the complexity of the proposed filter is briefly discussed. 1) Certain F eatur e Information: The spatial PDF of the PPP modeling the undetected features needs to co ver the whole space where new features appear . Due to this, a newly detected feature does not provide certain information to update the vehicle state and may be neglected in the vehicle state update. Furthermore, one can approximate the vehicle state update by considering only previously detected features with a high existing probability (c.f. Sec. 2). When detection probability is high, (32) can be approximated by p ( s | Z ) ∝ ∼ X j X Z 1 ... ] Z n ] Z u = Z (33) × n Y i =1 w j i ρ j i ( Z i ) ! q j ( s | Z 1 , . . . , Z n , Z u ) , where q j ( s | Z 1 , . . . , Z n , Z u ) ∝ ∼ p + ( s ) n Y i =1 (1 − δ ∅ ( Z i )) Z g ( z i | x i , s ) p j i ( x i )d x i . (34) 2) Mar ginal Association: For the feature update, the global hypothesis weights are proportional to Q z i ∈ Z u ρ ( z i ) Q n i =1 w j i ρ j i ( Z i ) . For the vehicle state update, the global hypothesis weight is proportional to Q n i =1 w j i ρ j i ( Z i ) ). A global hypothesis over all features can be approximated by the product of marginal association weights using the FG approach of [31]. For the feature state update, the TOMB/P filter [12] permits the reduction of the posterior PMBM density to a PMB density containing only a single global hypothesis (so that the summation ov er j disappears in the next time step). For the v ehicle state update, each likelihood term is weighted w .r .t. the single hypothesis. From the feature state update, the obtained marginal data association weights for the feature update can be reused in the vehicle state update, since they use the same weights for the same hypotheses. The weighted likelihoods times the prior vehicle state is then approximately proportional to the vehicle state posterior conditioned on the measurement set Z . 3) Complexity: For Gaussian linear models, a GNSS mea- surement update (c.f. (24)) has same complexity as a Kalman filter update. Using the marginal association proposed by the TOMB/P filter , the update of the feature state density by V2F measurements has comparable complexity as an update step of the TOMB/P filter with the added complexity of the marginalization over the sensor state (c.f. Appendix A). For the vehicle state update with the V2F measurements we hav e the same number of hypotheses and hence similar complexity . G. Multi-V ehicle Generalization of Pr oposed F ilter Up to this point, we discussed joint vehicle-feature state tracking using a single vehicle, where GNSS and V2F mea- surements are incorporated. T o achieve feature tracking as described in Section III-A, where a GNSS and a V2F sensor is mounted on each vehicle, we hav e to consider the multi- vehicle/multisensor case, where each vehicle is equipped with a GNSS and a V2F sensor . Since GNSS measurements are straightforward to deal with (they can be applied prior to the V2F measurements), we focus only on V2F measurements, considering a two-v ehicle case. There are different approaches to handle the multi-v ehicle setting, where we highlight two of them next. 1) P arallel Approac h: Given time-synchronized measure- ments from 2 vehicles (with measurements Z 1 collected by vehicle 1 and Z 2 collected by vehicle 2 , respectively), the joint posterior can be approximated by f ( s 1 , s 2 , X | Z 1 , Z 2 ) ≈ p ( s 1 , s 2 | Z 1 , Z 2 ) f ( X | Z 1 , Z 2 ) (35) similar to the approximation (26). Note that the set of global hypotheses is now the Cartesian product of the individual v e- hicle’ s set of hypotheses. Sev eral different approaches can be employed to tackle this D A problem in a tractable manner . For instance, by employing sequential sensor-by-sensor measure- ment updates (also called iterator-corrector method) [5], or by partition of the measurement set into subsets associated with the Bernoulli components [32], or by performing variational inference [33], or by solving the D A in parallel on a sensor- by-sensor basis [16]. Note that in a system where sensors are spatially distributed (c.f. Sec. III-A) synchronization between sensors is inv olved and a sequential measurement update procedure may be used instead. 2) Sequential Appr oach: Here, we employ the sequential measurement update strategy together with the TOMB/P al- gorithm (c.f. Section IV -C). Note that in a real system sensors among different vehicles are dif ficult to synchronize and then this approach can be used. The update is first performed with respect to the first vehicle (c.f. Sec. IV -D and Sec. IV -E): f ( s 1 , X | Z 1 ) ∝ ∼ Z p + ( s 1 ) l ( Z 1 | s 1 , X ) f + ( X ) δ X × Z p + ( s 1 ) l ( Z 1 | s 1 , X ) f + ( X )d s 1 (36) = p ( s 1 | Z 1 ) f ( X | Z 1 ) . (37) 7 − 200 − 150 − 100 − 50 0 50 100 150 − 300 − 200 − 100 0 100 200 x dimension in m y dimension in m vehicle feature Fig. 2. One realization of two vehicle and fiv e feature trajectories for the experiment with synthetic data. Then, the density f ( X | Z 1 ) is used as a prior when performing the update with respect to the second vehicle: f ( s 2 , X | Z 1 , Z 2 ) ∝ ∼ Z p + ( s 2 ) l ( Z 2 | s 2 , X ) f ( X | Z 1 ) δ X × Z p + ( s 2 ) l ( Z 2 | s 2 , X ) f ( X | Z 1 )d s 2 (38) = p ( s 2 | Z 1 , Z 2 ) f ( X | Z 1 , Z 2 ) . (39) Using this method, subsequent vehicles will benefit from updated vehicle and feature information of preceding vehicles. Note that contrary to (35), the v ehicle states are approximated here as being independent of each other . In our application example (c.f. Section III-A), this means that an update of the joint vehicle-feature density , with measurements from a well-localized vehicle (certain vehicle state), results in an improv ement of feature tracking performance when prior in- formation on the features is lo w . An update of the joint vehicle- feature density , with measurements from a poorly localized vehicle (uncertain vehicle state), permits the reduction of the uncertainty of its own vehicle state when prior information on the features is high. V . R E S U LT S W I T H S Y N T H E T I C D A TA W e consider a scenario similar to the one outlined in Fig. 1, where we apply the proposed multisensor-multifeature state tracking filter presented in Section IV. A. Setup The state of vehicle v ∈ V at time step t is s v ,t = [ p T s,t , ˙ p T v ,t ] T with position p v ,t ∈ R 2 and velocity ˙ p v ,t ∈ R 2 . W e use a linear constant velocity (CV) model to model vehicle dynamics, where s s,t = As s,t − 1 + w s,t (40) with state-transition matrix A =  1 T s 0 1  ⊗ I 2 , (41) where T s = 0 . 5 s is the sampling time. Above, w s,t ∼ N (0 , W ) denotes the IID process noise with W = r  T 3 s / 3 T 2 s / 2 T 2 s / 2 T s  ⊗ I 2 , (42) where r = 0 . 05 m 2 . Above, ⊗ denotes the Kronecker product. W e set the initial state of vehicle 1 to s 1 , 0 = [0 , 200 , 0 , − 2] T and to s 2 , 0 = [0 , − 200 , 0 , 2] T for vehicle 2, respectiv ely . The state of feature k at time t , denoted x k,t ∈ R 4 , is comprised of Cartesian position and velocity , similar to the vehicle state s v ,t . There are at most fiv e features present, if not noted otherwise. Furthermore, feature dynamics follow the CV model with the same parameters used for the vehicles. T o generate a challenging scenario for D A, we initialize the feature states x k,t ∼ N (0 , 0 . 25 I 4 ) at time step t = 175 for all features k ∈ K and run the CV model forward and backward in time similar to [12, Sec. VI]. The first feature enters the scene after t = 0 , the second after t = 20 and so on. Once present, features stay aliv e for the remaining simulation time. A realization of vehicle and feature trajectories is shown in Fig. 2. For the GNSS measurements, we use the linear measure- ment model z G ,t = H G s t + r t , (43) where the observation matrix is H G = [1 0] ⊗ I 2 and r t ∼ N (0 , R ) denotes the measurement noise with R = σ 2 G I 2 . For vehicle 1 , we assume it has low location uncertainty using Real T ime Kinematic (R TK) with σ 2 G = 5 . 76 · 10 − 4 m 2 and for vehicle 2 high location uncertainty using Standard Positioning System (SPS) with σ 2 G = 12 . 96 m 2 , corresponding to a vehicle with high quality GNSS receiv er and one with low quality GNSS receiver 3 . In the single sensor case, only vehicle 1 is present, and in the multisensor case both vehicles are present, if not noted otherwise. For the V2F measurements, we use the linear measurement model z t = H 1 s t + H 2 x t + q t (44) with H 1 , H G and H 2 , − H G , and q t ∼ N (0 , Q ) with measurement noise covariance matrix Q = σ 2 V2F I 2 . Follo wing [12], we set the initial unknown fea- ture intensity to D u − ( x k,t ) = 10 N ( 0 , P ) , where P = diag([100 2 , 100 2 , 1 , 1] T ) to cov er the ranges of interest of the feature state. The feature birth intensity is set to D b ( x k,t ) = 0 . 05 N ( 0 , P ) , the average number of false alarms per scan to λ c = 10 , with uniform spatial distribution on [ − r max , r max ] with parameter r max = 500 m . Furthermore, the probability of surviv al is p S = 0 . 7 and the probability of detection is p D ( s v ,t , x k,t ) , p D = 0 . 9 . T o assess feature tracking per- formance, we use the optimal sub-pattern assignment (OSP A) metric with cut-off parameter c = 20 and order p = 2 [36]. The filter tracking performance for the vehicle state at time step t is assessed in terms of the position estimation error 3 According to [34] and [35], the x/y position accuracy of the GNSS recei ver R T3000 from O XTS is σ 2 G = 12 . 96 m 2 for SPS, σ 2 G = 2 . 0736 m 2 for Satellite-Based Augmentation System (SBAS), σ 2 G = 0 . 9216 m 2 for Differential Global Positioning System (DGPS), and σ 2 G = 5 . 76 · 10 − 4 m 2 for R TK. 8 0 2 4 6 8 10 12 0 5 10 15 20 σ 2 G in m 2 A vg. feature OSP A TOMB/P I, no sensor update TOMB/P II, no sensor update proposed, no sensor update proposed, sensor update Fig. 3. A verage feature OSP A for different values of GNSS measurement variance σ 2 G using the proposed MTT filter with sensor update from the tracked features to the sensor (proposed, sensor update) and without sensor update (proposed, no sensor update). For comparison, results using the TOMB/P filter ignoring sens or state uncertainty (TOMB/P I, no sensor update) and by increasing the V2F noise variance by the GNSS v ariance (TOMB/P II, no sensor update) are shown. e t = k p t, true − ˆ p t k 2 , where p t, true is the true v ehicle position and ˆ p t the mean estimate of the filter . W e analyze the proposed MTT filter w .r .t. feature tracking performance with sensor update from the tracked features to the sensor (proposed, sensor update), and without sensor update (proposed, no sensor update). For comparison, results are shown using the TOMB/P filter ignoring sensor state uncertainty [12], i.e., using the GNSS measurement as sen- sor state (TOMB/P I, no sensor update), and the TOMB/P filter ignoring sensor state uncertainty , but increasing the V2F variance by the GNSS measurement variance (TOMB/P II, no sensor update), which is possible in the considered linear measurement scenario. As a benchmark for vehicle localization performance, results from a centralized Kalman filter (KF) are plotted as well, where measurement-to-feature D A is kno wn and where the augmented state vector contains all vehicle and all feature states. This is denoted genie method. Furthermore, tracking performance using a local KF is plotted. The local KF performs filtering separately on the individual vehicle state using only GNSS measurements and does not estimate feature states. Note, the performance of the local KF can be considered as the worst-case performance on vehicle state estimation, since V2F measurements are not considered at all. B. Discussion First, we discuss the impact of an uncertain vehicle state on feature tracking performance using a single vehicle and multiple features. After that, we consider the multisensor- multifeature case from Section IV -G. 1) Impact of uncertain vehicle state on featur e tracking performance: For one simulation run, the features and v ehicle trajectories are outlined in Fig. 2. For the case the GNSS mea- surement variance is very small w .r .t. the V2F measurement variance, e.g., σ 2 G = 5 . 76 · 10 − 4 m 2 corresponding to R TK 0 0 . 2 0 . 4 0 . 6 0 . 8 1 0 2 4 6 8 10 σ 2 V2F in m 2 A vg. feature OSP A TOMB/P I, no sensor update TOMB/P II, no sensor update proposed, no sensor update proposed, sensor update Fig. 4. A verage feature OSP A for different values of V2F measurement variance σ 2 V2F . The GNSS noise variance is set to σ 2 G = 0 . 9216 m 2 . 0 0 . 5 1 1 . 5 2 2 . 5 3 0 0 . 2 0 . 4 0 . 6 0 . 8 1 Error in m CDF V1 (all filters) V2 (proposed, sensor update) V2 (local KF) V2 (genie method) Fig. 5. CDF plot of vehicle position estimation error . and σ 2 V2F = 0 . 42 m 2 , the feature tracking performance of the proposed filter is comparable with the TOMB/P filter with known sensor state. Hence, we will not focus on this case and refer the reader to [12] for performance results regarding the TOMB/P filter . In Fig. 3, the feature state OSP A av erage of one simulation run of 351 time steps av eraged ov er 50 Monte-Carlo (MC) trajectory realizations is plotted for different values of GNSS measurement v ariance σ 2 G for two variants of the proposed MTT filter as well as with the TOMB/P filter . For all filter variants, the increase of GNSS measurement variance leads to an increased vehicle state uncertainty with the effect of an increase of the av erage feature OSP A. This OSP A increase consists of two components. First, an increased feature state estimation error due to a higher value of σ 2 G . Second, this results in features appearing in the filter spatially close together w .r .t. the feature state measurement uncertainty ( σ 2 G and σ 2 V2F together) for a longer period of time around time step t = 175 . Hence, D A is more challenging with the effect of an increased feature OSP A in this regime. From the figure, we observe that not considering the present vehicle state uncertainty leads to the worst feature tracking 9 performance (TOMB/P I). Incorporating this uncertainty by increasing the V2F measurement variance improves feature tracking significantly . Modeling the present vehicle state un- certainty without artificially increasing the V2F measurement variance, as proposed in this paper, leads to a slightly better feature tracking performance (proposed, no sensor update). For the case the vehicle state is updated by the V2F measurement (proposed, sensor update), feature tracking performance dete- riorates. The reason for this is that feature and vehicle state become correlated after the V2F measurement update, which is not modeled by the proposed MTT filter . This leads to the conclusion that the sensor should not use V2F measurements to update its o wn state if no other vehicles hav e updated the sensor state in the previous time step. With more vehicles, this effect of correlation will be diluted. Similar observations hav e been reported in [26]. In Fig. 4, the av erage feature state OSP A is plotted for different values of V2F measurement noise variance σ 2 V2F using the different filter variants. W e observe that a higher V2F noise variance leads to an increased average feature OSP A value except for the T OMB/P filter which ignores sensor state uncertainty (TOMB/P I). In the former, this reduction comes from the fact that sensor state uncertainty is absorbed from the increased V2F noise variance. For all methods the single feature state estimation error increases and D A becomes more challenging. Since the sensor state uncertainty is negligible, all filter variants show the same performance. 2) Multisensor-multifeatur e tracking performance: Here, we limit the discussion on the two v ehicle (sensor) case since it is suf ficient to analyze the ef fect of updating the vehicle state using V2F measurements. In Fig. 5, the cumulati ve distrib ution function (CDF) of the vehicle position estimation error is plotted for the two vehicle scenario outlined in Sec. V -A. W e observe that for vehicle 1 , which has lo w GNSS measurement noise, all three filter methods deliver a similar performance. The reason for this is that, due to the high accuracy of GNSS measurements, not a lot of information (to improve the vehicle state) is provided from feature tracking. Moving our focus to vehicle 2 , we observ e that the error of the local KF is much higher compared to the central KF, which is caused by the high noise in the GNSS measurements. Due to the low estimation error of vehicle 1 ’ s state, there is relev ant position information in the system , which can be transfered from vehicle 1 to vehicle 2 via the features utilizing the V2F measurements. In 80% of all cases, the estimation error of vehicle 2 is below 0 . 5 m using the proposed filter, compared to 2 . 3 m using the local KF. Despite this great improvement of the proposed filter ov er the local KF, it does not achiev e the performance of the central KF (genie method), where the error is around 0 . 4 m . The reason for the dif ference is that the central KF has kno wledge of the correct DA, knows the true number of features present, and ignores clutter V2F measurements. Furthermore, it tracks any present correlations between features and vehicles not modeled by the proposed filter . The proposed filter needs to infer the measurement-to-feature DA, estimate the number of features currently present, and needs to appropriately handle clutter in the V2F measurement set Z . V I . R E S U L T S W I T H E X P E R I M E N TA L D A TA A. Experiment Description Measurement data w as recorded using the COPPLAR project test vehicle, a V olv o XC90 equipped with different kinds of automotive sensors. If not stated otherwise, the filter parameters are the same as described in Section V -A. The GNSS sensor is a high-precision Applanix POSL V , and the V2F sensor is a onboard stereo vision camera from Autoli v looking in the car dri ving direction, which pro vides detections of objects w .r .t. the sensor coordinate frame. Measurements from both sensors arri ve time-stamped, but are not syn- chronized. In order to obtain synchronized measurements, we linearly interpolated measurements from each sensor and sampled them at a lower rate of T s = 0 . 1 s . 4 Since the coordinate frame of GNSS and V2F sensor are different, we first mapped the GNSS measurements on the Cartesian coordinate system, and then used the heading measurements from the accurate GNSS sensor as ground truth to rotate and translate the V2F measurements on the same co- ordinate system. This procedure allo ws to use the measurement models of Section III-C without further modification. Due to the absence of an exact ground truth in this dy- namic measurement scenario, measurements were considered as ground truth and IID zero-mean Gaussian measurement noise was artificially added to the GNSS measurements with σ 2 G = 0 . 9216 m 2 for vehicle 1 corresponding to a DGPS receiv er and with σ 2 G = 12 . 95 m 2 for vehicle 2 corresponding to a SPS receiv er . Additionally , noise was added on the V2F measurements with Q t = 0 . 42 I 2 m 2 which is the worst case performance of the stereo vision camera for object positioning. In the scene (c.f. Fig. 6), there are two pedestrians (features) standing at an intersection and vehicle 1 and 2 are driving along perpendicular roads. Due to the limited sensor FoV of the camera, features become visible at approximately 50 m distance. Since we had only one physical vehicle, we recorded first sensor data obtained by driving along one lane segment and afterwards from the perpendicular lane segment. The two vehicle driving scenario was then obtained by adjusting the time basis of one lane recording. Due to this hardware limitation, the proposed filter had to be run offline. Note that additional results using the same hardware and the proposed filter hav e been reported in [37]. B. Discussion In Fig. 6, the trajectories of vehicle 1 (V1) and vehicle 2 (V2) are plotted together with the estimated feature and vehicle positions. V ehicle 1 mo ving from the lower right upwards the left of the figure, uses a DGPS receiv er , and is therefore able to track the features quite accurately . In contrast, v ehicle 2 , which uses a SPS receiv er , cannot contrib ute much in accurate feature tracking, but its V2F measurements allo w to transfer position information to update the vehicle state. From the figure, we observe that the estimated vehicle track is much closer to the true trajectory compared to a local KF using only GNSS 4 Note that this synchronization step is not needed, but it simplified analysis of the collected measurements w .r .t. filters’ estimates. 10 200 210 220 230 240 − 150 − 140 − 130 − 120 − 110 − 100 − 90 − 80 x dimension in m y dimension in m V1 estimate (local KF) V2 estimate (local KF) V1 estimate (proposed) V2 estimate (proposed) Feature estimates (proposed) V1 true position V2 true position Fig. 6. Estimated vehicle (V1, V2) and feature positions using a local KF and with the proposed MTT filter for one measurement realization. 0 5 10 15 20 25 30 0 1 2 3 4 time in s avg. error in m V1 & V2 observe features only V2 observes features V ehicle 1 (proposed) V ehicle 2 (proposed) V ehicle 1 (local KF) V ehicle 2 (local KF) Fig. 7. A verage v ehicle position estimation error is over time for experimental measurement data. measurements. As vehicle 2 approaches the intersection, its performance deteriorates and achie ves the same performance as the local KF. The reason for this is that v ehicle 1 has already passed all features at this point in time and cannot provide tracking information on the features. Since we assume mobile features, no significant position information towards vehicle 2 is provided after a few time steps. In Fig. 7, the vehicle position estimation error averaged ov er 50 measurement realizations is plotted over time for this two vehicle experiment. Here, we can clearly observe how the performance of vehicle 2 approaches the performance of vehicle 1 when both vehicles observe the features (until approximately 12 s time). It then deteriorates from 12 s to 16 s due to the absence of V2F measurements from vehicle 1 . After approximately 16 s , vehicle 2 has passed all features as well and performance of the proposed MTT filter equals the performance of the local KF. W e can conclude that with the proposed multisensor MTT filter , joint feature tracking provides relev ant position information towards the vehicle’ s position allowing to improve the positioning quality for vehicles with different/v arying position accuracies. V I I . C O N C L U S I O N S This paper presented a low-comple xity Poisson multi- Bernoulli filter to jointly track multiple features (tar gets) as well as the state of multiple mobile sensors. This was enabled by an approximation minimizing the Kullback-Leibler div ergence. T wo different kinds of sensors providing obser- vations of the sensor state itself as well as observations of features enable accurate feature and sensor state tracking. The resulting filter incorporates the uncertain sensor state in the feature tracking task by marginalizing over the uncertain sensor state in the single feature state likelihood. Information from multiple sensors is incorporated by asynchronous update steps, executed whenever sensor measurements arrive at the central node. In doing so, data association is limited to a per- sensor basis. Furthermore, in a multi-vehicle scenario with varying sensor qualities, an update of the uncertain sensor state is achieved by measurements of the sensor state and by means of feature tracking. Simulation and experimental results showed that through the incorporation of measurements provided by dif ferent sensors, the feature tracking performance is superior to the track-oriented marginal multiple tar get multi-Bernoulli/Poisson (TOMB/P) filter which ignores sensor uncertainty , and comparable when its measurement noise is artificially increased. Furthermore, we observed in a multi- vehicle scenario that through joint vehicle-feature state track- ing the vehicle state uncertainty can be significantly reduced compared to track the vehicle state alone. Applications of the proposed low-comple xity filter inv olve cooperative vehicle driving scenarios when both information of the en vironment and the vehicle themselves are of interest. Furthermore, no direct v ehicle-to-vehicle observ ations are needed, which makes it interesting for urban en vironments with applications such as extending the situational aw areness beyond the visibility of the ego vehicle alone. A P P E N D I X A P RO O F O F T H E O R E M 3 Plugging (17) into (28) results in f ( X | Z ) ∝ Z p + ( s ) X U ] X 1 ... ] X n = X f u + ( U ) X j × n Y i =1 w j i f j i ( X i ) l ( Z | X , s )d s . (45) 11 Now we add the constraint | X i | ≤ 1 ∀ i , since f j i ( X i ) is Bernoulli and then replace the likelihood by (14) leading to (c.f. [13]) f ( X | Z ) ∝ X U ] X 1 ... ] X n = X X j X Z 1 ... ] Z n ] Z u = Z f u + ( U ) (46) × Z p + ( s ) l ( Z u | U , s ) n Y i =1 w j i f j i ( X i ) t ( Z i | X i , s )d s | {z } ≈ q ( Z u | U ) Q n i =1 w j i q ( Z i | X i ) f j i ( X i ) , (47) where the approximation follo ws by in voking Lemma 1, whereby the factors are found as follows: Using Lemma 1 again, we find q ( Z u | U ) ≈ X Y ] X 1 ... ] X m = U e −h λ c ;1 i [1 − p D ] Y m Y i =1 ˜ l ( z i | X i ) , (48) where ˜ l ( z i | X i ) =      p D R g ( z i | s , x ) p + ( s )d s X i = { x } , λ c ( z i ) X i = ∅ , 0 otherwise . (49) In a similar fashion, we obtain q ( Z i | X i ) =          p D R g ( z | s , x ) p + ( s )d s Z i = { z } , X i = { x } , 1 − p D Z i = ∅ , X i = { x } , 1 Z i = ∅ , X i = ∅ , 0 otherwise . (50) Separating in (46) all terms in volving U , the update of the PPP and the ne wly detected features is gi ven by (c.f. [13, Eqns. 15 to 24]) f ( U | Z u ) ∝ f u + ( U ) q ( Z u | U ) ∝ X Y ] X 1 ... ] X m = U f ( Y ) m Y i =1 f ( X i | z i ) , (51) where f ( Y ) ∝ [(1 − p D ) D u + ( · )] Y , (52) f ( X i | z i ) = ˜ l ( z i | X i ) f u + ( X i ) e −h D u + ;1 i ρ ( z i ) (53) and ρ ( z i ) , R ˜ l ( z i | X i ) f u + ( X i ) δ X i e −h D u + ;1 i ρ ( z i ) = λ c ( z i ) + e ( z i ) , (54) e ( z i ) = p D Z Z g ( z i | s , x ) p + ( s )d s D u + ( x )d x , (55) where we have defined ρ ( z i ) similar to [13, Eqns. 19 to 21] by normalization over e −h D u + ;1 i . The updated Bernoulli components for the existing features are obtained by f j i ( X i | Z i ) = t ( Z i | X i ) f j i ( X i ) ρ j i ( Z i ) , (56) where ρ j i ( Z ) = ( p D r j i R R g ( z | s , x ) p + ( s )d s p j i ( x )d x Z = { z } , 1 − p D r j i Z = ∅ (57) and t ( Z i | X i ) = Z t ( Z i | X i , s ) p + ( s )d s . (58) P arameters of the Bernoulli Components From the deriv ation above, it is possible to find the parame- ters of all the Bernoulli components modeling newly detected features and existing features. Newly Detected F eatur es: The posterior for a newly de- tected feature, denoted f ( X i | z i ) (c.f. (53)), h as Bernoulli components with parameters r ( z i ) = e ( z i ) ρ ( z i ) , (59) p ( x | z i ) = p D D u + ( x ) R g ( z i | s , x ) p + ( s )d s e ( z i ) . (60) Existing F eatures: The posterior of an existing Bernoulli component, denoted f j i ( X i | Z i ) (c.f. (56)), has updated pa- rameters r j i ( Z i ) = ( 1 Z i = { z } , r j i (1 − p D ) 1 − p D r j i Z i = {∅} , (61) p j i ( x | Z i ) ∝ ( (1 − p D ) p j i ( x ) Z i = {∅} , p D p j i ( x ) R g ( z | s , x ) p + ( s )d s Z i = { z } . (62) A P P E N D I X B P RO O F O F T H E O R E M 4 Plugging (17) into (28) results in p ( s | Z ) ∝ p + ( s ) Z X U ] X 1 ... ] X n = X f u + ( U ) X j × n Y i =1 w j i f j i ( X i ) l ( Z | X , s ) δ X . (63) Similar to the feature posterior, we add the constraint | X i | ≤ 1 ∀ i , since f j i ( X i ) is Bernoulli, replace the likelihood by (14), and make use of Lemma 2 resulting in p ( s | Z ) ∝ p + ( s ) Z X U ] X 1 ... ] X n = X : | X i |≤ 1 ∀ i X j × X Z 1 ... ] Z n ] Z u = Z f u + ( U ) n Y i =1 w j i f j i ( X i ) l ( Z | X , s ) δ X (64) = X j X Z 1 ... ] Z n ] Z u = Z p + ( s ) q ( Z u | s ) n Y i =1 w j i q j i ( Z i | s ) , (65) where q ( Z u | s ) ∝  λ c ( z i ) + p D Z g ( z i | s , x ) D u + ( x )d x  (66) 12 and q j i ( Z i | s ) = Z t ( Z i | X i , s ) f j i ( X i ) δ X i . (67) T o obtain the global hypothesis weight, we express (65) as a mixture of vehicle state distributions. The weights are given by Z q ( Z u | s ) n Y i =1 w j i q j i ( Z i | s ) p + ( s )d s ≈ Z q ( Z u | s ) p + ( s )d s n Y i =1 w j i Z q j i ( Z i | s ) p + ( s )d s , (68) where the approximation is due to Lemma 1. W e now look at the factors indi vidually . Applying again Lemma 1, the first factor can be approximated by Z q ( Z u | s ) p + ( s )d s ∝ ∼ Y z i ∈ Z u ρ ( z i ) , (69) where ρ ( z i ) was defined in (54), with the only difference that the order of integration is exchanged. For each factor in the product term in (68), we get Z q j i ( Z i | s ) p + ( s )d s = ρ j i ( Z i ) , (70) where ρ j i ( Z i ) was defined in (57). Substitution of the terms abov e into (65) yields the vehicle state posterior (32), where q j ( s | Z 1 , . . . , Z n , Z u ) = p + ( s ) q ( Z u | s ) Q n i =1 q j i ( Z i | s ) R q ( Z u | s ) Q n i =1 q j i ( Z i | s ) p + ( s )d s . (71) R E F E R E N C E S [1] J. Leonard, J. How , S. T eller , M. Berger , S. Campbell, G. Fiore, L. Fletcher , E. Frazzoli, A. Huang, S. Karaman et al. , “ A perception- driv en autonomous urban vehicle, ” Journal of Field Robotics , vol. 25, no. 10, pp. 727–774, 2008. [2] C. Cadena, L. Carlone, H. Carrillo, Y . Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: T o ward the robust-perception age, ” IEEE T ransactions on Robotics , vol. 32, no. 6, pp. 1309–1332, 2016. [3] F . Meyer, O. Hlinka, H. W ymeersch, E. Riegler , and F . Hlawatsch, “Distributed localization and tracking of mobile networks including noncooperativ e objects, ” IEEE T ransactions on Signal and Information Pr ocessing over Networks , vol. 2, no. 1, pp. 57–71, March 2016. [4] G. Soatti, M. Nicoli, N. Garcia, B. Denis, R. Raulefs, and H. W ymeersch, “Enhanced V ehicle Positioning in Cooperative ITS by Joint Sensing of Passi ve Features, ” in IEEE 20th International Confer ence on Intelligent T ransportation Systems (ITSC) , Oct 2017. [5] R. P . Mahler , Statistical multisour ce-multitarget information fusion . Artech House, Inc., 2007. [6] ——, Advances in Statistical Multisour ce-Multitarget Information Fu- sion . Artech House, 2014. [7] S. Mori, C.-Y . Chong, E. Tse, and R. P . Wishner , “Multitarget multi- sensor tracking problems. part 1. a general solution and a unified vie w on Bayesian approaches, ” Advanced Information and Decision Systems Mountain V iew , CA, T ech. Rep. AI/DS-TR-1048-01-REV , 1984. [8] S. Mori, C.-Y . Chong, E. Tse, and R. Wishner , “T racking and classifying multiple targets without a priori identification, ” IEEE T ransactions on Automatic Control , vol. 31, no. 5, pp. 401–409, 1986. [9] B.-N. V o and W .-K. Ma, “The Gaussian Mixture Probability Hypothesis Density Filter, ” IEEE T ransactions on Signal Processing , v ol. 54, no. 11, pp. 4091–4104, 2006. [10] B. N. V o, S. Singh, and A. Doucet, “Sequential Monte Carlo methods for multitarget filtering with random finite sets, ” IEEE T ransactions on Aer ospace and Electr onic Systems , vol. 41, no. 4, pp. 1224–1245, Oct 2005. [11] B.-T . V o, B.-N. V o, and A. Cantoni, “ Analytic implementations of the cardinalized probability hypothesis density filter, ” IEEE T ransactions on Signal Pr ocessing , vol. 55, no. 7, pp. 3553–3567, 2007. [12] J. L. Williams, “Marginal multi-bernoulli filters: RFS derivation of MHT , JIPDA, and association-based MeMBer, ” IEEE T ransactions on Aer ospace and Electronic Systems , vol. 51, no. 3, pp. 1664–1687, 2015. [13] ´ A. F . Garc ´ ıa-Fern ´ andez, J. L. W illiams, K. Granstr ¨ om, and L. Svensson, “Poisson multi-Bernoulli mixture filter: direct deriv ation and imple- mentation, ” IEEE T ransactions on Aerospace and Electr onic Systems , vol. 54, no. 4, pp. 1883–1901, 2018. [14] B.-N. V o, B.-T . V o, and D. Phung, “Labeled random finite sets and the bayes multi-target tracking filter , ” IEEE T ransactions on Signal Pr ocessing , vol. 62, no. 24, pp. 6554–6567, 2014. [15] S. Reuter , B.-T . V o, B.-N. V o, and K. Dietmayer, “The labeled multi- Bernoulli filter , ” IEEE T ransactions on Signal Processing , vol. 62, no. 12, pp. 3246–3260, 2014. [16] F . Meyer , T . Kropfreiter, J. L. W illiams, R. Lau, F . Hlawatsch, P . Braca, and M. Z. W in, “Message passing algorithms for scalable multitarget tracking, ” Proceedings of the IEEE , v ol. 106, no. 2, pp. 221–259, Feb 2018. [17] F . Me yer, P . Braca, P . Willett, and F . Hlawatsch, “ A scalable algorithm for tracking an unknown number of targets using multiple sensors, ” IEEE T ransactions on Signal Processing , v ol. 65, no. 13, pp. 3478–3493, 2017. [18] H. Durrant-Whyte and T . Bailey , “Simultaneous localization and map- ping: part i, ” IEEE robotics & automation magazine , vol. 13, no. 2, pp. 99–110, 2006. [19] J. Mullane, B.-N. V o, M. D. Adams, and B.-T . V o, “ A random-finite-set approach to Bayesian SLAM, ” IEEE T ransactions on Robotics , vol. 27, no. 2, pp. 268–282, 2011. [20] E. Brekke, B. Kalyan, and M. Chitre, “ A novel formulation of the Bayes recursion for single-cluster filtering, ” in Aerospace Conference, 2014 IEEE . IEEE, 2014, pp. 1–16. [21] C. S. Lee, D. E. Clark, and J. Salvi, “SLAM with dynamic tar gets via single-cluster PHD filtering, ” IEEE Journal of Selected T opics in Signal Pr ocessing , vol. 7, no. 3, pp. 543–552, 2013. [22] M. ¨ Uney , B. Mulgrew , and D. E. Clark, “Distributed localisation of sensors with partially overlapping field-of-views in fusion networks, ” in 2016 19th International Confer ence on Information Fusion (FUSION) , July 2016, pp. 1340–1347. [23] ——, “ A cooperativ e approach to sensor localisation in distrib uted fusion networks. ” IEEE T rans. Signal Processing , vol. 64, no. 5, pp. 1187– 1199, 2016. [24] J. T eng, H. Snoussi, C. Richard, and R. Zhou, “Distributed variational filtering for simultaneous sensor localization and target tracking in wireless sensor networks, ” IEEE T ransactions on V ehicular T echnology , vol. 61, no. 5, pp. 2305–2318, 2012. [25] N. Kantas, S. S. Singh, and A. Doucet, “Distrib uted maximum likelihood for simultaneous self-localization and tracking in sensor networks, ” IEEE Tr ansactions on Signal Pr ocessing , vol. 60, no. 10, pp. 5038– 5047, 2012. [26] M. Fr ¨ ohle, C. Lindberg, and H. W ymeersch, “Cooperati ve localization of vehicles without inter-vehicle measurements, ” in IEEE W ireless Communications and Networking Confer ence (WCNC) , 2018, pp. 1–6. [27] C. M. Bishop, P attern Recognition and Mac hine Learning . Springer , 2006. [28] J. L. W illiams, “Graphical model approximations of random finite set filters, ” arXiv preprint , 2011. [29] M. S. Arulampalam, S. Maskell, N. Gordon, and T . Clapp, “A T utorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian T rack- ing, ” IEEE T ransactions on Signal Pr ocessing , vol. 50, no. 2, pp. 174– 188, 2002. [30] J. A. Castellanos, J. D. T ard ´ os, and G. Schmidt, “Building a global map of the en vironment of a mobile robot: The importance of correlations, ” in Robotics and Automation, 1997. Pr oceedings., 1997 IEEE International Confer ence on , vol. 2. IEEE, 1997, pp. 1053–1059. [31] J. W illiams and R. Lau, “ Approximate Evaluation of Marginal Asso- ciation Probabilities with Belief Propagation, ” IEEE T ransactions on Aer ospace and Electronic Systems , vol. 50, no. 4, pp. 2942–2959, 2014. [32] A. A. Saucan, M. J. Coates, and M. Rabbat, “ A multisensor multi- Bernoulli filter , ” IEEE T ransactions on Signal Processing , vol. 65, no. 20, pp. 5495–5509, Oct 2017. [33] J. L. Williams and R. A. Lau, “Multiple scan data association by con vex variational inference, ” IEEE Tr ansactions on Signal Processing , vol. 66, no. 8, pp. 2112–2127, 2016. [34] HIGHTS project deli verable D5.2, “Specifications of implemented co- operativ e and fusion algorithms, ” Jacobs University Bremen, T ech. Rep., 2016. 13 [35] O. T . S. Ltd, “Datasheet R T3000 v2 GNSS/INS for high dynamic v ehicle testing, ” Oxford T echnical Solutions Ltd, United Kingdom, T ech. Rep., 2018. [36] D. Schuhmacher, B.-T . V o, and B.-N. V o, “ A consistent metric for performance ev aluation of multi-object filters, ” IEEE Tr ansactions on Signal Pr ocessing , vol. 56, no. 8, pp. 3447–3457, 2008. [37] M. Fr ¨ ohle, K. Granstr ¨ om, and H. W ymeersch, “Multiple target tracking with uncertain sensor state applied to autonomous vehicle data, ” in 2018 IEEE Statistical Signal Processing W orkshop (SSP) , 2018, pp. 628–632. Markus Fr ¨ ohle (S’11) receiv ed the B.Sc. and M.Sc. degrees in T elematics from Graz Univ ersity of T echnology , Graz, Austria, in 2009 and 2012, respectiv ely . From 2012 to 2013, he was a Re- search Assistant with the Signal Processing and Speech Communication Laboratory , Graz Univer - sity of T echnology . Since 2013 he is working to- wards the Ph.D. de gree in electrical engineering with the Department of Electrical Engineering, Chalmers Univ ersity of T echnology , Gothenburg, Sweden. His current research interests include signal processing for wireless multi-agent systems, and localization and tracking. Christopher Lindberg received the B.Sc. degree in electrical engineering, M.Sc. degree in engineering mathematics and computational science, and degree of Licentiate of Engineering in electrical engineering from Chalmers Univ ersity of T echnology in 2010, 2012, and 2015, respectively . Since 2012 he is working tow ards the Ph.D. degree in the Communi- cation Systems group at the Department of Electrical Engineering, Chalmers University of T echnology , Gothenbur g, Sweden. Currently he is working with sensor fusion for AD/AD AS at DENSO Sweden AB. His research interests inv olve cooperative distributed processing in networks based on consensus algorithms and belief propagation. Karl Granstr ¨ om (M’08) is a postdoctoral research fellow at the Department of Signals and Systems, Chalmers University of T echnology , Gothenburg, Sweden. He receiv ed the MSc degree in Applied Physics and Electrical Engineering in May 2008, and the PhD degree in Automatic Control in November 2012, both from Link ¨ oping Univ ersity , Sweden. He previously held postdoctoral positions at the De- partment of Electrical and Computer Engineering at Univ ersity of Connecticut, USA, from September 2014 to August 2015, and at the Department of Electrical Engineering of Link ¨ oping Univ ersity from December 2012 to August 2014. His research interests include estimation theory , multiple model estimation, sensor fusion and target tracking, especially for extended targets. He receiv ed paper awards at the Fusion 2011 and Fusion 2012 conferences. He has organised several workshops and tutorials on the topic Multiple Extended T arget Tracking and Sensor Fusion. Henk W ymeersch (S’01, M’05) obtained the Ph.D. degree in Electrical Engineering/Applied Sciences in 2005 from Ghent Univ ersity , Belgium. He is currently a Professor of Communication Systems with the Department of Electrical Engineering at Chalmers Univ ersity of T echnology , Sweden. Prior to joining Chalmers, he was a postdoctoral re- searcher from 2005 until 2009 with the Laboratory for Information and Decision Systems at the Mas- sachusetts Institute of T echnology . Prof. W ymeersch served as Associate Editor for IEEE Communication Letters (2009-2013), IEEE T ransactions on W ireless Communications (since 2013), and IEEE Transactions on Communications (2016-2018). His current research interests include cooperativ e systems and intelligent transportation.