Augmented Filtering Based on Information Weighted Consensus Fusion for Simultaneous Localization and Tracking via Wireless Sensor Networks

This paper develops a novel augmented filtering framework based on information weighted consensus fusion, to achieve the simultaneous localization and tracking (SLAT) via wireless sensor networks (WSNs). By integrating augmented transition and observation models, we formulate a dynamical system that encodes both the target moving manners and coarse sensor locations in an augmented state. We then conduct augmented filtering based on augmented extended Kalman filters to estimate the augmented state. We further refine our target estimate according to information weighted consensus filtering which fuses the target information obtained from neighboring sensors. The fused information is fed back as the target estimate to the augmented filter. Our framework is computationally efficient because it only requires neighboring sensor communications. Experiments on SLAT problem validate the effectiveness of the proposed algorithm in terms of tracking accuracy and localization precision in limited ranging conditions.


Introduction
Simultaneous localization and tracking (SLAT) has gained great research interest recently.It is popular to formulate SLAT framework using a centralized architecture, where all the sensors transmit their measurements to a central node that fuses the received information and computes target trajectories.One typical example is the SLAT framework based on Bayesian inference proposed in [1], in which the sensor positions are assumed unknown and thus moment matching is required to obtain the position information.Similarly, in non-line-of-sight environments, cubature Kalman filters have been developed for SLAT [2], with augmented state vector constructed by concatenating a target state and a sensor location.Traditional SLAT frameworks rely on centralized fusion of sensor-based target state estimation, and they suffer from heavy communication overheads and are thus inefficient.Furthermore, the performance of a centralized SLAT solution is significantly affected by the central sensor, which limits the robustness of the whole sensor network.
Distributed SLAT frameworks can possibly overcome the aforementioned problems and have attracted increasing research interest.For one thing, they show significant potential in improving individual sensor functionalities that are usually limited by their simple hardware implementations.For another, they can thoroughly exploit the advantages supported by distributed sensing [3].One early attempt of the distributed strategies is the decentralized data fusion method with all-to-all sensor communications [4], where a decentralized version of the recursive maximum likelihood for Hidden Markov Model and belief propagation message passing algorithms have been exploited to localize the sensor network simultaneously with target tracking.Almost at the same time, a distributed variational filter for SLAT has been proposed [5], which takes the messages with both belief propagation and bandwidth consumption into consideration.However, these distributed frameworks rely on a specific communication network topology and are not generally applicable to arbitrarily connected networks.Most of them are derived based on the belief propagation method in 2 International Journal of Distributed Sensor Networks Bayesian filtering framework, which may lead to heavy computation costs.Moreover, these methods heavily rely on distance measurements, which, however, are not always guaranteed in practice due to the limited sense range (LSR) of individual sensors [6].
The difficulty of sensor localization lies in the fact that a target may not be observed by some or all sensors in a practical network.To overcome this limited observability, many strategies have been proposed, such as the distributed sensor localization framework with weighted consensus [7], the hybrid peer-to-peer tracking architecture with Kalman consensus filter [8], and the information weighted consensus filter [9].In particular, particle-based distributed message passing algorithms for SLAT have been employed to solve the LSR problem.Here, one representative study is the intersensor measurements composed of nonparametric belief propagation with a likelihood consensus scheme [10].However, LSR conditions are task-specific and these methods are not applicable to a general SLAT scenario.
In order to solve the practical SLAT problem with LSR constraints, we propose a distributed SLAT algorithm for the networks whose sensors can cooperate by exchanging messages among neighbors.Specifically, we design a distributed augmented consensus estimator for SLAT under LSR conditions.Our framework inherits some desirable properties from the information weighted filter (ICF) [9].The errors in the information held by each sensor become highly correlated with each other as consensus approaches.We thus utilize the cross-covariances of the information to estimate the optimal weights of prior states and measurements.By employing a general state evolution model, we are able to characterize the coupling state for both the target and one activated sensor and then estimate the model state through augmented filtering based on the augmented extended Kalman filter.We then update the target state through consensus filtering by fusing the distributed filtering information and the weighted consensus information.
Comparing with the belief propagation schemes, our framework has the advantage of low computation cost because it reduces the unnecessary communication overheads.Furthermore, it addresses the error decoupling problem, which is usually unavoidable in the algorithms that distributively estimate sensor positions and target trajectories, by proposing a two-stage filtering architecture.
The structure of the paper is as follows.We begin with the specifications of the statistical model for the localization and tracking problem in Section 2. Section 3 proposes the distributed augmented filter with consensus.Section 4 presents numerical examples on small sized networks and validates the proposed framework.Section 5 discusses and concludes the paper.

