Multisensor Target Tracking with Delayed States

Anindha Parthy, Tim Bailey, Hugh Durrant-Whyte

AbstractIn any system consisting of sensors connected via a very computationally intensive task. A distributed processing
complex communication network, the arrival of observations and architecture would be advantageous since the cost of multiple
information at the fusion process will be delayed, and potentially low power processing units is significantly less than a single
out of order. This problem is known in the literature as the out-
of-sequence measurement (OOSM) problem. This paper proposes processing unit with equal capability. A fully decentralised
a delayed state approach, using the information-form of the system has the additional advantage of improved reliability
Kalman filter, to the problem of multisensor target tracking. and survivability, since there is no single point of failure. An
By retaining the statistical dependencies between past state issue with decentralised architectures are the high bandwidth
estimates, this approach provides an exact solution to the OOSM requirements. A naive solution where sensors send all raw
problem. The delayed state approach allows the estimate to the
represented by a sparse information matrix, which results in a measurements to each of the processing nodes, results in
computationally efficient estimation process. This approach also bandwidth requirements proportional to the number of sensors.
facilitates efficient communication and a decentralised processing With our approach, if a number of sensors observe a target
architecture, which are desirable properties when deploying simultaneously, the bandwidth required to communicate the
systems with a large number of nodes and sensors. combined information is the same as communicating a single
Keywords: Out of sequence measurement (OOSM), target observation. This has significant benefit especially when there
tracking, delayed state, information form. are a very large number of sensors simultaneously observing
a target.
I. I NTRODUCTION
Section II describes how the delayed state approach provides
Combining information from multiple sensors generally an exact solution to the OOSM problem. Section III discusses
results in a more accurate estimate than using a single sensor. issues related to applying this technique to a real world
It is also possible to use several smaller, low cost sensors problem. This section describes how using the delayed state
to produce an estimate which is as accurate as an estimate approach results in a sparse banded information matrix, which
produced using a single more sophisticated sensor. Continual is an essential property for efficient computation, storage and
advancements in hardware, and economies of scale, make it communication. Finally, section IV summarises the delayed
feasible to deploy very large scale sensor networks. state approach and its applicability to multisensor target track-
In any system consisting of sensors connected via a complex ing.
communication network, the arrival of observations and infor-
mation at the fusion process will be delayed, and potentially II. D ELAYED S TATE A PPROACH TO OOSM P ROBLEM
out of order. This problem is known in the literature as
the out-of-sequence measurement (OOSM) problem. When A. Problem Formulation
performing an update using a delayed measurement, the cor- Consider a system for tracking a single target using multiple
relation between the late measurement and the filter over the stationary sensors and a single processing centre. The system
delayed period must be accounted for in order to provide could be described by a linear state transition equation of the
an exact solution. Existing solutions in the literature store form,
some form of filter history, and use a modified observation
x(k) = F (k)x(k) + G(k)w(k), (1)
update equation in order to explicitly account for the time
delay and correlations that arise in the process noise and where x(k) is the state vector, w(k) is a random variable
current state [1][4]. Our delayed state approach is similar describing uncertainty in the evolution of the state, F (k) is
to [5], in that both approaches involve estimating the joint the system model matrix, and G(k) is the noise matrix.
probability density of current and past target states, but our Each sensor makes independent observations according to
approach differs by its use of the information form of the the observation model,
Kalman filter, which results in significant benefits in terms
of simplicity and efficiency. The technique of using delayed z(k) = H(k)x(k) + v(k), (2)
states, and using a sparse information-form representation was
introduced by recent research in simultaneous localisation and where z(k) is the observation vector, v(k) is a random variable
mapping (SLAM) [6], [7]. describing uncertainty in the observation, and H(k) is the
Combining information from a large number sensors is a observation model matrix. It is assumed that the random
sequences w(k) and v(k) describing process and observation  
noise are Gaussian, temporally uncorrelated and zero-mean, Y Yk
Y = (10)
Yk Ykk
E{w(k)} = E{v(k)} = 0, k, (3)
At time-step k + 1, the state vector is augmented with state
with known covariance, xk+1 ,
E{w(i)w(j)T } = ij Q(i), E{v(i)v(j)T } = ij R(i). (4) x
x = xk , (11)
The sensors send raw observations to the processing centre xk+1
over a wireless connection. For example, the sensors consist
of stereo cameras, observing position, and radars, observing then the information form estimate is augmented as,
velocity. Due to the intermittency and latency of a wireless
y
connection (or any standard network connection), the mea-
yaug = yk F T Q1 , and (12)
surements will arrive delayed, and potentially in the wrong
Q1 w
order.
B. Algorithm and Operations
Y Yk 0

