In this paper, we propose a box particle filtering algorithm for state estimation in nonlinear systems whose model assumes two types of uncertainties: stochastic noise in the measurements and bounded errors affecting the system dynamics.These assumptions respond to situations frequently encountered in practice. The proposed method includes a new way to weight the box particles as well as a new resampling procedure based on repartitioning the box enclosing the updated state. The proposed box particle filtering algorithm is applied in a fault detection schema illustrated by a sensor network target tracking example.
For various engineering applications, system state estimation plays a crucial role. Kalman filtering (KF) has been widely used in the case of stochastic linear systems. The Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) are KF’s extensions for nonlinear systems. These methods assume unimodal, Gaussian distributions. On the other hand, Particle Filtering (PF) is a sequential Monte Carlo Bayesian estimator which can be used in the case of non-Gaussian noise distributions. Particles are punctual states associated with weights whose likelihoods are defined by a statistical model of the observation error. The efficiency and accuracy of PF depend on the number of particles used in the estimation and propagation at each iteration. If the number of required particles is too large, a real implementation is unsuitable and this is the main drawback of PF. Several methods have been proposed to overcome these shortcomings, mainly based on variants of the resampling stage or different ways to weight the particles ([1]).
Recently, a new approach based on box particles was proposed by [2; 3]. The Box Particle Filter handles box states and bounded errors. It uses interval analysis in the state update stage and constraint satisfaction techniques to perform measurement update. The set of box particles is interpreted as a mixture of uniform pdf’s [4]. Using box particles has been shown to control quite efficiently the number of required particles, hence reducing the computational cost and providing good results in several experiments.
In this paper, we take into account the box particle filtering ideas but consider that measurements are tainted by stochastic noise instead of bounded noise. The errors affecting the system dynamics are kept bounded because this type uncertainty really corresponds to many practical situations, for example tolerances on parameter values. Combining these two types of uncertainties following the seminal ideas of [5] and [6] within a particle filter schema is the main issue driving the paper. This issue is different from the one addressed in [7] in which the focus is put on Bernouilli filters able to deal with data association uncertainty. The proposed method includes a new way to weight the box particles as well as a new resampling procedure based on repartitioning the box enclosing the updated state.
The paper is organized as follows. Section 2 describes the problem formulation. A summary of the Bayesian filtering is presented and the box-particle approach is introduced. The main steps of this approach are developed in section 3. Section 4 and 5 are devoted to the repartitioning of the boxes and the computation of the weight of the box particles in order to control the number of boxes. In section 6 the box particle filter is used for state estimation and fault detection; the results obtained with the proposed method for a target tracking in a sensor network are presented in section 7. Conclusion and future work are overviewed in the last section. 2
We consider nonlinear dynamic systems represented by discrete time state-space models relating the state x(k) to the measured variables y(k)
x(k + 1) = f (x(k), u(k), v(k))
y(k) = h(x(k)) + e(k), k = 0, 1, . . .
(
In this work the process noise is assumed bounded | vi(k)| ≤ σi with i = 1, . . . , nx, i.e pv ∼ U ([V ]), where [V ] = [−σ1, σ1] × · · · × [−σnx , σnx ]. 2.1
Given a vector of available measurements at instant k: Y(k) = { y(i), i = 1, ..., k} , Y(0) = y(0), the Bayesian solution to compute the posterior distribution p(x(k)| Y(k)) of the state vector at instant k + 1, given past observations Y(k) is given by (Gustafsson 2002):
Z
p(y(k)| x(k)) = pe(y(k) − h(x(k))
(
Equations (
Considering the assumptions of our problem, we adopt a particle filtering schema which is well-known for solving numerically complex dynamic estimation problems involving nonlinearities. However, we propose to use box particles and to base our method on the interval framework. Box particle filters have been demonstrated efficient, in particular to reduce the number of particles that must be considered to reach a reasonable level of approximation [2].
Let’s consider the current state estimate X (k) as a set,
denoted by {X (k)} , that is approximated by Nk disjoint boxes
[x(k)]i i = 1, · · · , Nk
where [x(k)]i = [x(k)i, x(k)i], with x(k)i, x(k)i ∈
Rnx . The width of every box is smaller or equal to a given
accuracy for every component, i.e
xj (k)i − xj (k)i ≤ δj i = 1, · · · , Nk, j = 1, . . . , nx
(
Moreover, every box [x(k)]i is given a prior probability
denoted as
(6)
(
P ([x(k)]i| Y(k − 1)) i = 1, · · · , Nk
Nk X P ([x(k)]i| Y(k − 1)) ≥ γ where γ ∈ [0, 1] is a confidence threshold.
Then, given a new output measurement y(k), the problem that we consider in this paper is: • to compute the state estimate X (k + 1), • to decide about the number Nk+1 of disjoint boxes of the approximation of X (k + 1), each with accuracy smaller or equal to δj , • to provide the prior probabilities associated to the particles of the new state estimation set
P ([x(k + 1)]i| Y(k)) i = 1, · · · , Nk+1 (10) 3
Interval Bayesian formulation This section deals with the evaluation of the Bayesian solution of the state estimation problem considering bounded state boxes (6). 3.1
Whereas each particle is defined as a box by (6), the measurement is tainted with stochastic uncertainty defined by the pdf pe. The weight w(k)i associated to a box particle is updated by the posterior probability P ([x(k)]i| Y(k)):
P ([x(k)]i| Y(k − 1))pe(y(k) − h([x(k)]i) then i=1
The deduction of the measurement update equation (11)
from the particle filtering update equation (
We assume that the new boxes are of the same size, that they cover the whole space defined by the union of the updated boxes [x(k + 1)| x(k)]i i = 1, . . . , Nk, and that their weight is proportional to the weight of the former boxes.
For this purpose, a support box set Z is computed as the minimum box such that Z is partitioned into M disjoint boxes of the same size
Nk Z ⊇ [[x(k + 1)| x(k)]i. [z]i i = 1, · · · , M where [z]i = [zi, zi], zi, zi ∈ Rnx , and zj − zji = εj i i = 1, · · · , M j = 1, . . . , nx.
(18) The box component widths are computed as εj =
Zj − Zj mj
j = 1, . . . , nx where mj is the number of intervals along dimension j computed as mj = ⌈
Zj − Zj δj ⌉ j = 1, . . . , nx where ⌈.⌉ indicates the ceiling function and δj the minimum accuracy for every state component j defined in Section 2.2. In this way, we guarantee that
εj ≤ δj j = 1, . . . , nx (21)
Finally, the number M of boxes of the uniform grid partition is given by
nx M = Y mj
j=1
Once the new boxes [z]i have been computed, the weight of the new boxes wzi can be computed as
Nk wzi = X j=1
Qnx l=1 | [xl(k + 1)| x(k)]j T[zl]i| w(k)j Qnx l=1 | [xl(k + 1)| x(k)]j |
i = 1, . . . , M where [vl]i refers to the l-th component of the vector [v]i and the interval width xl − xl is denoted by | [xl]| for more compactness. The new weights fulfill
M X wzi = i=1
The new weights wzi in (4.1) can be computed efficiently using Algorithm 1. This algorithm searches the number Ninter of boxes of Z that intersect every [x(k + 1)| x(k)]j . Then, the weight w(k)j is distributed proportionally to the volume of the intersection between the updated boxes [x(k + 1)| x(k)]j and each of the Ninter boxes of Z that have a non-empty intersection.
Algorithm 1 Weights of the new boxes.
Algorithm Weights-new-boxes (Z, [x(k + 1)| x(k)]1, . . . , [x(k + 1)| x(k)]Nk , w(k)1, . . . w(k)Nk ) wzi ← 0 i = 1, . . . , M for j = 1, . . . , Nk do [Ninter, Vinter ] = intersec([x(k + 1)| x(k)]j , Z) for h = 1, . . . , Ninter do i = Vinter (h) wzi = wzi + Qln=Qx1ln|=[xx1l|([kx+l(1k)+|x1()k|x)](jkT)]j[ z|l]i| w(k)j end for end for
Return (wz1, . . . , wzM ) endAlgorithm 4.2
Controlling the number of boxes Once the new disjoint boxes and their associated weights have been computed, the associated weights can be used to select the set of boxes that are worth pushing forward through the next iteration. This is performed by selecting the boxes with highest weights and discarding the others. In order to fulfill the confidence threshold criterium (9) proposed in Section 2.2, Algorithm 2 is proposed. The set Wz of weights wzi associated to the boxes [z]i is defined as
Wz = { wz1, . . . , wzM } .
Given a desired confidence threshold γ, the M disjoint boxes [z]i that compose the uniform grid partition of Z and vector Wz with the associated weights, Algorithm 2 determines the minimum number Nk+1 of boxes [z]i with highest weights wzi that fulfill
Nk+1 X wzi ≥ γ i=1
The new state estimate X (k + 1) is approximated by this set of Nk+1 boxes and their prior probability by P ([x(k + 1)]i| Y(k)) ≈ Wki+1 i = 1, . . . , Nk+1. (27) where Wki+1 are the Nk+1 highest weights of Wz associated with the disjoint boxes [x(k + 1)]i, i = 1, · · · , Nk+1, that approximate X (k + 1). Wki+1 can be referred as the a priori weights.
Algorithm 2 State update at step k + 1 with confidence threshold γ.
Algorithm State-update([z]1, . . . , [z]M ,Wz ,γ) γc ← 0, {X (k +1)} ← {∅} , Wk+1 ← {∅} , Nk+1 ← 0 while γc < γ do [value, pos] = max(Wz ) addbox(X (k + 1), [z]pos) addelement(Wk+1, value) γc = γc + value Wz(pos) ← 0
Nk+1 ← Nk+1 + 1 endwhile
Return (X (k + 1), Wk+1, Nk+1) endAlgorithm (16) (17) (19) (20) (22) (23) (24) (25) (26) This algorithm generates a set of state boxes {X (k + 1)} a list of weights Wki+1, a cumulative weight variable γc, and a cardinality variable Nk+1. At the beginning of the algorithm, the state boxes and weight list are initialized as empty sets and cumulative weight and cardinality variable are initialized to zero. The loop "while" operates as a sorting, eliminating the boxes with smallest weights so that the cumulative sum of the boxes with largest weights is greater or equal to the threshold γ. If the state space is not bounded, the threshold 0 < γ < 1 does not guarantee a bounded number of boxes in a worst-case scenario in which the measurements do not emphasize some particles against others. In this case, a maximum number of particles N max should be imposed. 5 5.1
State estimation and fault detection
Once the set of Nk+1 disjoint boxes [x(k + 1)]i, i = 1, · · · , Nk+1, that approximate X (k + 1) and their associated a priori weights Wki+1 have been computed, their measurement updated weights w(k + 1)i are obtained using (11). Then, according to [2], the state at instant k + 1 is approximated by
Nk+1 xˆ(k + 1) = X w(k + 1)ixi0(k + 1) (28) i=1 where xi0(k + 1) is the center of the particle box [x(k + 1)]i.
Algorithm 3 summarizes the whole state estimation procedure.
Algorithm State estimation
Initialize X (0), N0 and
P ([x(k)]i| Y(k − 1))k=0,i=1...N0 for k = 1, . . . , end do
Obtain Input/Output data { u(k), y(k)} Measurement update compute Λ(k) using Eq. (12) compute w(k)i using Eq.(11) i = 1 . . . N0 State estimation
compute xˆ(k) using (28) State update compute [x(k + 1)| x(k)]i i = 1 . . . N0 using (15) compute Z that fulfils (16) compute disjoint boxes [z]i i = 1, · · · , M of (17) compute weights wzi using Algorithm 1 compute new state estimation using Algorithm 2 Nk+1 disjoint boxes that approximate X (k + 1) Prior probabilities given by weights Wk+1 end for endAlgorithm 5.2
In our framework, fault detection can be formulated as detecting inconsistencies based on the state estimation. To do so, we propose the two following indicators: • Abrupt changes in the state estimation provided by (28) from instant k−1 to instant k, i.e. abnormal high values of p(xˆ(k) − xˆ(k − 1))(xˆ(k) − xˆ(k − 1))T • Abnormal low sum of the unnormalized posterior probability of all the particles at instant k, which means that all the particles have been penalized by the current measurements. This abnormality can be checked by thresholding Λ(k) defined in (12).
If enough representative fault free data are available, the indicators defined above can be determined by means of thresholds computed with these data. For example, the threshold that defines the abnormal abrupt change in state estimation can be computed as
(xˆ(i) − xˆ(i − 1)) (xˆ(i) − xˆ(i − 1))T Δxˆmax = β1 i=m2,a·x,L q (29) where L is the length of the fault free scenario and β1 > 1 a tuning parameter. Then the fault detection test consists in checking at each instant k if q (xˆ(k) − xˆ(k − 1)) (xˆ(k) − xˆ(k − 1))T > Δxˆmax (30)
In a similar way, threshold Λmin that defines the minimum expected unnormalized posterior probability can be computed as Λmin = β2 i=2,· ,L min (Λ(i)) where Λ(i) is determined using (12) and 0 < β2 < 1 is a tuning parameter. Then the fault detection test consists in checking at each instant k if
Λ(k) < Λmin 6
In this section a target tracking in a sensor network example presented in [8] is used to illustrated the state estimation method presented above. The problem consists of three sensors and one target moving in the horizontal plane. Each sensor can measure distance to the target, and by combining these a position fix can be computed. Fig. 1 depicts a scenario with a trajectory and a certain combination of sensor locations (S1, S2 and S3). (31) (32) 4 3.5 2.5 3 2 1 0 0.5
The behaviour of the system can be described by the
following discrete time state-space model:
Box particle filtering weight of boxes using measurement y1(
q y1(k) yy32((kk)) = qq(x1(k) − S2,1)2 + (x2(k) − S2,2)2 where x1(k) and x2(k) are the object coordinates bounded by −1 ≤ x1(k) ≤ 3 and −1 ≤ x2(k) ≤ 4 ∀k ≥ 0. Ts = 0.5s is the sampling time, v1(k) and v2(k) are the speed components of the target that are unknown but considered bounded by the maximum speed σv = 0.4m/s (| v1(k)| ≤ σv and | v2(k)| ≤ σv). y1(k), y2(k) and y3(k) are the distances measured by the sensors. Si,j denotes the component j of the location of sensor i. e1(k), e2(k) and e3(k) are the the stochastic measurement additive errors pei ∼ N (0, σi) with σ1 = σ2 = σ3 = √0.05m.
Fig. 2 shows the evolution of the real sensor distances and measurements in the target trajectory scenario depicted in Fig. 1.
)4 m ( 1 e2 c n a it 00 s D
In order to apply the state estimation methodology presented above, a minimum accuracy δ1 = δ2 = δ = 0.2m has been selected for both components. No a priori information has been used in the initial state. Then, a uniform grid of disjoint boxes with the same weights and component widths ε1 = ε2 = δ that covers all the bounded coordinates −1 ≤ x1 ≤ 3 and −1 ≤ x2 ≤ 4 has been chosen as initial state X (0). Posterior probabilities of the boxes have been approximated by weights w(k)i computed using the new sensor distances measurements in (4.1). State update has been computed considering speed bounds in (33). The new boxes have been rearranged considering the minimum accuracy δ and their associated weights have been computed using (4.1). Finally, Algorithm 2 with threshold γ = 1 has been applied to reduce the number of boxes.
Figs. 3 and 4 depict the box weights and their contours
using measurement y1(
Box particle filtering weight contour of boxes using measurement y1(
The real trajectory and the one estimated using (28) are shown in Fig. 6.
Finally, different additive sensor faults have been simulated and satisfactory results of the fault detection tests (30) and (32) have been obtained for faults bigger than 0.5m using thresholds Δxˆmax and Λmin computed with (29) and (31)with L = 3200, β1 = 1.1 and β2 = 0.9.
Fig. 7 shows the real trajectory and the one estimated using (28) when an additive fault of +0.5m affects sensor S1 at time k = 22. The behaviour of fault detection tests (30) and (32) is depicted in Fig. 8. As seen in this figure, both thresholds are violated at time instant k = 22 and therefore the fault is detected at this time instant.
Box particle filtering weight of boxes using available measurements at instant k=2 2 0 Box particle filtering weight contour of boxes using available measurements at instant k=2 4 real Box Particle Filter
S2 1 x1 (m) S1 10 10 Real trajectory Box particle estimation
S1
S2 1 x1 (m)
S3 −0.5 0 0.5 1 1.5 2 2.5 3 A Box particle algorithm has been proposed for estimation and fault detection in the case of nonlinear systems with stochatic and bounded uncertainties. Using this method in the case of a target tracking sensor networks illustrates its feasibility. It has been shown how the measurement update state for the box particle is derived from the particle case. However convergence and stability of this filter have to be proved. Resampling unfortunatly drops information and waives guaranteed results that characterize interval analysis based solutions. However without resampling the particle filter suffers from sample depletion. This is the reason why resampling is a critical issue in particle filtering (Gustafsson 2002). This approach has to be compared to other PF variants which reduce the number of particles [2] and further investigations concerning resampling are required, in particular if we want to take better benefit of the interval based approach.
This work has been partially funded by the Spanish Ministry of Science and Technology through the Project ECOCIS (Ref. DPI2013-48243-C2-1-R) and Project HARCRICS (Ref. DPI2014-58104-R).
A
Demonstration of Measurement update: "From particles to boxes" A.1
Consider the particles { x(k)j } jN=1 uniformly distributed in x(k)j ∈ [x(k), x(k)] ∀j = 1, . . . , N where x(k), x(k) ∈ R. Then according to [1] the relative posterior probability for each particle is approximated by
1 P (x(k)j| Y(k)) ≈ c(k) P (x(k)j| Y(k − 1))pe(y(k) − h(x(k)j)) with
N c(k) = X P (x(k)j | Y(k)) j=1 (35) (36) { x(k)l} li=Δ1N+(i−1)ΔN ∈ [x(k)]i [x(k)]i = [x(k) + (i − 1)ΔL, x(k) + iΔL] where with ∞ according to (35) ΔL = x(k) − x(k)
Ng iΔN
X j=1+(i−1)ΔN If the number of particles N → ∞ and therefore ΔN → P ([x(k)]i| Y(k)) ≈
P (x(k)j | Y(k)) (41)
P ([x(k)]i| Y(k)) ≈ PiΔN
j=1+(i−1)ΔN P (x(k)j| Y(k − 1))pe(y(k) − h(x(k)j)) PlN=g1 PljΔ=N1+(l−1)ΔN P (x(k)j| Y(k − 1))pe(y(k) − h(x(k)j)) (42)
If we consider the particles in the same group i have the same prior probabilities, then:
P ([x(k)]i| Y(k − 1))
ΔN and (42) leads to
p(x(k)j | Y(k − 1)) = ∀j = 1 + (i − 1)ΔN, . . . , iΔN (37) (38) (39) (40) (43) (45) (46) iΔN
X j=1+(i−1)ΔN Z (iΔN)Δx(k) (1+(i−1)ΔN)Δx(k)
Z x(k)∈[x(k)]i pe(y(k) − h(x(k)j ))Δx(k) ≈ pe(y(k) − h(x(k)))dx(k) ≈
(47) pe(y(k) − h(x(k)))dx(k)
Finally, multiplying the numerator and denominator of equation (44) by Δx, we obtain the particle box measurement update equation
P ([x(k)]i| Y(k − 1)) Rx(k)∈[x(k)]i pe(y(k) − h(x(k)))dx(k) PlN=g1(P ([x(k)]l| Y(k − 1)) Rx(k)∈[x(k)]l pe(y(k) − h(x(k)))dx(k)) (48) that corresponds to the equation (11) with
P ([x(k)]i| Y(k)) ≈ Ng X(P ([x(k)]l| Y(k − 1)) l=1
Z x(k)∈[x(k)]l pe(y(k) − h(x(k)))dx(k)) Λ(k) = (49)
P ([x(k)]i| Y(k)) ≈ P ([x(k)]i| Y(k − 1)) PiΔN
j=1+(i−1)ΔN pe(y(k) − h(x(k)j))
If the N particles are uniformly distributed in the interval [x(k), x(k)], i.e PlN=g1(P ([x(k)]l| Y(k − 1)) PlΔN j=1+(l−1)ΔN pe(y(k) − h(x(k)j))) [6] J. Xiong, C. Jauberthie, L. Travé-Massuyès, and F. Le (44) Gall. Fault detection using interval kalman filtering enhanced by constraint propagation. In Proceedings of the IEEE Conference on Decision and Control, pages 490– 495, 2013. where
x(k)j − x(k)j−1 = Δx(k) ∀j = 2, . . . , N Δx(k) = x(k) − x(k)
N = Proceedings of the 26th International Workshop on Principles of Diagnosis 74