Log-PF: Particle Filtering in Logarithm Domain

Hindawi Journal of Electrical and Computer Engineering Volume 2018, Article ID 5763461, 11 pages https://doi.org/10.1155/2018/5763461

Research Article Log-PF: Particle Filtering in Logarithm Domain Christian Gentner , Siwei Zhang

, and Thomas Jost

German Aerospace Center (DLR), Institute of Communications and Navigation, Oberpfaffenhofen, 82234 Wessling, Germany Correspondence should be addressed to Christian Gentner; [email protected] Received 25 August 2017; Revised 7 November 2017; Accepted 6 December 2017; Published 1 March 2018 Academic Editor: V´ıctor Elvira Copyright © 2018 Christian Gentner et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. This paper presents a particle filter, called Log-PF, based on particle weights represented on a logarithmic scale. In practical systems, particle weights may approach numbers close to zero which can cause numerical problems. Therefore, calculations using particle weights and probability densities in the logarithmic domain provide more accurate results. Additionally, calculations in logarithmic domain improve the computational efficiency for distributions containing exponentials or products of functions. To provide efficient calculations, the Log-PF exploits the Jacobian logarithm that is used to compute sums of exponentials. We introduce the weight calculation, weight normalization, resampling, and point estimations in logarithmic domain. For point estimations, we derive the calculation of the minimum mean square error (MMSE) and maximum a posteriori (MAP) estimate. In particular, in situations where sensors are very accurate the Log-PF achieves a substantial performance gain. We show the performance of the derived Log-PF by three simulations, where the Log-PF is more robust than its standard particle filter counterpart. Particularly, we show the benefits of computing all steps in logarithmic domain by an example based on Rao-Blackwellization.

1. Introduction Many scientific problems involve dynamic systems, for example, in navigation applications. Dynamic systems can be described by state-space models where the state is only observable by noisy measurements. Recursive Bayesian filters are algorithms to estimate an unknown probability density function (PDF) of the state recursively by measurements over time. Such a filter consists of two steps: prediction and update. In the prediction step, the PDF of the state is calculated based on the system model. During the update step, the current measurement is used to correct the prediction based on the measurement model. In this way, the posterior PDF of the state is estimated recursively over time. Particle filters (PFs) are implementations of recursive Bayesian filters which approximate the posterior PDF by a set of random samples, called particles, with associated weights. Several types of PFs have been developed over the last few years [1–8]. They differ in their choice of the importance sampling density and the resampling step. A common way is to choose the importance sampling density to be equal to the prior, for example, the bootstrap filtering algorithm [1]. However, if the width of the likelihood

distribution is too small in comparison to the width of the prior distribution or if measurements are located in the tail of the prior distribution, this choice may fail; see [4]. These situations may arise when sensors are very accurate or measurements rapidly change over time such that the particle states after the prediction step might be located in the tail of the likelihood. Additionally, numerical representation of numbers may limit the computational accuracy by floating point errors. In these situations, a common way is to use the likelihood particle filter (LPF) [3, 5]. The LPF uses the likelihood distribution for the importance sampling density and the prior for the weight update. The LPF is recommended when the width of the likelihood distribution is much smaller compared to the one of the prior and accordingly, the posterior density function is more similar to the likelihood than to the prior. However, in many situations, it is impossible to draw samples from the likelihood distribution. Furthermore, the LPF is not suitable for an underdetermined system where the number of measurements is lower than the number of states per time instant. Additionally, using the likelihood as proposal distribution might increase the variance of the simulated samples according to [5].

2

Journal of Electrical and Computer Engineering

In this paper, we derive a PF that operates in logarithmic domain (log-domain), called Log-PF. The Log-PF represents the weights in log-domain which enables a more accurate representation of low weights with a limited number of bits. Particularly, when the involved distributions contain exponentials or products of functions, the log-domain representation is computationally more efficient [9]. The derived LogPF uses the Jacobian logarithm [10–12] to describe all steps of the PF, including weight update, weight normalization, resampling, and point estimations in log-domain. In this paper, we derive the minimum mean square error (MMSE) and the maximum a posteriori (MAP) point estimators. The paper is structured as follows: First, we describe in Section 2 standard PFs; thereafter we derive the proposed Log-PF in Section 2. Afterwards, we derive in Section 4 two point estimators in log-domain: Section 4.1 describes the MMSE estimator and Section 4.2 the MAP estimator. We evaluate the Log-PF by simulations and compare the results to standard PF implementations and Kalman filters (KFs) in Section 5. Particularly, we show by an example based on Rao-Blackwellization the benefits by computing all steps in log-domain. For distributed particle filters like [13] similar results are expected. Finally, Section 6 concludes the paper. Throughout the paper, we will use the following notations:

∗(𝑗)

The unnormalized weight is denoted by 𝑤𝑘 , while the weight update is calculated as ∗(𝑗) 𝑤𝑘

=

(𝑗) p (z𝑘 𝑤𝑘−1

(v) 𝑎 ∼ N(𝜇𝑎 , 𝜎𝑎2 ) denotes a Gaussian distributed or multivariate random variable 𝑎 with mean 𝜇𝑎 and variance 𝜎𝑎2 .

(vi) E[𝑥] stands for expectation or sample mean of 𝑥. (vii) 1 : 𝑘 stands for all integer numbers starting from 1 to 𝑘, thus 1, 2, . . . , 𝑘. (viii) 𝑥̂ denotes the estimate of 𝑥. (ix) {𝑥(𝑖) }𝑁 𝑖=1 defines the set for 𝑥𝑖 with 𝑖 = 1, . . . , 𝑁.

PFs represent the probability density of the state vector x𝑘 at time step 𝑘 by 𝑁𝑝 particles. According to [1–3, 5] and assuming a first-order hidden Markov model, the posterior filtered density p(x𝑘 | z0:𝑘 ) is approximated as (𝑗)

p (x𝑘 | z0:𝑘 ) ≈ ∑𝑤𝑘 𝛿 (x𝑘 − x𝑘 ) ,

,

(3)

(𝑗)

∗(𝑗)

̂𝑘 𝑤

(𝑗)

(𝑗)

(𝑗)

(𝑗)

̂𝑘−1 + ln (p (z𝑘 | x𝑘 )) + ln (p (x𝑘 | x𝑘−1 )) =𝑤 (𝑗)

(4)

− ln (q (x𝑘 | x𝑘−1 , z𝑘 )) ,

(𝑗)

(1)

(𝑗)

∗(𝑗)

∗(𝑗)

domain) with 𝑤𝑘 = 𝑒𝑤̂𝑘 for 𝑗 = 1, . . . , 𝑁𝑝 , where the numerical accuracy is lost due to floating point representation. In order to obtain a more stable PF implementation, the ∗(𝑗) ̂𝑘 can be transferred to the lin-domain by weights 𝑤 +(𝑗)

𝑤𝑘 +(𝑗)

∗(𝑗)

= 𝑒𝑤̂𝑘

−max𝑙 (̂ 𝑤𝑘∗(𝑙) )

(6)

𝑁

3. Algorithm Derivation To compute all steps of the PF in log-domain, we obtain for the approximation of the posterior filtered density from (33) 𝑁𝑝

(2)

,

such that 𝑤𝑘 = 𝑤𝑘 / ∑𝑖=1𝑝 𝑤𝑘+(𝑖) ; see, for example, [9]. In the following we investigate a different approach, where the transformation from the log-domain to the lindomain is not necessary. Hence, we show that all steps of the PF can be computed in log-domain.

𝑗=1

where z0:𝑘 defines the measurement vector for the time steps (𝑗) 0, . . . , 𝑘, 𝛿(⋅) stands for the Dirac distribution, x𝑘 denotes (𝑗) particle state, and 𝑤𝑘 denotes the normalized weight with

(5)