1) Information Form: In standard moment form, a Gaussian Yaug T
= Yk Ykk + F T Q1 F F T Q1 . (13)
distribution is parameterised by a mean vector, x, and covari- 0 Q1 F Q1
ance matrix, P ; in information-form, a Gaussian distribution
is parameterised in terms of an information vector, y, and 3) Update: The update operation for the delayed state
information matrix, Y , where approach is equivalent to the update operation in traditional
estimation approaches. They key property of the delayed state
Y = P 1 and y = Y x. (5) approach is that an update can be performed on any past state
2) Prediction: The prediction stage involves propagating in the state vector, and not just the current state, as is the case
the estimate of the current state, x(k|k), using the system with traditional approaches. Since the relationship between all
model matrix, F (k), to form an estimate of the next state, states is explicitly represented, updating any state will affect
x(k + 1|k). The uncertainty of this prediction, P (k + 1|k), the estimate of all states, in an optimal manner. Given the
is determined by the uncertainty of the estimate of the previ- following state vector,
ous state, P (k|k), and the uncertainty of the system model,
 
x
Q(k). In standard estimation approaches, the prediction stage x= , (14)
x
involves directly propagating the estimate of the current state.
For the Kalman filter, the mean and covariance is propagated the information form update equations are,
to form the estimate of the next state,
 
y
y = (15)
x(k + 1|k) = F (k)x(k), and (6) y + H T R1 z
 
P (k + 1|k) = F (k)P (k|k)F (k)0 + Q(k). (7) Y =
Y Y
(16)
T
Y Y + H T R1 H
The prediction stages for the information filter and Kalman
filter are conceptually equivalent. Once the prediction of the In information form, measurement updates are constant-time
next state is computed, the estimate of the current state is not and additive. Note that the choice of x is arbitrary. For
used for any future calculations. a delayed measurement, the term x will simply be the
With the delayed state approach, rather than simply prop- corresponding state estimate. The order of updates is also
agating the estimate of the current state, the relationship irrelevant. The properties of the delayed state update operation
between the current state and next state, as given by F (k) forms the key to providing a solution to the OOSM problem.
and Q(k), is explicitly represented. Explicitly representing 4) Marginalisation: In real world problems, it is possible
the relationship between successive states is achieved by that a estimate could continue for hours or even days - which
augmenting the state vector each time-step with the new state means that continually growing the state vector is not feasible.
estimate. The size of the state vector can bounded by marginalising
At time-step k, the state vector is given by, delayed states that are no longer required. A delayed state
  is no longer required when all observations relating to that
x
x= , (8) state have already been fused, which can be determined
xk
by communicating extra book-keeping information between
where x is the set of states x0 , x1 , ..., xk1 . The correspond- nodes or by using a heuristic, such as maximum network
ing information vector and information matrix, propagation time. For an information-form Gaussian,
   
y y
y = (9) y = (17)
yk y
   observing velocity. The state transition model and observation