Preliminaries
Consider one wireless sensor network consisting of a set of  sensors.Assume that the sensor network is an undirected graph and the communication range of the sensors determines the topology of the network.As illustrated in Figure 1, we denote the communication range and sensing range of the sensors as   and   , respectively.For LSR, ratio   /  is sufficiently smaller than 0.5 [8].Notation    denotes the measured distance between a sensor  and the target at the time step .If    <   , the sensor is active; otherwise, it is in a passive status.
We denote the state vector of a target as  ]  ∈ R 4×1 with respect to sensor .Specifically, for a moving target on a 2D plane, ( , , ,  , , ) denote the target position and (V , , , V , , ) denote the target velocities along -axis and -axis, respectively.In ideal conditions, when the distributed system achieves convergence for a long time, we could get the relationship as We use a linear Gaussian model to formulate the target state transition: where is the state transition matrix, and   −1 ∈ R 4×4 is the noise distribution matrix.In practice, every sensor maintains an individual state for the target.These states are then fused to achieve a consensus subject to certain optimizing rules.
We denote the coordinate of sensor  as  ,  = [ , , ,  , , ]  ∈ R 2×1 , where superscript  means that  , , or  , , denotes a sensor state.As the sensors are randomly deployed, selflocalization is required for each sensor.In this context, the state transition for sensor  is formulated using a static model: where  , −1 ∈ R 2×1 is zero-mean white Gaussian noise with covariance matrix  , −1 ∈ R 2×2 , which is used to describe the uncertainty of sensor localization.

Augmented Filtering Based on Information Weighted Consensus Fusion
This section proposes an augmented filtering framework based on information weighted consensus fusion, for the purpose of target tracking and sensor localization.We first introduce the augmented transition and measurement models and then describe how to exploit augmented filtering to update the coupling information of the target state and sensor localization of each node.We establish the framework by developing an information weighted consensus filtering scheme which exploits online consensus fusion of local neighboring information to refine the target states.As an integrated framework, each augmented filter is specific to a separate sensor subsystem and gets feedback from the local information weighted consensus filter.Such structure enables the elimination of coupling errors for localization and tracking and thus improves the system accuracy.

Augmented Models.
In this subsection, we introduce the basic augmented model used in our framework.Specifically, the distance between a target state described in (2) and the sensor location of  described in (3) is given as follows: For a sensor network with LSR, sensor  can sense a target only if it falls within its sensing range at time step , that is,    ≤   .The group of these sensors   = { |  ∈  and    ≤   } is referred to as the active nodes subset.The remaining sensors form the passive sensor subset   , which does not obtain any meaningful measurement of the target because it is beyond their sensing ranges.
We can integrate the position information of a target and a sensor into one augmented vector    = [( ,  )  , ( ,  )  ]  ∈ R 6×1 .Similarly, we define the following parameters of an augmented system, Then the augmented state transition is formulated as follows: We assume the measurement model as a linear Gaussian model over a set of range measurements under measurement noise.Therefore, the measured receiving power by sensor  at time stamp ,    , can be denoted as follows: where   and  denote the transmission power and the path loss exponent, respectively.They are determined subject to the radio environment and the antenna characteristics [11].
V   is the measurement noise with covariance   .Following the relations in ( 4)-( 6), we obtain Based on (7), measurement ( 6) is converted to The aim of our study is to develop a distributed consensus estimator for the system characterized by ( 5) and ( 8), which are introduced in the following subsections.