̂𝑘 for particle 𝑗. After the log-domain weight (log-weight) 𝑤 ∗(𝑗) ̂𝑘 in log-domain, the weights are calculating the weights 𝑤 transferred for further processing to the linear domain (lin-

(𝑗)

2. Particle Filtering

(𝑗)

(𝑗)

with the importance density q(x𝑘 | x𝑘−1 , z𝑘 ), the likelihood (𝑗) distribution p(z𝑘 | x𝑘 ), and the transition prior distribution (𝑗) (𝑗) p(x𝑘 | x𝑘−1 ); see [1–3, 5]. For 𝑁𝑝 → ∞, the approximation used in (33) approaches p(x𝑘 | z0:𝑘 ). By (33), (2), and (3), the sequential importance sampling (SIS) PF can be described which is the basis of most PFs [2]. For numerical stability reasons, weights are often computed and stored in the log-domain, which is also computationally efficient when the distributions involved contain exponentials or products. Thus, we obtain from (3) the update equation in log-domain, with

(𝑗)

(iv) [x]𝑙 denotes the 𝑙th element of vector x.

𝑤𝑘 =

(𝑗)

̂𝑘 = ln (𝑤𝑘 ) 𝑤

(iii) Matrices are denoted by bold capital letters and vectors by bold small letters.

∗(𝑗) 𝑤𝑘 . 𝑁 ∑𝑖=1𝑝 𝑤𝑘∗(𝑖)

(𝑗)

q (x𝑘 | x𝑘−1 , z𝑘 )

(ii) I denotes an identity matrix.

(𝑗)

(𝑗)

where we define with

(i) All vectors are interpreted as column vectors.

𝑁𝑝

(𝑗)

| x𝑘 ) p (x𝑘 | x𝑘−1 )

(𝑗)

(𝑗)

p (x𝑘 | z0:𝑘 ) ≈ ∑𝑒𝑤̂𝑘 𝛿 (x𝑘 − x𝑘 )

(7)

𝑗=1

using (5). The normalization of the log-weight can be calculated directly in log-domain as a simple subtraction, with (𝑗)

∗(𝑗)

̂𝑘 ̂𝑘 = 𝑤 𝑤

̂𝑘 , −𝑊

(8)

Journal of Electrical and Computer Engineering

3

𝑛

ln(∑𝑙=1 𝑒𝛿𝑖 ) = Jacob({𝛿𝑙 }𝑛𝑙=1 ) (1) Init: Δ 1 = 𝛿1 ; (2) for 𝑙 = 2 : 𝑛 do (3) Δ 𝑙 = max(𝛿𝑙 , Δ 𝑙−1 ) + ln(1 + 𝑒−|𝛿𝑙 −Δ 𝑙−1 | ); 𝑛 (4) ln(∑𝑙=1 𝑒𝛿𝑖 ) = Δ 𝑛 ;

(𝑗)

Algorithm 1: Iterative Jacobian algorithm.

∗(𝑖)

̂𝑘 = ln (∑𝑒𝑤̂𝑘 ) . 𝑊

(9)

𝑖=1

(𝑗)

ln (𝑒𝛿1 + 𝑒𝛿2 ) = max (𝛿1 , 𝛿2 ) + ln (1 + 𝑒−|𝛿2 −𝛿1 | ) .

(10)

With (10) and as derived in [12], the expression ln(∑𝑛𝑙=1 𝑒𝛿𝑙 ) can be calculated iteratively as ln (𝑒𝛿1 + ⋅ ⋅ ⋅ + 𝑒𝛿𝑛 ) = ln (Δ + 𝑒𝛿𝑛 ) = max (ln (Δ) , 𝛿𝑛 )

(𝑗)

𝑛

ln (∑𝑒𝛿𝑙 ) = Jacob ({𝛿𝑙 }𝑙=1 ) ,

(𝑗)

̂𝑘 with the set for the particle states x𝑘 and log-weights 𝑤 𝑗 = 1, . . . , 𝑁𝑝 for time step 𝑘. One of the crucial problems of the SIS PF is degeneracy (another problem which is not discussed in this paper is the (𝑗) selection of the importance density q(x𝑘 | x𝑘−1 , z𝑘 ) of (4); see e.g., [2]). After a few time steps all particles except for one have low weights and do not contribute anymore to the computation of the posterior PDF; that is, the distribution estimation degenerates. A suitable measure of degeneracy is the effective sample size 𝑁eff [1–3, 5]. A widely used approximation for the effective sample size is (2) 𝑃𝑁 𝑝

(w𝑘 ) =

1 (𝑗) 2

𝑁

𝑝 (𝑤𝑘 ) ∑𝑗=1

=

𝑁

∗(𝑗)

𝑁

∗(𝑗) 2

𝑝 (𝑤𝑘 )) (∑𝑗=1

(2) ) and 1 ≤ 𝑃𝑁 (w𝑘 ) ≤ 𝑁𝑝 . A small 𝑝

(2) (w𝑘 ) indicates a severe degeneracy. By using the value of 𝑃𝑁 𝑝 Jacobian logarithm of (12), we obtain from (15) the effective sample size in log-domain, with 𝑁𝑝

𝑗=1

(𝑗) 𝑁𝑝

where {𝛿𝑙 }𝑛𝑙=1 defines the set for 𝛿𝑙 with 𝑙 = 1, . . . , 𝑛. Thus, the ̂𝑘 of (9) can be calculated iteratively by normalization factor 𝑊

∗(𝑗) 𝑁𝑝 Jacob ({̂ 𝑤𝑘 }𝑗=1 )

𝑙=1

(13)

Hence, we obtain for the log-weight normalization of (8), ∗(𝑗)

𝑁𝑝

− Jacob ({̂ 𝑤𝑘∗(𝑖) }𝑖=1 ) .

(14)

Please note that a complexity reduction can be obtained if the term ln(1 + 𝑒−|𝛿𝑙 −Δ 𝑙−1 | ) of Algorithm 1 is read from a hardcoded lookup table as a function of |𝛿𝑙 − Δ 𝑙−1 |. By using (14), the SIS PF can be described in log-domain as shown in Algorithm 2 by a pseudocode. Algorithm 2 is

(𝑗)