Y Y model for this problem are identical to those given in section
Y = (18)
Y Y II-A.
The marginal estimate of the state x is obtained as follows, The two most significant issues for such a scenario is how to
communicate all necessary data from the remote sensors, and
1 then how to process all that data in a computationally efficient
y = y Y Y y (19)
manner. The following section discusses why a decentralised
1
Y = Y Y Y Y . (20) architecture, with an ad hoc communication network is the
most suitable approach.
C. Algorithm Summary
Consider the simple multisensor tracking system introduced B. Efficiency Through Sparsity and Information Form Proper-
in section II-A. The system consists of a single fusion centre, ties
one stereo camera observing position, and one radar observing 1) Sparsity: An important property of the delayed state
velocity. The fusion centre and the sensors are all synchro- approach, is that the information matrix is a sparse banded
nised. matrix for target tracking applications. For the information
The fusion centre at each time step augments the state vector matrix, an entry Yij , will be non zero if two states xi and xj ,
with a prediction of the next state. The fusion centre receives are correlated, when conditioned on every other state. Concep-
raw observations from the sensors over a wireless network tually, the information matrix contains an off diagonal entry
connection, and performs an observation update operation. The for each direct correlation between states. As a consequence of
observation can have an arbitrary delay, since an observation the Markov property, a particular trajectory state, xk , is only
update can be applied to any state in the state vector. directly correlated to the previous state, xk1 , and the next
The fusion centre will marginalise out a state, xk , when all state, xk+1 - which results in a sparse banded information
observations up to time tk , have been received from both the matrix.
stereo camera and radar. Since the sensors may not make an It is clear from equations 13 and 16, that both the state
observation every time step, extra information is required by augmentation and observation update operations maintain the
the fusion centre to determine that all observations up to a sparse banded structure of the information matrix.
particular time have been received. This extra information is When marginalising out a state, xk , new entries must be
communicated by the sensor by appending a start timestamp, appended to the information matrix in order to represent
ts , and an end timestamp, te , to each packet sent by a direct correlations between states which previously were only
sensor to the fusion centre. The sensor must include every directly correlated to xk , but not each other. For the simple
observation between ts and te in that particular packet. The target tracking case, the only states directly correlated to
fusion centre must also keep a record of this extra information. xk , are xk1 and xk+1 . This means that, the block entries
For example, the fusion centre receives a packet from the representing xk1 and xk+1 are simply updated, and the
camera, pc = {ts = 1, te = 8, z3 , z7 }, and from the radar, block entry representing xk can be removed. Marginalisation
pr = {ts = 1, te = 6, z2 , z3 }; the fusion centre performs an also maintains the sparse banded structure of the information
update operation for each of the four observations received; matrix.
the fusion records that all observations up to t = 8 from the The following sections show how a sparse information ma-
camera, and up to t = 6, from the radar, have been received; trix allows efficient storage, computation and communication.
the states {x0 , x1 , ..., x6 } can be marginalised out since there 2) Storage Efficiency: Representing the information matrix
are no further observations to be received for these states; the in sparse format, results in a storage requirement linearly
book keeping information corresponding to these states can be proportional to the number of delayed states retained. The
deleted. number of delayed states required is dependent on the ap-
III. P RACTICAL I SSUES plication, since it is related to the maximum propagation time
of data through the network. For example, consider a network
A. Complex Problem Formulation topology where each node is connected to its three closest
The previous section used a simple multisensor target track- neighbours. This topology can be modelled as two balanced
ing problem in order to demonstrate the applicability of the binary trees connected at common leaf nodes. Now assume,
delayed state approach to the OOSM problem. In this section a at each time step a node forwards data to its neighbours.
more complex example will be introduced in order to illustrate The propagation delay for sending data from one side of
the practical issues that arise when deploying a target tracking the network to the other is 2 lg( N3+2 ) time steps, where N
system with a large number of sensors, and how the delayed is the number of nodes in the network. For a 100 node
state approach facilitates techniques to overcome these issues. network, the propagation delay is 11 time steps - which means
Consider a system consisting of a hundred stationary sensors that 12 delayed states must be retained in order to fuse all
tracking a vehicle in a large field with a perimeter of several delayed observations. In real world systems the reliability of
kilometres. These sensors include stereo cameras, laser range network connections must also be considered when developing
finders and microphone arrays, observing position, and radars a heuristic for network propagation delay.
 3) Computational Efficiency: State augmentation, observa- from the 100 sensors would need to be fused at one location.