Preliminary Augmented Filtering.
In order to estimate the states of a sensor and the target in a fully decentralized manner, an extended Kalman filter based estimator for augmented state (EKFAug) is applied.This procedure is referred to as augmented filtering where the estimator receives feedback from the information weighted consensus filter.
In this scenario, we assume that the one-step estimate of the augmented state x −1 and the error covariance P −1 is updated with information weighted consensus filtering for sensor  at time step  − 1.We adopt a linear approximation of observation model (8) for sensor , which uses Taylor expansion on the argument of the predicted mean   −1 as follows: where A single forward operation of the EKFAug on sensor  at the current time step is as follows.

International Journal of Distributed Sensor Networks
Step 1. Prediction is as follows: Step 2. Estimation is as follows: where    is the Jacobian transformation of function ℎ(   ), which calculates the derivative for each variable of the predicted nonlinear observation of the augmented state.   is the predicted covariance of the filter and    is the measurement noise.For the active and passive scenarios, it is formulated separately as follows: (16)

Fusion Based Refinement.
The EKFAug discussed in the previous subsection is specific to one individual sensor.The estimate of an augmented state based on ( 14) and ( 15) is only concerned with the individual sensor and does not explore information exchange between neighboring sensors.In order to characterize local neighboring information, we fuse their estimates by developing a distributed information filter with weighted consensus fusion and feed such information to the previous extended Kalman estimator.We refer to the overall framework as EKFAug-ICF, which stands for extended Kalman filter based estimator for augmented state with information weighted consensus fusion.Given the condition that the sensor state prediction  ,  is available from the EKFAug, Taylor expansion of ( 8) is used to approximate observation of sensor .Based on state  ,  of the information weighted consensus filter, we have where Note that observation model ( 17) is different from ( 9) in terms of augmented filtering.In (17) only the target states are used to approximate ℎ(⋅); in (9), both the target states and sensor locations are used.
Let   denote the set of neighboring sensors of sensor .Each sensor in   has a direct communication channel to sensor .Neighboring sensors can communicate with each other either directly or through the central node in such a way that information is allowed to be passed between them.In the SLAT scenario, messages can be exchanged between a sensor  and all of its neighbors in   .Given  ,  and  ,  of sensor , the consensus fusion is conducted as follows.
Step 1. Prepare data for fusion: where  ,  = (ℎ/  ( For all passive sensors, that is, for all  ∈   ,  ,  = 0, because no such information can be obtained.Therefore, their information vector and matrix are defined as zero, that is, for all  ∈   ,  ,  = 0 and  ,  = 0.In contrast to the all-to-all communication strategies such as the Bayesian filtering framework [5], our broadcasting procedure just exchanges neighboring information with a small amount of communication workloads and thus suffers comparatively lower computational overheads.
Step 3.After  iterations, compute state estimate and information matrix: P,  = ( Our framework is suitable for the practical applications which are limited by LSR conditions.The feedback scheme in (21) and ( 22) enables our framework to have the advantages of fault tolerance and scalability, which are validated in the experimental evaluation.11) and (12), respectively, are obtained from the augmented filters and fed to the information weighted consensus filters; outputs x,  , P,  of the weighted consensus filters denoted in (21) and ( 22) are fed back to the augmented filters as inputs.This closed-loop procedure addresses the coupling problem that normally exists in distributed SLAT and is able to correct the accumulated cross-correlated errors of sensor localization and target tracking.
The pseudo code of the proposed two-stage SLAT algorithm is summarized in Algorithm 1.

Simulation
In this section, we evaluate the performance of the proposed EKFAug-ICF algorithm using simulation experiments and compare it with the centralized matrix weighted fusion approach [12,13], denoted as EKFAug-MWF.We simulate a wireless sensor network comprising a moving target and a total of  = 9 sensors in Matlab, which are uniformly deployed with their locations kept unchanged in the experiments.Each of them has a set of wireless transceiver.
We commence by demonstrating how to solve the sensor self-localisation and target tracking problem using our framework.Dynamical model ( 2) is used to compute the target state.The process covariance is set to diag{10, 10, 1, 1}.Initial prior state x, 0 and prior covariance P, 0 are set by using the same method for each sensor.For example, P, 0 is given by For each sensor, the ranging field is considered to be a circle with a radius of 200 units.A sensor can detect a target International Journal of Distributed Sensor Networks only if the ground truth position of the target is within its range field.In this case, a measurement is generated using nonlinear observation model (8) with noise covariance   = 10 2 , where   = 100 and  = 2.5 [2].We assume that the sensors are connected using a peer-to-peer topology and the consensus iteration number is set as  = 3 [14], that is, An example of the tracking simulation experiment is demonstrated in Figure 3.The experiment results show that EKFAug-ICF performs better than EKFAug-MWF.This is because the estimation fusion with distributed consensus often achieves better error tolerance and robustness than that of the traditional state fusion approach.
We further investigate the performance of the two approaches through 200 independent trials.The simulation environment is the same with Figure 3.The root mean squared error (RMSE) of    for the two approaches is shown in Figure 4.The RMSE at time  is computed in terms of √∑ ∈ ∑ 200  ‖   − x, , ‖ 2 , where x, , denotes the estimated target state at time step  obtained from the th run.We observe that in EKFAug-MWF case the RMSE keeps increasing as step increases, and the EKFAug-ICF achieves convergence with better performance.
As target tracking updates online, the positions of sensors, which are prelocalized coarsely beforehand, are modified simultaneously.The ground truth and the estimated sensor locations of EKFAug-ICF and EKFAug-MWF are illustrated in Figure 5, which shows that the precision of sensor localization with EKFAug-ICF is greater than that of EKFAug-MWF.
Figure 6 shows the estimated errors sensor positions using the two approaches, where EKFAug-ICF provides almost identical position estimations to the ground truth.Both EKFAug algorithms work well in SLAT and ICF performs better.It can be seen that the proposed algorithm is able to accurately locate sensors within a wireless sensor network.

Discussion and Conclusion
EKFAug-ICF and EKFAug-MWF are two types of estimation methods for SLAT, where the first one is in a distributed manner while the second one is in a centralized manner.Theoretically, these two methods should have similar performance in terms of computational complexity and executing time.EKFAug-ICF may, however, encounter more difficulties in practice, especially when there exists time delay or packet loss in the communications among sensors.The proposed system is mainly targeting a typical indoor environment, where the settings are similar to those from [2] and the network setup is the same with [14].The effect of  in different scenarios, such as outdoor, will be considered in the future research.This paper proposes a filtering architecture based on extended Kalman filtering for characterizing the correlation of target tracking and sensor localization with augmented states.Specifically, we have described how to refine augmented filtering through the fusing information obtained from neighboring sensors based on the weighted information consensus filtering strategy.Our new scheme not only improves the performance of estimation through the fusion and feedback strategy but also reduces the computational overheads in sensor communication via the neighborhood confinement.The experiment results have shown that our novel framework exhibits robustness in the LSR situations.

3. 4 .
Framework Structure and Algorithm.Sections 3.2 and 3.3 have presented the two main filtering procedures of the proposed distributed estimation framework of each sensor.The overall diagram of our proposed framework is illustrated in Figure 2. Estimate    and covariance    denoted in (

Figure 3 :
Figure 3: Results of the distributed SLAT tracking experiment.

Figure 4 :Figure 5 :
Figure 4: The average error of target estimation.

Figure 6 :
Figure 6: The average error of sensor position estimation.