(2) 𝑤𝑘 (w𝑘 )) = − ln (∑ 𝑒2⋅̂ ) ln (𝑁eff ) ≈ ln (𝑃𝑁 𝑝

̂𝑘 }𝑗=1 ) = −Jacob ({2 ⋅ 𝑤

(𝑗)

(15)

𝑝 (𝑤𝑘 ) ∑𝑗=1

(12)

𝑁𝑝 Jacob ({̂ 𝑤𝑘∗(𝑖) }𝑖=1 ) .

2

(𝑁𝑝 ) 𝑇

with w𝑘 = (𝑤𝑘(1) , . . . , 𝑤𝑘

where 𝛿 = ln(𝑒𝛿1 + ⋅ ⋅ ⋅ + 𝑒𝛿𝑛−1 ) and Δ = 𝑒𝛿1 + ⋅ ⋅ ⋅ + 𝑒𝛿𝑛−1 . Hence, using the Jacobian logarithm allows computing operations such as summations like in (9) efficiently in the log-domain. For later conveniences, we express (11) by an iterative algorithm shown in Algorithm 1 by a pseudocode; that is,

̂𝑘 ̂𝑘 = 𝑤 𝑤

(𝑗) 𝑁

𝑝 ̂𝑘 }𝑗=1 evaluated at each time step 𝑘, where {x𝑘 , 𝑤 denotes

𝑁eff ≈ (11)

+ ln (1 + 𝑒−|ln(Δ)−𝛿𝑛 | ) ,

̂𝑘 = 𝑊

𝑁

(𝑗)

̂𝑘 of (9) without To compute the normalization factor 𝑊 transferring the log-weights to the lin-domain, the Jacobian logarithm [10, 11] can be used. The Jacobian logarithm computes the logarithm of a sum of two exponentials ln(𝑒𝛿1 + 𝑒𝛿2 ) using the max (⋅) operator and adding a correction term; that is,

𝑛

(𝑗)

Algorithm 2: SIS-PF in log-domain (SIS Log-PF).

̂𝑘 denotes the normalization factor with where 𝑊 𝑁𝑝

(𝑗) 𝑁

̂𝑘 }𝑗=1𝑝 = Log-SIS({x𝑘−1 , 𝑤 ̂𝑘−1 }𝑗=1𝑝 ) {x𝑘 , 𝑤 (1) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) (𝑗) (𝑗) (2) Draw: x𝑘 ∼ q(x𝑘 | x𝑘−1 , z𝑘 ); ∗(𝑗) ̂𝑘 according to (4); (3) Calculate: 𝑤 ∗(𝑗) 𝑁 ̂ (4) Calculate: 𝑊(𝑘) = Jacob({̂ 𝑤𝑘 }𝑗=1𝑝 ); (5) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) ∗(𝑗) ̂ ̂𝑘 = 𝑤 ̂𝑘 − 𝑊(𝑘); (6) Normalize: 𝑤

=2⋅

(16)

∗(𝑗) 𝑁𝑝

̂𝑘 }𝑗=1 ) . − Jacob ({2 ⋅ 𝑤 Alternative effective sample size approximations as introduced in [14] can also be represented in log-domain. Table 1 summarizes four generalized alternative effective sample size approximations in lin-domain and log-domain which depend (2) (w𝑘 ) as in (15) is obtained on the parameter 𝑟. Please note 𝑃𝑁 𝑝

(𝑟) from 𝑃𝑁 (w𝑘 ) with 𝑟 = 2. 𝑝 The Generic PF extends the SIS PF by a resampling step to prevent degeneration as shown in Algorithm 3 by a pseudocode. The basic idea of resampling is to eliminate

4

Journal of Electrical and Computer Engineering

(𝑟) (𝑟) Table 1: Generalized effective sample size functions 𝑃𝑁(𝑟)𝑝 (w𝑘 ), 𝐷𝑁 (w𝑘 ), 𝑉𝑁(𝑟)𝑝 (w𝑘 ), 𝑆𝑁 (w𝑘 ) to approximate 𝑁eff in lin-domain as defined in [14] 𝑝 𝑝 (𝑟) (𝑟) and log-domain with their coefficients 𝑎{⋅} and 𝑏{⋅} . Please note 𝑃𝑁(2)𝑝 (w𝑘 ) as in (15) is obtained from 𝑃𝑁(𝑟)𝑝 (w𝑘 ) with 𝑟 = 2.

𝑃𝑁(𝑟)𝑝 (w𝑘 ) =

1

𝑎𝑃(𝑟)

𝑁 ∑𝑗=1𝑝

(𝑗) 𝑟 (𝑤𝑘 )

+

(𝑗) 𝑁𝑝

̂𝑘 }𝑗=1 , ln (𝑏𝑃(𝑟) )}) ln (𝑃𝑁(𝑟)𝑝 (w𝑘 )) = −Jacob ({{ln (𝑎𝑃(𝑟) ) + 𝑟 ⋅ 𝑤 (𝑟) 𝐷𝑁 (w𝑘 ) = 𝑝

𝑁 [∑𝑗=1𝑝

1/𝑟 (𝑗) 𝑟 (𝑤𝑘 ) ]

+

(𝑗) 𝑟

𝑎𝑉(𝑟) =

𝑗=1

(𝑗) 𝑁𝑝

̂𝑘 }𝑗=1 , ln (𝑏𝑉(𝑟) )}) ln (𝑉𝑁(𝑟)𝑝 (w𝑘 )) = Jacob ({{ln (𝑎𝑉(𝑟) ) + 𝑟 ⋅ 𝑤 (w𝑘 ) =

𝑎𝑆(𝑟)

𝑟

[𝑗=1

𝑘

]

+ 𝑏𝑟 𝑏𝑆(𝑟)

𝑎𝑆(𝑟) =

1 (𝑗) 𝑁𝑝 (𝑟) ̂𝑘 }𝑗=1 ) , ln (𝑏𝑆(𝑟) )}) ln (𝑆𝑁 (w𝑘 )) = Jacob ({ln (𝑎𝑆(𝑟) ) + Jacob ({𝑟 ⋅ 𝑤 𝑝 𝑟

(𝑗)

(𝑗) 𝑁

𝑁𝑝 − 1

(𝑗)

(𝑗)

𝑁

̂𝑘 }𝑗=1𝑝 = Generic-Log({x𝑘−1 , 𝑤 ̂𝑘−1 }𝑗=1𝑝 ) {x𝑘 , 𝑤 (1) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) (𝑗) (𝑗) (2) Draw: x𝑘 ∼ q(x𝑘 | x𝑘−1 , z𝑘 ); ∗(𝑗) ̂𝑘 according to (4); (3) Calculate: 𝑤 ∗(𝑗) 𝑁 ̂𝑘 = Jacob({̂ (4) Calculate: 𝑊 𝑤𝑘 }𝑗=1𝑝 ); (5) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) ∗(𝑗) ̂𝑘 ; ̂𝑘 = 𝑤 ̂𝑘 − 𝑊 (6) Normalize: 𝑤 (7) Calculate ln(𝑁eff ) according to Table 1; (8) if ln(𝑁eff ) < ln(𝑁thr ) then (9) Resample with Algorithm 4: Obtaining (𝑗) (𝑗) 𝑠 ̂𝑘 }𝑁 {x𝑘 , 𝑤 𝑗=1 ;

𝑁𝑝 − 𝑁𝑝1/𝑟 1 − 𝑁𝑝1/𝑟 𝑁𝑝 − 𝑁𝑝1/𝑟

𝑁𝑝𝑟−1 (𝑁 − 1) 1 − 𝑁𝑝𝑟−1

𝑏𝑉(𝑟) =

1/𝑟

𝑁𝑝

[∑ (𝑤(𝑗) ) ]

𝑁𝑝2−𝑟 − 𝑁𝑝

𝑏𝐷(𝑟) =

𝑉𝑁(𝑟)𝑝 (w𝑘 ) = 𝑎𝑉(𝑟) ∑ (𝑤𝑘 ) + 𝑏𝑉(𝑟)

(𝑟) 𝑆𝑁 𝑝

𝑁𝑝2−𝑟 − 1

𝑎𝐷(𝑟) =

𝑏𝐷(𝑟)

1 (𝑗) 𝑁𝑝 (𝑟) ̂𝑘 }𝑗=1 ) , ln (𝑏𝐷(𝑟) )}) ln (𝐷𝑁 (w𝑘 )) = −Jacob ({ln (𝑎𝐷(𝑟) ) + Jacob ({𝑟 ⋅ 𝑤 𝑝 𝑟 𝑁𝑝

𝑁𝑝2−𝑟 − 𝑁𝑝

𝑏𝑃(𝑟) =

1 𝑎𝐷(𝑟)

1 − 𝑁𝑝

𝑎𝑃(𝑟) =

𝑏𝑃(𝑟)

𝑏𝑆(𝑟) =

(𝑗)

(𝑗) 𝑁

(𝑗)

𝑁𝑝𝑟 − 1 𝑁𝑝𝑟−1 − 1

𝑁𝑝 − (1−𝑟)/𝑟 𝑁𝑝 𝑁𝑝(1−𝑟)/𝑟

1 −1

− 𝑁𝑝

(1 − 𝑟)/𝑟 − 1

(𝑗) 𝑁

̃𝑘 }𝑗=1𝑝 = Log-Resampling({x𝑘 , 𝑤 ̂𝑘 }𝑗=1𝑝 ) {̃x𝑘 , 𝑤 (𝑗) ̂𝑘 ; (1) Initialize the log-CDF: [c]1 = 𝑤 (2) for 𝑗 = 2 : 𝑁𝑝 do (3) Construct log-CDF using the Jacobian logarithm: (𝑗)

(𝑗)

𝑤𝑘 | 𝑤𝑘 , [c]𝑗−1 ) + ln(1 + 𝑒−|[c]𝑗−1 −̂ ); [c]𝑗 = max(̂ (4) 𝑖 = 1; (5) Draw starting point: [u]1 ∼ U[0, 𝑁𝑝−1 ]; (6) for 𝑗 = 1 : 𝑁𝑝 do (7) [u]𝑗 = ln([u]1 + 𝑁𝑝−1 (𝑗 − 1)); (8) while [u]𝑗 > [c]𝑖 do (9) 𝑖 = 𝑖 + 1; (𝑗) (𝑗) ̃𝑘 } = {x𝑘(𝑖) , − ln(𝑁𝑝 )}; (10) Assign: {̃x𝑘 , 𝑤

Algorithm 3: Generic-PF in log-domain (Generic Log-PF). Algorithm 4: Resampling in log-domain.

particles with low weights and reproduce particles with high weights. Whenever a significant degeneracy is observed in the Generic PF, that is, ln(𝑁eff ) is less than a threshold ln(𝑁thr ), the particles are resampled. Algorithm 4 shows a pseudocode of the systematic resampling algorithm [15] transferred into log-domain. In Algorithm 4, U[0, 𝑁𝑝−1 ] denotes the uniform distribution on the interval [0, 𝑁𝑝−1 ] (cf. Algorithm 4 Line 5). Similarly to the descriptions before, the Jacobian logarithm is used to construct the estimated sampled cumulative distribution function in log-domain (log-CDF); see Algorithm 4 Line 3. The estimated sampled log-CDF is presented by a vector c

with length 𝑁𝑝 and element [c]𝑗 with 𝑗 = 1, . . . , 𝑁𝑝 . By [x]𝑗 , we denote the 𝑗th element of the vector x. According to the estimated sampled log-CDF, particles with high weights are reproduced and particles with low weights are eliminated. In Section 5, we use the sequential importance resampling (SIR) PF; see [1], as an example for comparing the performance of the linear domain PF (Lin-PF) and Log-PF. Therefore, Algorithm 5 shows a pseudocode of the SIR PF in log-domain, which is derived from the Generic PF by setting the importance density to be equal to the transitional prior

Journal of Electrical and Computer Engineering

(𝑗)

(𝑗) 𝑁

(𝑗)

(𝑗)

5 4.1. Minimum Mean Square Error Estimate. The MMSE point estimate using the approximated a posteriori density, see, for example, [16], is defined by

𝑁

̂𝑘 }𝑗=1𝑝 = Log-SIR({x𝑘−1 , 𝑤 ̂𝑘−1 }𝑗=1𝑝 ) {x𝑘 , 𝑤 (1) for 𝑗 = 1 : 𝑁𝑝 do (𝑗)

(𝑗)

(𝑗)

(2) Draw: x𝑘 ∼ p(x𝑘 | x𝑘−1 ); ∗(𝑗) (𝑗) ̂𝑘 = ln(p(z𝑘 | x𝑘 )); (3) Calculate: 𝑤 𝑁 ∗(𝑗) ̂𝑘 = Jacob({̂ (4) Calculate: 𝑊 𝑤𝑘 }𝑗=1𝑝 );

𝑁𝑝

x̂𝑘MMSE

(5) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) ∗(𝑗) ̂𝑘 ; ̂𝑘 = 𝑤 ̂𝑘 − 𝑊 (6) Normalize: 𝑤 (7) Resample with Algorithm 4: Obtaining (𝑗) (𝑗) 𝑠 ̂𝑘 }𝑁 {x𝑘 , 𝑤 𝑗=1 ;

(𝑗)

𝑗

= ∑ 𝑒𝑤̂𝑘 x𝑘 ,

(17)

𝑗=1

where the 𝑙th element of the vector x̂𝑘MMSE can expressed as 𝑁𝑝

[̂x𝑘MMSE ]𝑙

Algorithm 5: SIR-PF in log-domain (SIR Log-PF).

(𝑗)

(𝑗)

= ∑𝑒𝑤̂𝑘 [x𝑘 ]𝑙 .

(18)

𝑗=1

In order to use the Jacobian logarithm to compute (18) in log(𝑗) domain, we separate the positive and negative values of [x𝑘 ]𝑙 with (𝑗)

(𝑗) 𝑁

(𝑗)

(𝑗)

(𝑗)

𝑁

{x𝑘 , 𝑤𝑘 }𝑗=1𝑝 = Generic-Lin-Log({x𝑘−1 , 𝑤𝑘−1 }𝑗=1𝑝 ) (1) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) (𝑗) (𝑗) (2) Draw: x𝑘 ∼ q(x𝑘 | x𝑘−1 , z𝑘 ); ∗(𝑗) ̂𝑘 according to (4); (3) Calculate: 𝑤 (4) for 𝑗 = 1 : 𝑁𝑝 do (5) Transfer and Normalize: +(𝑗)

∗(𝑗)

A+,𝑙 = {𝑗 | 𝑗 ∈ {1, . . . , 𝑁𝑝 } ∧ [x𝑘 ]𝑙 > 0} ,

∗(𝑙)

𝑤𝑘 ) ; 𝑤𝑘 = 𝑒𝑤̂𝑘 −max𝑙 (̂ 𝑁𝑝 +(𝑗) (6) Calculate: 𝑊𝑘 = ∑𝑗=1 𝑤𝑘 ; (7) for 𝑗 = 1 : 𝑁𝑝 do (𝑗) +(𝑗) (8) Normalize: 𝑤𝑘 = 𝑤𝑘 /𝑊𝑘 ; (9) Calculate 𝑁eff according to Table 1; (10) if 𝑁eff < 𝑁thr then (11) Resample with Algorithm 4: Obtaining (𝑗) (𝑗) 𝑁 {x𝑘 , 𝑤𝑘 }𝑗=1𝑠 ;

(𝑗)

(𝑗)

̂𝑘 accordingly. Please and the corresponding log-weight 𝑤 (𝑗)

(𝑗)

note [x𝑘 ]𝑙 = 0 is not considered in (19) because (𝑒𝑤̂𝑘 ⋅ (𝑗) [x𝑘 ]𝑙 ) = 0. Thus, we obtain from (18) for the MMSE estimate, ln(∑𝑗∈A

[̂x𝑘MMSE ]𝑙 = 𝑒

+,𝑙

𝑒

ln(∑𝑗∈A

−𝑒

ln(∑𝑗∈A

=𝑒

(𝑗) (𝑗) ̂ +ln(|[x ]𝑙 |) 𝑤 𝑘 𝑘 )

−,𝑙

𝑒

(𝑗) (𝑗) ̂ +ln(|[x ]𝑙 |) 𝑤 𝑘 𝑘 )

(20) +,𝑙

𝑒

𝑚𝑗,𝑙

)

ln(∑𝑗∈A

−𝑒

Jacob ({𝑚𝑗,𝑙 }𝑗∈A+,𝑙 )

=𝑒 Algorithm 6: Generic-PF in lin-domain with weight calculation in log-domain (Generic Lin-Log-PF).

(𝑗)

(𝑗)

(𝑗)

distribution, with q(x𝑘 | x𝑘−1 , z𝑘 ) = p(x𝑘 | x𝑘−1 ) and using 𝑁eff = 1, that is, performing resampling at each time step [2]. Additionally, we compare in Section 5 the proposed LogPF to the PF implementation which computes the weights in log-domain and uses (6) to obtain the weights in lindomain, called Lin-Log-PF in the following. A pseudocode of the Generic Lin-Log-PF is shown in Algorithm 6: the weights are calculated in log-domain according to (4) and normalized and transferred to the lin-domain according to +(𝑗)

∗(𝑗)

∗(𝑙)

𝑤𝑘 ) 𝑤𝑘 = 𝑒𝑤̂𝑘 −max𝑙 (̂ as mentioned in Section 3 and, for example, [9]. Please note further improvements can be ∗(𝑗) ̂𝑘 are directly propagated in logobtained if the weights 𝑤 domain if resampling is not necessary.

𝑒

)

Jacob ({𝑚𝑗,𝑙 }𝑗∈A−,𝑙 )

,

where we introduced 𝑚𝑗,𝑙 with

󵄨 (𝑗) 󵄨 (𝑗) ̂𝑘 + ln (󵄨󵄨󵄨󵄨[x𝑘 ]𝑙 󵄨󵄨󵄨󵄨) . 𝑚𝑗,𝑙 = 𝑤 󵄨 󵄨

(21)

4.2. Maximum A Posteriori Estimate. The MAP point estimate is defined as x̂𝑘MAP = arg max p (x𝑘 | z1:𝑘 ) , x

(22)

𝑘

which can be approximated, see [17], by x̂𝑘MAP

≈ arg max p (z𝑘 | (𝑗) x𝑘

𝑁𝑝 (𝑗) (𝑗) x𝑘 ) ∑p (x𝑘 𝑖=1

(𝑖) (𝑖) | x𝑘−1 ) 𝑤𝑘−1 ,

(23)

for 𝑗 = 1, . . . , 𝑁𝑝 . The corresponding MAP state estimator using weights in log-domain can be calculated using the Jacobian logarithm of (12) with (𝑗)

In many applications, we are interested in a point estimate of the state instead of its a posteriori PDF. In this section we derive the MMSE and MAP point estimators based on the a posteriori density estimated by the Log-PF.

−,𝑙

𝑚𝑗,𝑙

−𝑒

𝑁𝑝

(𝑗)

x̂𝑘MAP ≈ arg max p (z𝑘 | x𝑘 ) ∑𝑒ln(p(x𝑘

4. Log-PF Point Estimators

(19)

A−,𝑙 = {𝑗 | 𝑗 ∈ {1, . . . , 𝑁𝑝 } ∧ [x𝑘 ]𝑙 < 0}

(𝑗)

x𝑘

(𝑖) (𝑖) |x𝑘−1 ))+̂ 𝑤𝑘−1

𝑖=1

(𝑗)

= arg max p (z𝑘 | x𝑘 )

(24)

(𝑗) x𝑘

(𝑗)

𝑁𝑝

(𝑖) (𝑖) ̂𝑘−1 × Jacob ({ln (p (x𝑘 | x𝑘−1 )) + 𝑤 }𝑖=1 ) .

6

Journal of Electrical and Computer Engineering 101

In this section, we demonstrate the performance of the LogPF using floating point 64-bit number accuracy according to IEEE Standard 754 for double precision with three simulations.

100

5.1. Linear Processes. First, we simulate a linear Gaussian model. The KF introduced in [18] is an optimal recursive Bayesian filter which can be used if the considered system is linear and the probabilistic model is Gaussian. Hence, we compare the Log-PF and the Lin-PF to the KF as benchmark. The simulation considers the linear transition model x𝑘+1 = Fx𝑘 + k𝑘 = (

1 0.5 0 1

) x𝑘 + k𝑘 ,

(25)

with the transition matrix F, the state vector x𝑘 = (𝑎𝑘 , 𝑏𝑘 )𝑇 , and the zero-mean multivariate Gaussian distributed process noise k𝑘 ∼ N(0, 𝜎V I) with standard deviation 𝜎V and the identity matrix I. The measurement model is defined by 𝑧𝑘 = Hx𝑘 + 𝑛𝑘 = (1 1) x𝑘 + 𝑛𝑘 ,

(26)

with the measurement matrix H and the zero-mean Gaussian distributed measurement noise 𝑛𝑘 ∼ N(0, 𝜎𝑛 ) with standard deviation 𝜎𝑛 . Based on the measurements 𝑧𝑘 , the state sequence x𝑘 for 𝑘 = 1, . . . , 60 is estimated using a KF, the Lin-PF and the Log-PF with 𝑁𝑝 = 200 particles and known initial state x1 = (2, 2)𝑇 . For the Lin-PF, we use the standard PF implementation as well as the Lin-Log-PF. First, we compare the KF to the SIR Lin-PF, SIR Lin-LogPF, and the SIR Log-PF. In order to see the robustness of the SIR Log-PF, we variate the measurement noise standard deviation 𝜎𝑛 from 108 down to 10−150 . We simulate 1000 different realizations with known initial state for each run. Figure 1 shows the root mean square error (RMSE) averaged over all time steps and simulations versus the decreasing measurement noise standard deviation 𝜎𝑛 . The abbreviation SIR Log-PF MAP stands for the MAP point estimate and SIR Log-PF MMSE for the MMSE point estimate of the SIR Log-PF. Respectively, the abbreviations SIR Lin-PF MAP, SIR Lin-Log-PF MAP, SIR Lin-PF MMSE, and SIR Lin-Log-PF MMSE stand for the SIR Lin-PF and SIR Lin-Log-PF point estimates. We see that the KF obtains the best estimation results followed by the SIR Log-PF and SIR Lin-Log-PF. Figure 1 shows additionally an enlarged subfigure of the region for 108 < 𝜎𝑛 < 10−8 . For 𝜎𝑛 > 10−2 , all SIR PFs obtain equivalent performance. As soon as 𝜎𝑛 decreases, the RMSE decreases for the SIR PFs until 𝜎𝑛 = 10−2 . For lower measurement noise standard deviations, the RMSE of the SIR Lin-PF and increases up to a limit of 0.7 whereas the accuracy of the SIR Log-PF and SIR Lin-Log-PF are limited by the number of particles. This effect is caused by the number representation of the particle weights of the SIR Lin-PF. For 𝜎𝑛 < 100 , the particle weights of the SIR Lin-PF are small and at similar order as numerical errors due to number representation. Therefore, numerical errors dominate the resampling step such that the resampling algorithm draws

RMSE

5. Simulations

10−1

10−2

108

100

100

10−8

10−100 10−125 10−25 10−55 10−75 Measurement standard deviation n SIR Log-PF MAP SIR Log-PF MMSE SIR Lin-PF MAP SIR Lin-PF MMSE

10−150

SIR Lin-Log-PF MAP SIR Lin-Log-PF MMSE Kalman filter

Figure 1: Performance evaluation of the SIR Log-PF, SIR Lin-PF, SIR Lin-Log-PF, and KF by the RMSE versus a decreasing measurement standard deviation 𝜎𝑛 .

particles based on numerical inaccuracies. Thus, the update step of the SIR Lin-PF loses its effect and we obtain 0.7 as the expected error of the process model in (25). For lower standard deviations, the accuracy of the SIR Log-PF and LinLog-PF is limited by the number of particles. The RMSE of the SIR Log-PF and Lin-Log-PF stay constant around 0.05 for the standard deviations between 10−1 > 𝜎𝑛 > 10−150 . The KF is the optimal filter for this simulation; hence, the RMSE of the KF is limited by the process noise; hence, the RMSE is constant around 0.01. To summarize, we obtain for all considered standard deviations equal or better estimation results using the SIR Log-PF compared to the SIR Lin-PF. However, the Lin-LogPF which computes the weights in log-domain using (6) obtains similar simulation results compared to the SIR LogPF. Hence, in the following we show the benefits of the LogPF by using SIS PFs, where no resampling is preformed. The resampling step is a key point for the success of a PF which is applied to avoid the degeneracy. However, resampling yields to a loss of diversity in the propagation of particles and entails an additional computational cost [14]. Similar to the SIR PF, we set the importance density to be equal to the transitional (𝑗) (𝑗) (𝑗) prior distribution, with q(x𝑘 | x𝑘−1 , z𝑘 ) = p(x𝑘 | x𝑘−1 ). Figure 2 shows the RMSE averaged over all time steps and simulations versus the decreasing measurement noise standard deviation 𝜎𝑛 . The curve looks similar to Figure 1. For 𝜎𝑛 > 10−2 , the SIS PFs obtain equivalent performance. As soon as 𝜎𝑛 decreases, the RMSE decreases for the SIS PFs until 𝜎𝑛 = 10−2 . For lower measurement noise standard deviations, the RMSE of the SIS Lin-PF and Lin-Log-PF increase up to the limit of 0.7. The RMSE of the SIS Log-PF stays constant

Journal of Electrical and Computer Engineering

7 1

101

RMSE

10

Resampling rate R()

0.75 0

10−1

10−2

108

100

10−8

0.5

0.25

0

100

10−25 10−55 10−75 10−100 10−125 Measurement standard deviation n SIS Log-PF MAP SIS Log-PF MMSE SIS Lin-PF MAP SIS Lin-PF MMSE

10−150

1



Figure 2: Performance evaluation of the SIS Log-PF, SIS Lin-PF, SIS Lin-Log-PF, and KF by the RMSE versus a decreasing measurement standard deviation 𝜎𝑛 .

around 0.03 for the standard deviations between 10−2 > 𝜎𝑛 > 10−150 . However, since the SIS PF uses no resampling step, the transformation of the weights from the log-domain to the lin-domain is not essential. Hence, if the weights of the SIS Lin-Log-PF are not transferred to the lin-domain and the (𝑗) ∗(𝑗) ̂𝑘 = 𝑤 ̂𝑘 − max𝑙 (̂ weights are normalized with 𝑤 𝑤𝑘∗(𝑙) ) and propagated to the next time instant, the Lin-Log-PF obtains equivalent estimation results than the SIS Log-PF. Therefore, the transformation to the lin-domain in Line 6 in Algorithm 6 may introduce numerical inaccuracies. As long as no normalization is needed, the Lin-Log-PF can be computed completely in the log-domain. 5.2. Nonlinear Processes: Generic Particle Filter. In this section, we use the Generic PF which decides adaptively when the resampling step is performed. Similar to the SIR PF, we set the importance density to be equal to the transitional (𝑗) (𝑗) (𝑗) prior distribution, with q(x𝑘 | x𝑘−1 , z𝑘 ) = p(x𝑘 | x𝑘−1 ). We compare the Generic Log-PF to the Generic Lin-PF and Generic Lin-Log-PF. As mentioned before, a pseudocode of the Generic Lin-Log-PF is shown in Algorithm 6. In all algorithms, resampling is performed whenever the effective sample size 𝑁eff falls below a threshold 𝑁thr . We use the (2) approximation 𝑃𝑁 (w𝑘 ) for the effective sample size in the 𝑝 (2) lin-domain and respectively ln(𝑃𝑁 (w𝑘 )) in the log-domain. 𝑝 Similar to [14], we consider a stochastic volatility model

𝑧𝑘 = 𝑒𝑥𝑘 /2 𝑛𝑘

0.25 0.5 0.75 (2) Normalized resampling bound  = PN (wk )/Np Generic Log-PF Generic Lin-PF Generic Lin-Log-PF

SIS Lin-Log-PF MAP SIS Lin-Log-PF MMSE Kalman filter

𝑥𝑘 = 𝛼𝑥𝑘−1 + V𝑘 ,

0

(27)

Figure 3: Performance evaluation of the Generic Log-PF, Generic Lin-PF, and Generic Lin-Log-PF by the resampling rate 𝑅(𝜖) versus the normalized resampling bound 𝜖 = 𝑃𝑁(2)𝑝 (w𝑘 )/𝑁𝑝 .

with 𝛼 = 0.99 and the zero-mean Gaussian distributed process and measurement noise V𝑘 ∼ N(0, 𝜎V ) and 𝑛𝑘 ∼ N(1, 𝜎𝑛 ), where 𝑛𝑘 is a multiplicative noise. Based on the measurements 𝑧𝑘 , the state sequence 𝑥𝑘 for 𝑘 = 1, . . . , 3000 is estimated using the Generic PFs with 𝑁𝑝 = 100 particles. We set the measurement noise standard deviation to 𝜎𝑛 = 0.01 and the process noise standard deviation to 𝜎V = 0.01. For performance evaluation, we try to recreate situations with rapidly changing measurements, that is, a certain model mismatch between the true likelihood of the process and the likelihood representation inside the PF. Inside the Generic PFs, we use the measurement noise standard deviation 𝜎𝑛,𝑃 = 10−4 . Hence, taking the model mismatch into account it is more likely that particle states are located in the tail of the PF’s likelihood after the prediction step. For the simulations, we (2) variate the normalized resampling bound 𝜖 = 𝑃𝑁 (w𝑘 )/𝑁𝑝 𝑝 from 0 to 1 using a grid of 0.01 resolution (equivalently for the log-domain). We simulate 1000 different realizations with known initial state and count the number of performed resampling for each run. The resampling rate is afterwards calculated as 𝑅(𝜖) = E[Number Resampling using 𝜖/3000], where E[𝑥] stands for the sample mean of 𝑥. Figure 3 shows the resampling rate 𝑅(𝜖) averaged over all time steps and simulations versus the normalized resampling bound 𝜖. For 0.2 < 𝜖 < 0.8, the Generic Log-PF has a lower resampling rate than the Generic Lin-PF and the Generic LinLog-PF. Figure 4 shows the RMSE versus the resampling rate 𝑅(𝜖), where the RMSE decreases with increasing resampling rate for all Generic PFs. However, we can observe that, for a resampling rate of 0.15 < 𝑅(𝜖) < 0.5, we obtain a slightly lower RMSE with the Generic Log-PF than using the Generic Lin-PF or Generic Lin-Log-PF.

8

Journal of Electrical and Computer Engineering rT,i,k

1.3

1.25

di,k−1

1.2 RMSE

xu,k+1 1.15

xu,k xu,k−1

1.1

1.05

uk+1

uk

dj,k 0

0.25

0.5

0.75

1

Resampling rate R() Generic Log-PF MAP Generic Log-PF MMSE Generic Lin-PF MAP

dj,k−1

dj,k+1

Generic Lin-PF MMSE Generic Lin-Log-PF MAP Generic Lin-Log-PF MMSE

Figure 4: Performance evaluation of the Generic Log-PF, Generic Lin-PF, and Generic Lin-Log-PF by the RMSE versus the resampling rate 𝑅(𝜖).

5.3. Rao-Blackwellization. In this section we consider a simultaneous localization and mapping (SLAM) example with radio signals indicated in Figure 5 according to the system model in [19] (similar results are expected in, e.g., belief propagation [20] or distributed PFs [13]). A receiver which is moving along an arbitrary trajectory is measuring the distances 𝑇

d𝑘 = [𝑑1 (𝑘) , . . . , 𝑑𝑁𝑘 (𝑘)] ,

(28)

󵄩 󵄩 𝑑𝑖 (𝑘) = 󵄩󵄩󵄩r𝑢,𝑘 − r𝑇,𝑖,𝑘 󵄩󵄩󵄩 + 𝑐𝑖,𝑘 + 𝑛𝑖,𝑘 ,

(29)

with between the receiver at location r𝑢,𝑘 and 𝑖 = 1, . . . , 𝑁𝑘 transmitters at location r𝑇,𝑖,𝑘 with the distance offset 𝑐𝑖,𝑘 and zero-mean Gaussian distributed measurement noise 𝑛𝑖,𝑘 ∼ N(0, 𝜎𝑖 ) with standard deviation 𝜎𝑖 . The receiver uses the control input u𝑘 to move from state x𝑢,𝑘−1 to state x𝑢,𝑘 . In order to use the distance measurements d𝑘 for positioning, the positioning algorithm estimates the receiver and transmitter states simultaneously. The state vector x𝑘 describing the complete system at time instant 𝑘 is 𝑇

𝑇 𝑇 x𝑘 = (x𝑢,𝑘 , x𝑇,𝑘 ) ,

(30)

True receiver position

True transmitter position

Est. receiver position

Est. transmitter position

Figure 5: Overview of the SLAM example: the moving receiver simultaneously estimates its position and the location of the transmitters.

for 𝑖 = 1, . . . , 𝑁𝑘 transmitters. We consider a static environment with a fixed number of transmitters and a receiver moving along an arbitrary trajectory. However, for notational convenience, a time dependence on 𝑘 is introduced here for the transmitter positions r𝑇,𝑖,𝑘 . Additionally, we assume based on [19] that the distance offset 𝑐𝑖,𝑘 is constant. The state space is estimated by an algorithm according to [19, 21] based on Rao-Blackwellization [22], where the states space of x𝑘 is partitioned into subspaces. Hence, we use PFs to estimate the subspaces representing the transmitters inside a PF. The reason to use a PF instead of a low complexity extended Kalman filter (EKF) is the nonlinearity of the measurements in (29). The algorithm of [19, 21] is based on a superordinate particle filter (superPF) and subordinate particle filters (subPFs). Each particle 𝑗 = 1, . . . , 𝑁𝑝 of the

𝑇

(31)

(32)

p (x𝑢,𝑘 , x𝑇,𝑘 | d1:𝑘 , u1:𝑘 , x𝑢,0 ) ≈ ∑ 𝑤𝑘 𝛿 (x𝑢,𝑘 − x𝑢,𝑘 ) , (33)

𝑁𝑝

x𝑇,𝑖,𝑘 =

(𝑗) 𝑇

(𝑗,𝑎)

with 𝑇 (r𝑇𝑇,𝑖,𝑘 , 𝑐𝑖,𝑘 ) ,

(𝑗) 𝑇

(𝑗)

superPF with the state vector x𝑢,𝑘 = (r𝑢,𝑘 , k𝑢,𝑘 )𝑇 holds 𝑁𝑘 subPFs. Each subPF is represented by the particles x𝑇,𝑖,𝑘 with 𝑎 = 1, . . . , 𝑁𝑃,𝑗,𝑖 where 𝑁𝑃,𝑗,𝑖 stands for the number of (𝑖) . particles in the 𝑖th subPF with 𝑖 = 1, . . . , 𝑁𝑘 , estimating x𝑇,𝑖,𝑘 Similar to [19, 21], the posterior distribution p(x𝑢,𝑘 , x𝑇,𝑘 | d1:𝑘 , u1:𝑘 , x𝑢,0 ) can be approximated by importance samples, as

with the receiver x𝑢,𝑘 and the transmitter states x𝑇,𝑘 which are 𝑇 𝑇 ) includes also unknown. The receiver state x𝑢,𝑘 = (r𝑇𝑢,𝑘 , k𝑢,𝑘 the receiver position r𝑢,𝑘 and the receiver velocity k𝑢,𝑘 , while the transmitter states are defined by 𝑇 𝑇 ) x𝑇,𝑘 = (x𝑇,1,𝑘 , . . . , x𝑇,𝑁 𝑘 ,𝑘

rT,j,k

𝑗=1

(𝑗)

(𝑗)

Journal of Electrical and Computer Engineering

9

(𝑗)

where 𝑤𝑘 defines the weight for the 𝑗th particle at time instant 𝑘 with (𝑗)

𝑁𝑘 𝑁𝑃,𝑖,𝑗

(𝑗)

(𝑗,𝑎)

𝑤𝑘 ∝ p (d𝑘 | x𝑢,𝑘 , d𝑘−1 ) ∝ ∏ ∑ 𝑤𝑖,𝑘

12 End position

10 8

(34)

𝑖=1 𝑎=1

and the weight of the 𝑎th particle of the 𝑖th subPF for the 𝑗th particle of the superPF at time instant 𝑘 with (𝑗,𝑎)

(𝑗)

6

y (m)

(𝑗,𝑎) 𝑤𝑖,𝑘

4

Transmitter 3

Transmitter 1 2

(𝑗,𝑎)

𝑤𝑖,𝑘 ≜ p (𝑑𝑖,𝑘 | x𝑢,𝑘 , x𝑇,𝑖,𝑘 ) .

(35) 0

Resampling is performed at each time instant to prevent degeneration; hence, (34) and (35) do not depend on the (𝑗) (𝑗,𝑎) weights 𝑤𝑘−1 and 𝑤𝑖,𝑘−1 , respectively. In the following we compare the position accuracy of three different implementations: (i) Lin-PF-SLAM. SLAM algorithm which calculates the posterior distribution according to (33), (34), and (35) in the lin-domain as proposed in [19, 21].

Receiver starting position

−2 −2

Transmitter 2

0

2

4 x (m)

6

8

10

Figure 6: Simulated scenario with three transmitters indicated by the red circles and a receiver moving on the blue track. The figure indicates the propagation paths from the transmitters to the receiver for 𝑘 = 30 s.

Table 2: Transmitter properties.

(ii) Log-PF-SLAM. SLAM algorithm which calculates the posterior distribution in the log-domain. By transferring (33), (34), and (35) to the log-domain, we obtain (𝑗)

(𝑗)

̂𝑘 ∝ ln (p (d𝑘 | x𝑢,𝑘 , d𝑘−1 )) 𝑤 𝑁𝑘

𝑁𝑃,𝑖,𝑗

𝑖=1

𝑎=1

𝑁𝑘

(𝑗,𝑎)

(𝑗,𝑎) 𝑁𝑃,𝑖,𝑗

𝑤𝑖,𝑘 }𝑎=1 ) . ∝ ∑ln ( ∑ 𝑒𝑤̂𝑖,𝑘 ) = ∑Jacob ({̂

Transmitter 𝑖 Transmitter 1 Transmitter 2 Transmitter 3

𝑥-pos [m] 0 3 4

1 0

(𝑗)

= 𝑒𝑤̂𝑘

−max 𝑙 (̂ 𝑤𝑘(𝑙) )

,

(37)

𝑁𝑘

∝ ∑max 𝑚 𝑖=1

(𝑗,𝑚) (̂ 𝑤𝑖,𝑘 )

Δ𝑘

0

0 1 0 Δ𝑘 ) ( ̇ ̇ ) A (Ψ̇ 𝑘 , Δ 𝑘 ) = ( (0 0 cos (Ψ𝑘 ) − sin (Ψ𝑘 )) , 0 0 sin (Ψ̇ 𝑘 ) cos (Ψ̇ 𝑘 ) (

(40)

)

0

where (𝑗) ̂𝑘 𝑤

𝜎𝑖 [m] 0.001 0.01 0.01

with

(iii) Lin-Log-PF-SLAM. SLAM algorithm which calculates the weights in the log-domain and transfers the weights afterwards to the lin-domain using (6). Hence, using (6) for (34), we obtain +(𝑗)

𝑐𝑖,𝑘 [m] 1 1 0

(36)

𝑖=1

𝑤𝑘

𝑦-pos [m] 2 0 0

𝑁𝑃,𝑖,𝑗

(𝑗,𝑎)

(𝑗,𝑚)

̂𝑖,𝑘 −max (̂ 𝑤𝑖,𝑘 ) 𝑤

+ ln ( ∑ 𝑒

𝑚

) . (38)

𝑎=1

We evaluate the performance of the different algorithms using a two dimensional scenario with three static transmitters and a moving receiver indicated in Figure 6. The transmitters are located at constant locations summarized in Table 2. The receiver is moving on a random pathway for 60 s with a system sampling interval of 1 s. The transition model of the receiver states uses a standard discrete white noise acceleration model [23], with x𝑢,𝑘 = A (Ψ̇ 𝑘 , Δ 𝑘 ) x𝑢,𝑘−1 + bn𝑇𝑡,𝑘 ,

(39)

0 b = ( ), 1

(41)

1 in a two-dimensional Cartesian coordinate system with the zero-mean multivariate Gaussian distributed process noise n𝑡,𝑘 ∼ N(0, 𝜎V I) with standard deviation 𝜎V . The receiver state vector x𝑢,𝑘 = (r𝑢,𝑘 , k𝑢,𝑘 )𝑇 consists of the 𝑥-𝑦 positions r𝑢,𝑘 = (𝑟u,x,𝑘 , 𝑟u,y,𝑘 )𝑇 and the velocities k𝑢,𝑘 = (Vu,x,𝑘 , Vu,y,𝑘 )𝑇 where Vu,x,𝑘 , Vu,y,𝑘 are the corresponding velocities in 𝑥-𝑦 direction. The transition matrix in (40) includes a rotation matrix with the heading changes Ψ̇ 𝑘 , which are used as control input u𝑘 . The transmitter states x𝑇,𝑖,𝑘 are time-invariant; hence, we obtain for the transition prior p(x𝑇,𝑖,𝑘 | x𝑇,𝑖,𝑘−1 ) of the 𝑖th transmitter p(x𝑇,𝑖,𝑘 | x𝑇,𝑖,𝑘−1 ) = 𝛿(x𝑇,𝑖,𝑘 − x𝑇,𝑖,𝑘−1 ).

10

Journal of Electrical and Computer Engineering

Conflicts of Interest

4/10

The authors declare that there are no conflicts of interest regarding the publication of this paper.

RMSE (m)

3/10

Acknowledgments 2/10

The authors would like to thank Patrick Robertson and Luigi Bruno for many interesting discussions on this problem. This work has been performed in the framework of the DLR project Navigation 4.0 and the European Union Horizon 2020 research and innovation programme under Grant Agreement no. 636537 HIGHTS (High precision positioning for Cooperative-ITS applications).

1/10

0 0

10

20 30 40 Receiver traveled time (s)

50

60

Log-PF SLAM MMSE Lin-PF SLAM MMSE Lin-Log-PF SLAM MMSE

Figure 7: RMSEs of the estimated receiver positions versus receiver traveled time for different algorithms.

The simulations are performed using 𝑁𝑝 = 3000 and 𝑁𝑃,𝑗,𝑖 = 1000 particles for all transmitters 𝑖 = 1, . . . , 𝑁𝑘 . For the initialization, we use prior information x𝑢,0 which is the knowledge on the starting position and velocity. Please note that an unknown starting position and direction or larger initial uncertainties may result in a biased and rotated coordinate system for the estimation. For simplicity, we use also prior information on the transmitter states x𝑇,𝑖,0 (please see [19, 21], e.g., on unknown transmitter states). The prior information includes a Gaussian distribution with standard deviation of 0.5 m centered around the true transmitter states, that is, position and distance offset. For computing the position estimate ̂r𝑢,𝑘 , we use the MMSE estimate as introduced in [19, 21]. Figure 7 shows the RMSE versus the receiver traveled time for the different implementations with RMSEu,𝑘 = √E{‖r𝑢,𝑘 − ̂r𝑢,𝑘 ‖2 } and 200 independent evaluations. At the starting time, the RMSE for all algorithms are similar because of the identical initialization. Afterwards, the RMSE increases caused by the dilution of precision (DOP). However, we can clearly see that we obtain a higher accuracy using the Log-PF compared to the Lin-PF and the Lin-Log-PF.

6. Conclusion In this paper we derived a particle filter representation in logarithmic domain, called Log-PF. The derivations show that the weight calculation, weight normalization, resampling, and point estimations can be expressed in logarithmic domain using the Jacobian logarithm. Representing the weight of each particle in logarithmic domain allows reducing the effect of numerical issues. Furthermore, the algorithm derived in this paper can be generalized to multidimensional nonparametric marginalization.

References [1] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to nonlinear/non-gaussian Bayesian state estimation,” IEE proceedings Radar Signal Processing, vol. 140, no. 2, pp. 107– 113, 1993. [2] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, 2002. [3] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the kalman filter: particle filters for tracking applications, Artech House, 2004. [4] R. van der, N. Merwe, A. Doucet, and E. Wan, “The Unscented Particle Filter,” Tech. Rep., Cambridge University Engineering Department, August 2000. [5] Z. Chen, “Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond,” Tech. Rep., McMaster University, 2003. [6] F. Gustafsson, “Particle filter theory and practice with positioning applications,” IEEE Aerospace and Electronic Systems Magazine, vol. 25, no. 7, pp. 53–81, 2010. [7] L. Martino, J. Read, V. Elvira, and F. Louzada, “Cooperative parallel particle filters for online model selection and applications to urban mobility,” Digital Signal Processing, vol. 60, pp. 172–185, 2017. [8] C. C. Drovandi, J. M. McGree, and A. N. Pettitt, “A sequential Monte Carlo algorithm to incorporate model uncertainty in Bayesian sequential design,” Journal of Computational and Graphical Statistics, vol. 23, no. 1, pp. 3–24, 2014. [9] J. Lien, U. J. Ferner, W. Srichavengsup, H. Wymeersch, and M. Z. Win, “A comparison of parametric and sample-based message representation in cooperative localization,” International Journal of Navigation and Observation, Article ID 281592, 2012. [10] J. Erfanian, S. Pasupathy, and G. Gulak, “Reduced Complexity symbol detectors with parallel structures for isi channels,” IEEE Transactions on Communications, vol. 42, no. 234, pp. 1661–1671, 1994. [11] W. Koch and A. Baier, “Optimum and sub-optimum detection of coded data disturbed by time-varying intersymbol interference,” in Proceedings of the IEEE Global Telecommunications Conf., vol. 3, pp. 1679–1684, December 1990. [12] P. Robertson, E. Villebrun, and P. Hoeher, “A comparison of optimal and sub-optimal MAP decoding algorithms operating in the log domain,” in Proceedings of the IEEE Int. Conf. on Communications, vol. 2, pp. 1009–1013, June 1995.

Journal of Electrical and Computer Engineering [13] S. Zhang, E. Staudinger, W. Wang, C. Gentner, A. Dammann, and E. Sandgren, “DiPLoc: Direct signal domain particle filtering for network localization,” in Proceedings of the 28th International Technical Meeting of the Satellite Division of the Institute of Navigation, ION GNSS 2015, pp. 2267–2274, usa, September 2015. [14] L. Martino, V. Elvira, and F. Louzada, “Effective sample size for importance sampling based on discrepancy measures,” Signal Processing, vol. 131, pp. 386–401, 2017. [15] G. Kitagawa, “Monte Carlo filter and smoother for nonGaussian nonlinear state space models,” Journal of Computational and Graphical Statistics, vol. 5, no. 1, pp. 1–25, 1996. [16] A. Doucet, N. J. Gordon, and V. Krishnamurthy, “Particle filters for state estimation of jump Markov linear systems,” IEEE Transactions on Signal Processing, vol. 49, no. 3, pp. 613–624, 2001. [17] S. Saha, Y. Boers, H. Driessen, P. K. Mandal, and A. Bagchi, “Particle based map state estimation: a comparison,” in Proceedings of the In Proc. of the 12th Int. Conf, pp. 278–283, July 2009. [18] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45, 1960. [19] C. Gentner, T. Jost, W. Wang, S. Zhang, A. Dammann, and U.C. Fiebig, “Multipath assisted positioning with simultaneous localization and mapping,” IEEE Transactions on Wireless Communications, vol. 15, no. 9, pp. 6104–6117, 2016. [20] A. T. Ihler, J. W. Fisher, R. L. Moses, and A. S. Willsky, “Nonparametric belief propagation for self-localization of sensor networks,” IEEE Journal on Selected Areas in Communications, vol. 23, no. 4, pp. 809–819, 2005. [21] C. Gentner, R. P¨ohlmann, M. Ulmschneider, T. Jost, and S. Zhang, “Positioning using terrestrial multipath signals and inertial sensors,” Mobile Information Systems, vol. 2017, pp. 1–18, 2017. [22] A. Doucet, N. de Freitas, P. Kevin, and J. Stuart, “RaoBlackwellised Particle Filtering for Dynamic Bayesian Networks,” in Proc. Conf. Uncertainty in Artificial Intelligence, pp. 176–183, UAI, 2000. [23] Y. Bar-Shalom, Li. Rong, and T. Kirubarajan, Estimation with Applications to Tracking And Navigation: Theory Algorithms and Software, Wiley Sons, 2004.

11

International Journal of

Rotating Machinery

(QJLQHHULQJ Journal of

Hindawi Publishing Corporation http://www.hindawi.com

Volume 201

The Scientific World Journal Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

International Journal of

Distributed Sensor Networks

Journal of

Sensors Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Journal of

Control Science and Engineering

Advances in

Civil Engineering Hindawi Publishing Corporation http://www.hindawi.com

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Volume 2014

Submit your manuscripts at https://www.hindawi.com Journal of

Journal of

Electrical and Computer Engineering

Robotics Hindawi Publishing Corporation http://www.hindawi.com

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Volume 2014

VLSI Design Advances in OptoElectronics

International Journal of

Navigation and Observation Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Hindawi Publishing Corporation http://www.hindawi.com

Hindawi Publishing Corporation http://www.hindawi.com

Chemical Engineering Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Volume 2014

Active and Passive Electronic Components

Antennas and Propagation Hindawi Publishing Corporation http://www.hindawi.com

$HURVSDFH (QJLQHHULQJ

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

+LQGDZL3XEOLVKLQJ&RUSRUDWLRQ KWWSZZZKLQGDZLFRP

9ROXPH

Volume 201-

International Journal of

International Journal of

,QWHUQDWLRQDO-RXUQDORI

Modelling & Simulation in Engineering

Volume 2014

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Shock and Vibration Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

Advances in

Acoustics and Vibration Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014