tion update, and marginalisation are constant time operations The delayed state approach enables 10 sub-processing nodes
with respect to the number of delayed states. to be used, each processing measurements from 10 sensors and
The state augmentation operation, equations 12 and 13, only forming a cumulative, i(k) and I(k). A single central node (or
modifies entries in the information vector, yaug , and infor- multiple nodes in a decentralised scenario), will receive the
mation matrix, Yaug corresponding to the previous state, xk , cumulative i(k) and I(k) from the 10 subprocessing nodes
and the newly augmented state, xk+1 . The entries, y in the and form the complete estimate. This approach is of benefit
information vector, and Y in the information matrix, remain for systems with many synchronised sensors.
unchanged; these terms correspond to all states other than xk 3) Communication: Generally, wireless communication is
and xk+1 . The state augmentation operation is an efficient used for large scale sensor networks since it is cost effective
operation, since its time complexity is only proportional to and much more flexible, especially when sensors are not
the size of a single state, and not the number of the delayed closely located. When using wireless communication, min-
states. imising bandwidth requirements is a significant issue. For
For an information form update (equations 15 and 16), large networks, a decentralised architecture is favourable due
entries in the information vector and information matrix that to improved reliability and survivability. For decentralised
correspond to the state for which the observation update is architectures, communication is a more significant issue since
being applied are modified. The update operation is also an data must be sent to each fusion node.
efficient operation since its time complexity is only propor- The delayed state approach allows the minimisation of
tional to the size of a single state, and not the number of communication requirements due the sparse structure, and
delayed states. because observation updates are additive in information-form.
Marginalisation is not an efficient operation (equations 17 For an information form update, equations 23 and 24, the
and 18) since it involves inverting the matrix Y which sizes of i(k) and I(k) are constant in relation to the number of
corresponds to all states which are being marginalised out, x . observation which have been combined. Once again consider
Generally the number of states being marginalised out, will be the complex problem discussed in the previous section. This
much less than the size of the information matrix. The sparse time assume a decentralised architecture with 10 fusion cen-
banded structure of Y also allows efficient sparse inversion tres, which are each directly connected to 10 sensors. If each
techniques to be used, resulting in a time complexity for the fusion centre simply forwards measurements to each of the
inversion of O(n2 ). other fusion centres, the communication requirement would
be proportional to the number of sensors. However, if each
C. Distributed Processing Through Additive Update and In-
fusion centre combines measurements from each of its sensors,
formation Form Properties
and communicates i(k) and I(k), then the communication
1) Additive Update: In information-form observation up- requirement is simply proportional to the number of nodes.
dates are additive. The update operation in information-form For a sensor network consisting of a large number of nodes,
can be written as, the difference in communication requirements is considerable.
y(k|k) = y(k|k 1) + i(k), (21) Even if the system is not fully decentralised, communication
requirements can be reduced by having low power sub-nodes
Y (k|k) = Y (k|k 1) + I(k), (22) that combine measurements from multiple sensors and send
where, i(k) and I(k) to a central node where the full estimate is
formed.
N N
X X When combining information from only a small number of
i(k) = in (k) = HnT (k)Rn1 (k)zn (k), and (23) nodes, sending complete i(k) and I(k), may be less effective
n=1 n=1
than sending measurement vectors, z(k). Generally the obser-
N N
X X vation model matrix, H, and the observation noise covariance
I(k) = Ii (k) = HiT (k)Ri1 (k)Hi (k). (24) matrix, R, are diagonal matrices. For diagonal H, the number
i=1 i=1
of non-zero entries is equal to the size of the corresponding
The sizes of i(k) and I(k) are constant in relation to the measurement vector, z. For observation models of this form,
number of observation which have been combined. This is the number of non-zero entries contained in i(k) and I(k)
an important result for minimising communication overhead are exactly equal the size of the measurement vector. Sending
and facilitating distributed processing of sensor data. only the non-zero values of i(k) and i(k), results a reduction
2) Distributed Processing: The additive update property in communication from O((dimension of state vector)2 ) to
of the information form, described in the previous section, O(dimension of measurement vector).
allows the computational load of the estimation process to be
distributed. For systems that include a large number of sensors, IV. C ONCLUSION
the computational load of performing the update operation be- This paper introduces the delayed state approach to multi-
comes significant. Consider the scenario introducted in section sensor target tracking. Delayed states capture statistical depen-
III-A, using a standard centralised approach, all measurements dence with past estimates and between multiple nodes. The
ability to retain historical correlations results in an elegant
solution to the OOSM problem. The additive update property
of the information filter, and the ability to represent the
estimate using a sparse information matrix, results in a solution
that is efficient in terms of computation and communication.
R EFERENCES
[1] Y. Bar-Shalom, Update with out-of-sequence measurements in tracking:
exact solution, Aerospace and Electronic Systems, IEEE Transactions
on, vol. 38, no. 3, pp. 769777, 2002.
[2] M. Mallick, S. Coraluppi, C. Carthel, A. Inc, and M. Burlington,
Advances in asynchronous and decentralized estimation, Aerospace
Conference, 2001, IEEE Proceedings., vol. 4, 2001.
[3] E. Nettleton and H. Durrant-Whyte, Delayed and asequent data in
decentralised sensing networks, Proc. SPIE Conf, vol. 4571, pp. 255
266, 2001.
[4] K. Zhang and X. Li, Optimal update with out-of-sequence measurements
for distributed filtering, Information Fusion, 2002. Proceedings of the
Fifth International Conference on, vol. 2, 2002.
[5] S. Challa, R. Evans, and X. Wang, A Bayesian solution and its
approximations to out-of-sequence measurement problems, Information
Fusion, vol. 4, no. 3, pp. 185199, 2003.
[6] R. Eustice, H. Singh, and J. Leonard, Exactly Sparse Delayed-State
Filters, Robotics and Automation, 2005. Proceedings of the 2005 IEEE
International Conference on, pp. 24172424, 2005.
[7] F. Dellaert and M. Kaess, Square Root SAM: Simultaneous Localization
and Mapping via Square Root Information Smoothing, The International
Journal of Robotics Research, vol. 25, no. 12, p. 1181, 2006.