650 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO.

3, MARCH 2010

Cooperative Filters and Control

for Cooperative Exploration
Fumin Zhang, Member, IEEE, and Naomi Ehrich Leonard, Fellow, IEEE

Abstract—Autonomous mobile sensor networks are employed to Mission design for a mobile sensor network requires a com-
measure large-scale environmental fields. Yet an optimal strategy bination of cooperative control and cooperative sensing. This
for mission design addressing both the cooperative motion control is because the nature and quality of collected information are
and the cooperative sensing is still an open problem. We develop
strategies for multiple sensor platforms to explore a noisy scalar coupled with the motion of the sensor platforms. Therefore,
field in the plane. Our method consists of three parts. First, we the challenges in developing successful sensing algorithms are
design provably convergent cooperative Kalman filters that apply complementary to those addressed in earlier work on distributed
to general cooperative exploration missions. Second, we present a but fixed wireless sensor networks (c.f. review articles [16],
novel method to determine the shape of the platform formation to [17]). Recent theoretical and experimental developments sug-
minimize error in the estimates and design a cooperative forma-
tion control law to asymptotically achieve the optimal formation
gest that a balance between data collection and feasible motion
shape. Third, we use the cooperative filter estimates in a provably is key to mission success [15], [18], [19]. Finding an optimal
convergent motion control law that drives the center of the plat- strategy is a challenging task.
form formation to move along level curves of the field. This control In this paper, we present a general Kalman filter design for
law can be replaced by control laws enabling other cooperative ex- mobile sensor networks to perform cooperative exploration mis-
ploration motion, such as gradient climbing, without changing the
sions. Exploration missions are frequently encountered in envi-
cooperative filters and the cooperative formation control laws. Per-
formance is demonstrated on simulated underwater platforms in ronmental applications where the mobile sensor platforms are
simulated ocean fields. commanded to measure an unknown scalar field corrupted by
(correlated) noise. Since each platform can only take one mea-
Index Terms—Adaptive Kalman filtering, cooperative control,
cooperative filtering, mobile sensing networks. surement at a time, the platforms should move in a formation or
a cluster to estimate local structures of the field.
The Kalman filter combines sensor readings from platforms
I. INTRODUCTION in a formation to provide estimates for the field value and the
gradient. A separate cooperative filter is developed to estimate
ISSIONS that require measuring and exploring a scalar the Hessian. We demonstrate that the formation shape can be
M field such as a temperature or a salinity field are encoun-
tered, for example, in ocean science and meteorology. Since the
made adaptive to minimize the error covariance of the estimate
produced by the cooperative Kalman filter. We prove a set of
scalar field is often distributed across a large area, it takes too sufficient conditions that the formation and its motion need
many sensors to obtain a snapshot of the field if the sensors are to satisfy to achieve the convergence of the Kalman filter.
installed at fixed locations. Mobile sensor networks are ideal Derivation of these sufficient conditions is based on funda-
candidates for such missions: a small number of moving sensor mental results connecting controllability and observability of a
platforms can patrol a large area, taking measurements along (time-varying) filtering system to its convergence in [20]–[22].
their motion trajectories. Exploration activities of great interest More recent developments in [23]–[25] have relaxed the con-
include climbing gradients of a scalar field [1], monitoring en- ditions for convergence of Kalman filters to stabilizability and
vironmental boundaries [2]–[5], patroling the perimeter of a re- detectability, with even weaker conditions for some special
gion or a contour [6]–[12], and providing sampling coverage cases. In this paper, we develop the sufficient conditions based
over a large area [13]–[15]. Various methods are developed and on controllability and observability conditions because the
demonstrated in the above references. resulting constraints on formation design are already mild
enough, hence are acceptable in typical applications.
Kalman filtering for mobile sensor network applications has
Manuscript received February 14, 2008; revised November 13, 2008. First received recent attention in the literature. In [26], a distributed
published January 26, 2010; current version published March 10, 2010. This Kalman filter method was proposed to decompose a high-order
work was supported in part by ONR Grants N00014-02-1-0826, N00014-02-1-
0861, N00014-04-1-0534, N00014-08-1-1007, and NSF ECCS-0841195. Rec-
central Kalman filter into “micro” filters computable by each
ommended by Associate Editor Z. Qu. sensor node. The estimates made by each node are then com-
F. Zhang is with the School of Electrical and Computer Engineering, Georgia bined using consensus filters [27]. A similar approach is taken
Institute of Technology, Savannah, GA 31407 USA (e-mail: fumin@gatech.
edu).
in [28] to address target tracking and coverage problems. An-
N. E. Leonard is with the Department of Mechanical and Aerospace other type of Kalman filter design is proposed in [29] where the
Engineering, Princeton University, Princeton, NJ 08544 USA (e-mail: entire field is partitioned into cells and the movement of agents is
naomi@princeton.edu).
Color versions of one or more of the figures in this paper are available online
controlled to maximize collected information. The above con-
at http://ieeexplore.ieee.org. tributions assume that a (dynamic) model for a planar field is
Digital Object Identifier 10.1109/TAC.2009.2039240 known to all nodes, hence each individual is able to compute a
0018-9286/$26.00 © 2010 IEEE

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 651

Kalman filter. Accordingly, the goal there is to implement a dis- field. We demonstrate the cooperative Kalman filter and the co-
tributed algorithm on many sensor nodes to improve tracking or operative control law in a simulated ocean temperature field in
mapping precision. Section VII. A summary and discussion for future directions are
For the cooperative exploration problem, on the other hand, presented in Section VIII.
the field is completely unknown; a Kalman filter can only be
computed by combining readings across platforms. The interest II. INFORMATION DYNAMICS OF COOPERATIVE EXPLORATION
here is to take advantage of the Kalman filter design in order In this section, we define the cooperative exploration problem
to use a minimum number of sensor platforms to navigate in and introduce the corresponding information dynamic model.
the unknown scalar field and reveal its structure, e.g., to follow Let where be a smooth scalar field in the plane that
level curves or gradients. In [30], an adaptive scheme using a is unknown. In most practical situations, since the field is cor-
Kalman filter is developed for interpolating data to construct a rupted by noise and the sensing devices are imperfect, it is dif-
scalar field. This contribution addresses different problems than ficult to estimate the field value using a single sensor platform.
in this paper and is complementary to our results. The key idea for mobile sensor networks is to employ multiple
In concert with our cooperative filter development, we de- moving sensor platforms to obtain the necessary estimates co-
sign provable cooperative control laws to stabilize desired for- operatively and reduce noise. This requires the platforms to be
mation shape and motion. There exist many contributions on in a formation, moving and collecting information simultane-
cooperative formation control that are closely related to graph ously.
theory, c.f. [31]–[35], to name only a few. In this paper, we em- In most applications, the sensor measurements are taken dis-
ploy a different approach based on geometric reductions. The cretely over time. This is because the spatial range of the scalar
Jacobi transform [36]–[38] is applied to decouple the motion of field is usually very large. Hence very small scale fluctuations
the formation center from the motion of the formation shape. in the field should be filtered out as noise. Let the moment when
Hence the control effort can also be decoupled into control for new measurements are available be where is an integer
the formation shape and control for the formation center. We de- index. To simplify the derivation, we do not consider the asyn-
sign the shape control and the center control separately and then chronicity in the measurements; we assume that all platforms
combine them to get the overall control. The key benefit of this
have new measurements at time . In reality, when there exists
approach is that it allows us to design formation shape control
asynchronicity, the technique we develop can still be applied
to improve performance of the Kalman filter. The center of the
with slight modifications.
formation can be controlled to perform gradient climbing, level
Let the positions of the sensor platforms at time be
curve tracking, or other motions while the Kalman filter and for-
where . We assume that the measurement
mation control remains the same. For this reason we name the
taken by the th platform is modeled as
Kalman filter associated with the formation control in this paper
the cooperative Kalman filter. (1)
In this paper we control the center of the formation to track
level curves of a scalar field corrupted by noise; this is a col- where is the value of the field at ,
laborative exploration behavior that reveals structure in the un- are i.i.d. Gaussian noise, and are spatially correlated
known field. The tracking control method is developed to steer Gaussian noise. We define the following vectors:
the center to follow its projection on a curve. This strategy was
first reported in [39], [40] and has been applied to curve tracking
for mobile robots, c.f. some recent developments in [41], [42]. A (2)
differential geometric approach was developed in [6] which ex- and assume that and are stationary, i.e., their statistics
tended the tracking method to 3-D curves. The tracking control are time invariant.
law in the present paper is a generalization of the differential Remark 2.1: By convention in ocean and atmospheric sci-
geometric results to the case of 2-D level curve tracking. The ences, modeling a physical field as a smooth field plus a
control law allows a formation to smoothly find and follow any spatially correlated random field is often desired to sepa-
desired regular level curve with proved convergence. rate larger and smaller scale phenomena. The assumptions we
The organization of this paper is as follows. In Section II, we impose here are idealizations for physical scalar fields. In addi-
derive the information dynamics of a typical platform forma- tion, the smoothness of field helps in developing Hessian
tion that moves in a planar scalar field. In Section III, Kalman filters and motion control laws in later sections.
filtering techniques are applied to the information dynamics. We We define the problem of cooperative exploration, as a spe-
establish sufficient conditions for the cooperative Kalman filter cial class of mapping problems, as follows:
to converge. We also show that the formation shape can be made Problem 2.2: Given the statistics of the noise and ,
adaptive to minimize the error covariance of the estimates pro- co-design cooperative motion and filtering that utilize collected
duced by the cooperative Kalman filter. In Section IV, we pro- measurements for mobile sensor platforms so that an esti-
vide a method to estimate the Hessian, which is necessary for the mate for the field that minimizes an error metric can be
cooperative Kalman filter. Formation shape and orientation con- obtained.
trol laws are derived based on the Jacobi transform in Section V. The choice of the error metric depends on application. In
In Section VI, a steering control law is designed to control the this paper, is chosen to be the mean square error over spatial
center of the formation to follow level curves of a planar scalar domain.

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
652 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

To solve this problem, there is no need to take measurements represent a column vector defined by rearranging the elements
at every point in the plane. Sufficient knowledge of the field of as follows:
can be gained by measuring (or estimating) the field value ,
the gradient , and Hessian at locations well distributed
across the plane and then interpolating the field. Note that this (5)
problem can be defined for a time-varying field i.e. . In
this paper, we address the time-invariant case. Then the Taylor expansions (3) for all sensor platforms near
We address Problem 2.2 by deriving rigorous tools that are can be re-written in a vector form as
particularly useful when there exist smaller regions within the (6)
global area with unknown features that require high resolution
sampling; we do not attempt to address Problem 2.2 globally in where is a 4 1 column vector obtained by rear-
this paper. Our approach includes one development that focuses ranging elements of the Hessian as defined by (5).
on minimizing the local error collectively using the cooperative Suppose that is an estimate for the Hessian
Kalman filter and coordinated motion that controls the shape of in vector form. Equation (1) can now be written as
the formation. A second development focuses on a cooperative
exploration behavior, namely formation motion control for level (7)
curve tracking. This contributes to reducing error at a some-
what larger scale than the filtering and even can contribute to where represents the error in the estimate of the Hessian.
global reduction in error if multiple formations are distributed Let , , and .
throughout the region. Central to our approach is the decoupling The noise is “colored” because it originates from the spatial
of the two developments, i.e., formation motion control can be correlation of . Let . We suppose that
designed independently from formation shape control and the , and are known once the positions of the platforms
cooperative Kalman filtering. Indeed the level curve tracking are known. This assumption is reasonable in ocean and meteo-
can be replaced or augmented with one or more other collab- rology applications since the statistical properties of ocean fields
orative exploration behaviors, such as wide-area coverage and and atmospheric fields are usually known from accumulated ob-
gradient climbing, to aid in global error minimization; further, servational data over a long period of time. We also assume that
because of the decoupling these can be implemented without af- determined by the accuracy of the Hessian estimation algo-
fecting the local error minimization. rithm is known.
The function can be locally approximated by a Taylor
series. Let be the center of the platform formation at time B. The State Dynamics
, i.e., . If is close to , then As the center of the formation moves, the states
it is sufficient to use the Taylor series up to second order. Let evolve according to the following equa-
, then tions:

(8)
(3) Let and

for . We are interested in estimates of , . We then rewrite (8) as

, and . In addition to providing insights on (9)
the structure of the scalar field, these estimates are also used in
the steering control for the center of the formation, as shown where we have introduced the noise vector which
later in Section VI. accounts for positioning errors, estimation errors for the Hes-
sians, and errors caused by higher-order terms omitted from the
Taylor expansion. We assume that are i.i.d Gaussian with
A. The Measurement Equations zero mean and known covariance matrix that is positive
definite.
Let . Let be the matrix Remark 2.3: We note that the assumption that is i.i.d
defined by Gaussian with zero mean may be unrealistic. Simulation or
physical data will help to validate the assumption.
.. .. (4) C. The Noise Dynamics
. .
The noise in the measurement (7) is colored. The standard
technique (c.f. [43]) to handle this issue is to model as
Let be the matrix with its th row vector defined by
where is the Kronecker
product. For any 2 2 matrix , we use the notation to (10)

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 653

where is white noise with positive definite correlation ma- 4) updated estimate
trix . Because
(16)

(11) 5) error covariance for the updated estimate

we have (17)

Here we use subscript to indicate predictions and to

(12) indicate updated estimates.
In order to design a Kalman filter with colored measurement
Remark 2.4: State equation (9) reveals the major difference noise , a well-known method devised in [46] can be applied
between the cooperative exploration problem considered in this by defining a new measurement as .
paper and the tracking/coverage problems considered in [26], This gives a new equation for measurements
[28], [29]. Equation (9), fundamental to the cooperative explo-
ration problem, is only valid for the formation and does not
make sense for each individual node, since and de-
pend on the location of all platforms in the formation. Therefore,
the distributed Kalman filter algorithms for tracking and cov- (18)
erage in [26], [28], and [29], which achieve consensus between
nodes and increase computational efficiency, are not applicable The equations (9), (10), and (18) are now the state and the mea-
here. The central problem here is to use the minimum number surement equations for the case when . The states are
of platforms with coordinated motion to estimate the field. For , the output is , the state noise is , and the ob-
this purpose, we design the cooperative Kalman filter in the next servation noise is
section. . The Kalman filter design procedure for this case can
be found in most textbooks and will not be repeated here.
III. THE COOPERATIVE KALMAN FILTER
B. Convergence of the Cooperative Kalman Filter
We observe from the information dynamics modeled by (9),
(10), and (7) that if the Hessian related term is known Kalman filters converge if the time-varying system dy-
for all , then the system belongs to the category for which namics are uniformly completely controllable and uniformly
Kalman filters can be constructed. In Section IV we show that completely observable [22]. In our case, these conditions are
can be estimated. Thus standard procedures can be fol- determined by the number of platforms employed, the geo-
lowed to obtain a Kalman filter, which will be called the co- metric shape of the platform formation, and the speed of each
operative Kalman filter because it can only be computed by a platform. We develop a set of constraints for these factors so
formation and its performance depends on the configuration of that the uniformly complete controllability and observability
the formation. Our main contribution in this section is to estab- conditions are satisfied, which then guarantees convergence of
lish sufficient conditions that a formation must satisfy for the the cooperative Kalman filter.
cooperative Kalman filter to converge. Let be the state transition matrix from time to
where . Then and
A. Cooperative Kalman Filter Equations . The following lemma follows from direct calcula-
The equations for Kalman filters are obtained by canonical tion.
procedures, the formulas are derived following textbooks Lemma 3.1: For as defined above and as defined
[43]–[45]. For the sake of clarity and convenience for later in (4), we have, for , and
references we list those formulas for the case when as
below:
.. .. .
1) the one-step prediction . .

(13) Remark 3.2: Note that this lemma holds for both and
. It applies to formations with any shape and any motion.
2) error covariance for the one-step prediction For clarity, we restate the definitions for uniformly complete
controllability and uniformly complete observability in [22]
(14) using notations in this paper.
Definition 3.3: The state dynamics (9) are uniformly com-
3) optimal gain pletely controllable if there exist , , and
(independent of ) such that the controllability Grammian
(15) satisfies

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
654 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

for all . Here is and the maximum eigenvalue is

the covariance matrix for state noise .
Definition 3.4: Suppose for all . The state
dynamics (9) together with the measurement equation
(7) is uniformly completely observable if there exist
, , and (independent of )
such that the observability Grammian
satisfies Since (Cd2) is satisfied and is the averaged move-
for all . Here and ment over all platforms between time and , we must have
are covariance matrices for noises and respectively. for all .
Note that in these definitions, the “ ” is a relation between It is straightforward to show that assumes its minimum
two symmetric matrices such that if and only if value when . This minimum value is
for any vector with compatible dimension. . We can see that
If , the measurement equation is (18) instead of . On the other hand, assumes its maximum value
(7). Then the observability Grammian is also when . This maximum value is
where , and . There-
and fore, we conclude that
for all . Thus
. Let and . Since and
do not depend on , we have proved the uniformly complete
(19)
controllability claim using Definition 3.3.
By the arguments for proving Lemma 3.5, we have also
The condition for uniformly complete observability is that for
proved the following lemma.
all , .
Lemma 3.6: Suppose condition (Cd2) is satisfied. Then there
In the following discussions, we derive constraints on the for-
exist constants , , and such that the state
mations so that the uniformly complete controllability and ob-
transition matrices satisfy
servability conditions are satisfied by showing that there exist
for all and for all .
positive real numbers that serve as time-inde-
To prove uniformly complete observability, we also need an
pendent bounds for various quantities. The actual value for these
elementary lemma for which we do not show the proof.
bounds do not affect the correctness of our arguments.
Lemma 3.7: Suppose two 2 1 vectors and form an
For uniformly complete controllability the following lemma
angle such that . Then the minimum eigenvalue
holds.
of the 2 2 matrix is strictly positive,
Lemma 3.5: The state dynamics (9) are uniformly completely
i.e., .
controllable if the following conditions are satisfied:
We have the following lemma regarding uniformly complete
(Cd1) The symmetric matrix is uniformly bounded,
observability of a moving formation.
i.e., for all and for some constants
Lemma 3.8: Suppose for all . The state dynamics
.
(9) with the measurement equation (7) are uniformly completely
(Cd2) The speed of each platform is uniformly bounded,
observable if (Cd2) and the following conditions are satisfied:
i.e., for all time , for ,
(Cd3) The symmetric matrices and are uniformly
and for some constant .
bounded, i.e., and
Proof: Due to condition (Cd1), the controllability Gram-
for all and for some constants
mian satisfies and
.
for any and (Cd4) The distance between each platform and the for-
such that . We first observe that is mation center is uniformly bounded from both above and
a positive semi-definite symmetric matrix for each such that below, i.e., for all , for
. If we can find uniform bounds for each of these , and for some constants .
matrices, i.e., , we obtain an overall bound for (Cd5) There exists a constant time difference and for all
the controllability Grammian. , there exist time instances where
We apply Lemma 3.1 to compute , i.e., , as well as two platforms indexed by and , such
that one of the following two conditions is satisfied:
(20) • (Cd5.1) The two vectors, and
form an angle that is uniformly bounded away from
0 or . In other words, there exists a positive constant
where we define . The minimum eigen- such that .
value of matrix (20) is • (Cd5.2) The two vectors, and
form an angle that is uniformly bounded away from
0 or . In other words, there exists a positive constant
such that .

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 655

Proof: Condition (Cd3) implies that is positive (Cd5.2), we can further decompose as the sum of two ma-
semi-definite, and condition (Cd4) implies that every com- trices: where with
ponent of is bounded above. Hence the matrix
is a positive semi-definite matrix with its maximum eigen-
value bounded above. Also from (Cd3), is a positive
definite symmetric matrix. Therefore, Weyl’s theorem (c.f. (25)
[47], Theorem 4.3.1), which states the eigenvalues of the
sum of two Hermitian matrices are bounded above by and is a positive semi-definite matrix.
the sum of the two maximum eigenvalues and bounded Because either condition (Cd5.1) or condition (Cd5.2) is sat-
below by the sum of the two minimum eigenvalues, can isfied, according to Lemma 3.7, there exists such that
be applied to . This implies that there exist the matrix . Therefore, using Weyl’s theorem (c.f.
positive constants such that [47], Theorem 4.3.1) we conclude that there exists such
where and .that . Then Lemma 3.6 guarantees the existence
Thus, one must have of such that , which further implies that
for all .
. Next, we prove the existence of positive uniform upper Because both the uniform upper and lower bounds for the
and lower bounds for for allobservability Grammian exist for all , we
. have proved the uniformly complete observability claim.
First for the upper bound, according to Lemma 3.1, we can Remark 3.9: We do not need to give formulas for all the s in
compute conditions (Cd1)–(Cd5). The values for these s will not change
the fact that the filter converges, but only affect the speed of
convergence.
We now consider the case when the colored noise .
(21) The following lemma establishes the sufficient conditions for
uniformly complete observability.
The conditions (Cd2) and (Cd4) imply that each component of Lemma 3.10: The state dynamics (9) and (10) with the mea-
the above matrix is bounded above. Hence there exists surement equation (18) are uniformly completely observable if
such that . (Cd2), (Cd4), and the following conditions are satisfied:
We now use condition (Cd5) to argue that there ex- (Cd6) The symmetric matrix is uniformly bounded i.e.
ists the lower bound such that for all and some positive
. Consider the two time constants and .
instances indexed by and as given by condition (Cd7) The matrix and the
(Cd5). It is sufficient to show that the matrix defined by matrix satisfy
and
satisfies . for some positive
Because , we have constants , , and .
where (Cd8) The constants in (Cd2) and the constants
in (Cd7) satisfy for some positive
(22) constant .
Proof: Condition (Cd6) implies that
By direct calculation one can verify that and
.
Consider . Using Lemma
(23) 3.1 we have where
Using Lemma 3.1 and the fact that , we
have .. .. . Therefore,
. .

. Applying the Hoffman-Wielandt the-

orem ([47], Theorem 7.3.8), we have

(24)

Then the matrix can be obtained by adding (23) and (24)

together. Considering the platforms and in (Cd5.1) and (26)

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
656 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

Thus using condition (Cd8), we have

Fig. 1. Symmetric arrangement of a formation of four sensor platforms. We

design a and b to minimize the mean square error when constant Kalman
(27) gain is adopted.

Therefore is uniformly
bounded below, away from singular matrices. It is also uni- D. The Cross Formation and Steady State Error Covariance
formly bounded above by conditions (Cd2), (Cd4) and (Cd7).
Hence is uniformly bounded below, away from As an example, we select a fixed coordinate frame formed by
singular matrices, and above. unit vectors and and arrange four platforms in a symmetric
With Lemmas 3.5, 3.8, and 3.10 justified, Theorem 7.4 in formation as shown in Fig. 1 so that
[22] can be applied to prove the convergence of the cooperative 1) is perpendicular to ;
Kalman filter. 2) and
Theorem 3.11: (Theorem 7.4 in [22]) Consider the ;
time-varying linear system formed by the state equation 3) the vector is aligned with and the vector
(9) and (10) with the measurement equation (18). If the system is aligned with .
is uniformly completely controllable and uniformly completely Then, in the lab coordinate frame
observable, then the Kalman filter for this system converges.
The following theorems can be viewed as corollaries of The-
orem 3.11.
Theorem 3.12: Suppose for all . Consider the state
dynamics (9) with the measurement equation (7). If the con-
ditions (Cd1)-(Cd5) are satisfied, then the cooperative Kalman (28)
filter given by (13)–(17) converges and the error covariance ma- which have very simple form because of the symmetry.
trix is bounded as . We may design the steady state formation shape so that the
Theorem 3.13: Consider the state dynamics (9) and (10) with steady state error covariance of the cooperative Kalman filter
the measurement equation (18). If the conditions (Cd1)-(Cd2), is minimized. In the case when for all , the error
(Cd4) and (Cd6)–(Cd8) are satisfied, then the cooperative covariance matrix satisfies the Riccati equation
Kalman filter for this case converges and the error covariance
matrix is bounded as .

(29)
C. Formation Design Principles

The conditions (Cd1)–(Cd8) have provided us the following The mission goal at steady state is to move the formation along
intuitive guidelines for formation design to yield successful co- a level curve. Then consider (8): as , since
operative Kalman filters. , we can replace by in (29)
1) If , there is no penalty in fixing the orientation of the as .
formation, as long as the shape is nonsingular. A singular We then determine the steady state values of and that
formation occurs when all platforms are on a straight line minimize the error covariance. Suppose and as
or collapse to a point. In fact, if the formation is singular . The following proposition holds.
only occasionally, the Kalman filter will still converge. Proposition 3.14: Suppose as , ,
2) If or a line formation is desired, then one should , and . Then converges to
make the orientation of the line change over time, such as a diagonal matrix . The trace of is
in a rocking or rolling motion.
3) The speed of the platforms needs to be bounded from both (30)
above and below to guarantee the controllability and ob-
servability conditions at the same time. Such bounds de-
pend on the strength of the error covariance matrices. where
4) In case of a correlated field, the relation between the size of
the formation and the speed of the formation should satisfy
(Cd8).

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 657

This formation can be made adaptive when the regular

Kalman gain is used. This is because by (15), since is
known, is a function of and . The resulting adaptive
(31) formation and the estimates will minimize the cost function
at each step . Therefore, we modify step (3) in Section III-A:
Proof: By direct calculation, one can verify that, as (3A) Let be a function of
and i.e.
. Find
and so that the function

(32) is minimized. Then let .

This new step is generally difficult to compute since and
As , is replaced by . Therefore, solved are matrices. But as we have discussed, using symmetric
from (29) is a diagonal matrix. By direct calculation, one can formations will greatly reduce the complexity.
verify that the trace of is given by (30).
Using the cross formation has simplified the procedure in F. Section Summary
solving the steady state Riccati equation (29). The resulting The convergence of the cooperative Kalman filter algorithms
satisfies ; a minimum of can be found when imposes constraints on feasible platform formations, and the
and assume value between . Finding analytical solu- shape of the formation affects error in the filters. In order for
tions for the optimal values and that minimize re- the cooperative Kalman filters to converge, formations should
quires solving a fourth order algebraic equation, which is best be designed to make the filter systems uniformly completely
solved using numerical methods. Since there are only two vari- controllable and uniformly completely observable. Formations
ables to optimize, the problem is rudimentary for most numeric with may need to rotate, but formations with
packages. can have fixed orientation if the formation is not co-linear. The
We can then compute , , and . In the case formations can be adaptively adjusted on-line to minimize esti-
that the noise covariance , and are time-in- mation error. Symmetric formations help to reduce complexity
variant, these computation can be performed “off-line,” in theoretical analysis, computation, and operation.
i.e., before any observations are made. Hence, a Kalman
filter gain can be given beforehand as
. IV. COOPERATIVE ESTIMATION OF THE HESSIAN
An estimate of the Hessian, , is needed to enable the
E. Adaptive Formation Kalman filter. At the end of the th time step, we have ob-
The constant Kalman gain computed beforehand is based tained an estimate from the cooperative Kalman filter.
on the assumptions on the steady state noise covariance. There- This includes an estimate for and an estimate
fore, the steady state gain may only be suboptimal if the noise for . We outline the procedure to coopera-
covariances are time varying. If the constant gain is used, tively compute as follows:
the formation shape may be adjusted to achieve more accurate 1) Start with an estimate or an initial guess .
Kalman filtering. Furthermore, if the regular Kalman filter gain 2) Use a one-step filter to reduce noise in the new measure-
is used, we show the error covariance of the estimates can ments.
be minimized by adjusting the shape of the platform forma- 3) Compute .
tion, e.g., by changing and . Our method extends the adap-
tive scheme previously developed in [1] to minimize estimation A. Cooperative One-Step Filter to Reduce Noise
error based on instantaneous measurements. Using the computed estimates and , we can
It is well known that sub-optimal filters can be derived using make predictions, before the arrival of measurements at time
. For example, when , the update equation can be step , for the field value at the positions of the sensor platforms
. A unique prop- that have moved from to as
erty of using multiple mobile sensor platforms is that we can
. The error of the prediction compared to the true
adjust and , by adjusting the geometric shape of the for-
value is Gaussian, i.e., . From properties of
mation, to minimize estimation error for this sub-optimal filter.
the Kalman filter, the covariance of is
The resulting formation is then adaptive and the resulting filter
where is the error covariance in the estimate .
is more accurate.
We then take new measurements at the th step using all plat-
Another well-known result for Kalman filter design
forms. Let be the vector of the measurements and be the
indicates that the error function to be minimized at
vector of the predictions. Let the updated measurements be
step is
. In fact, if and are
known, the gain that minimizes is exactly the Kalman
filter gain. (33)

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
658 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

Such minimizes the cost function On the other hand, we have . Taking
derivatives on both sides of this equation with respect to , we
get . This
implies that . Therefore, the estimate
for is
(34)

As we can see, serves as the weighting matrix that balances (38)

using the information from previous estimates and from current
measurements. The following proposition has been proved in The estimates and are elements of in the
our previous work [48]. Frenet-Serret coordinate system. Since the field is smooth,
Proposition 4.1: The estimator given in (33) is unbiased with we require . We also need to find out to
error covariance matrix . determine .
2) Algorithm to Estimate the Hessian: We show how to use
B. Cooperative Estimation of Hessian four sensor platforms to estimate the Hessian at . For ,
Using the cooperative Kalman filter, we may obtain a predic- the algorithm can be used directly. For , the algorithm can
tion for the as be extended by combining measurements from different time
instances.
(35) Since the procedure only involves information for step , we
drop the subscript in this section for simplicity.
Using the one-step filter we also have the filtered measurements With a formation of four moving sensor platforms, we are
. If the number of the sensor platforms and the for- able to estimate for the level curve at the center of the
mation is not co-linear, then since , one formation by the following steps:
may conjecture that the Hessian estimate can be solved, using (s.1) Compute an estimate of the field value and gradient
at the center using (35).
the least mean square method, as
(s.2) Considering the formation defined by , and ,
. However, it can be shown that for , the matrix
obtain the estimates and at the center of this
is singular, hence the least mean square method will not
three platform formation (Fig. 2) by solving the following
work.
equations for , 3, 4:
We now introduce an alternative method to estimate the Hes-
where the are given by (33)
sian that utilizes the relationship between the Hessian and the
and is the estimate of Hessian taken from previous time
curvature of level curves.
1) Curvature and Hessian: The level curve passing through step. Let , , and
the center of the formation can be parametrized by its
. Let be the 4 3 matrix with its
arc-length , hence is constant for all values of .
Suppose the gradient does not vanish along the curve.
three row vectors given by
The unit normal vector to the level curve is defined as
for , 3, 4. Then which implies
, and at any given point, the
that .
unit tangent vector to the curve, denoted by , satisfies
(s.3) Along the positive or negative direction of , we
. Then we have the following Frenet-Serret
may find the point (Fig. 2) where using
equations [49]:
.
(s.4) Estimate by solving the following equations for
(36) , 3, 4:
.
where is defined as the Frenet-Serret curvature of the level (s.5) Repeat the steps (s.2), (s.3) and (s.4) for the formation
curve. consisting of , and with appropriate changes in the
With this configuration, because along the subscripts for points and (Fig. 2).
level curve, the derivative with respect to is (s.6) Let , and denote the unit vectors along
which implies the directions of the gradient , and . Define
where is the Hessian of at , ,
. Because is the unit vector along the -axis, in the Frenet- , and . Obtain the estimate for
Serret frame we have . This sug- at as . Obtain the
gests that we can obtain , the estimate for , by estimate for according to (37).
(s.7) Approximate by
(37) . Then using (38), the estimate
is .
where is the estimate for the gradient and is (s.8) Solve
the estimate for the curvature . for where , 2, 3, 4.

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 659

Lagrange’s equations for the formation in the lab frame are

simply the set of Newton’s equations: where is the
control force for the th platform for . In terms of
the Jacobi vectors, these equations are equivalent to

(40)

where , and and are equivalent forces

satisfying

(41)

We design the control forces , , so that

as , where are desired vectors that define a
Fig. 2. Detection of a level curve using four sensor platforms. r denotes the
center of the entire formation. r denotes the center of the formation formed by constant formation. For example, when , we want
r , r and r . r denotes the center of the formation formed by r , r and r .
r and r are located on the same level curve as r . (42)

The resulting matrix can be used directly as the estimate where and are the two unit vectors defining the lab coor-
for the Hessian at the th step. Or we may repeat steps (s.1)–(s.8) dinate frame in Section III-D, and and are the optimal values
starting from to get a new estimate for the curvature and determined by methods in Section III-D to minimize the steady
then to improve the estimate . The procedure becomes an state error covariance. Since the controlled dynamics for are
iterative numerical algorithm that solves the set of nonlinear linear, the following control laws guarantee the goal (42) with an
equations that , , and satisfy given for exponential rate of convergence:
, 2, 3, 4. The prediction from step provides a reason- where are constant gains. This control law design
able initial value for this iterative algorithm. method can also by applied to stabilize the adaptive formation
obtained in Section III-E with the assumption that the optimal
formation will not change very fast over time.
V. FORMATION SHAPE AND ORIENTATION CONTROL
Comparing to existing formation control and stability results
In this section, we show that by using a powerful tool called for formation with fixed shape and orientation (for example,
the Jacobi transform, the formation shape and orientation dy- [53], [54]), this controller is much simpler and its stability is
namics can be decoupled from the dynamics of the center (or easy to prove. This is due to the fact that the reduced system is
the centroid) of the formation. Therefore, a formation shape and linear after the Jacobi transform.
orientation controller can be designed without considering the
motion of the formation center, and such a controller will not B. Formation Control With Rotation
affect the motion of the formation center. We can control the When only two sensor platforms are available, if we control
formation to have fixed orientation or to rotate according to pre- the motion of the two platform formation such that the forma-
scribed angular speed around the formation center. tion is rotating periodically, then the system will satisfy the ob-
servability condition and the Kalman filter will converge. The
A. Formation Control With Fixed Orientation difference between controller design in this section and in the
We view the entire formation as a deformable body. previous section is that the orientation of the platform forma-
The shape and orientation of this deformable body can tion will be changing.
be described using a special set of Jacobi vectors, c.f. The platform formation where there are platforms can be
[37], [38], [50]–[52] and the references therein. Here, as- described by the Jacobi vectors where .
suming that all platforms have unit mass, we define the The orientation of the collection of the Jacobi vectors in the in-
set of Jacobi vectors as , satis- ertial frame can be described by an angle which is the angle
fying where between a selected Jacobi vector and the horizontal axis of the
defines a linear coordinate transform that decouples the ki- lab fixed frame. It does not matter which Jacobi vector to select.
netic energy of the entire system, i.e., Let the matrix be . We define vectors
. We call the Jacobi
as . Let be the angular velocity of the forma-
transform. This allows us to separate motion of the center from
tion. Taking time derivatives on both sides of yields
shape and orientation changes. The transform is guaranteed
. Take time derivatives again on both sides
to exist. For example, when , the following definition of
yields
Jacobi vectors may be used
(43)

We then design and such that, for

(39) (44)

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
660 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

where is a constant vector and is a differentiable function procedure can be found in our previous works [14], [15]. Here
of . Then can be computed using (43). we briefly summarize and explain the results.
Once we have designed the combined force to con- At any time instant , there is a level curve of passing
trol the center of the formation, then the control forces , through . At this position , we let be the unit vector in
can be computed using (41). In Section VI, the direction of the gradient of the field , and let be the
we design so that the center of the formation tracks a level unit tangent vector to the level curve. By convention, and
curve. We note that our results in controller design for the form a right handed coordinate frame with pointing
platform formations pertain only to the deterministic formation to the reader. This coordinate convention is identical to the one
dynamics; this certainty equivalence approach does not produce used in Section IV-B to derive (36).
optimal controllers with input noise. For convenience, we introduce a variable such
that and . Along the trajectory of
VI. FORMATION MOTION CONTROL the center, it can be shown that
In this section, we derive the equations governing a Newto- where , ,
nian particle moving in a scalar field. Then we design tracking and represents the Hessian of the scalar field . Mean-
control laws so that a particle can be controlled to follow any while, along the trajectory of the center, the value of satisfies
non-trivial level curve. The particle is identified with the center .
of the formation so that a level curve tracking behavior is
achieved. Such level curve tracking behaviors complement gra- B. Steering Controller Design
dient climbing behaviors in cooperative exploration strategies Suppose the scalar field has extrema which are
and help reveal structure in an unknown field. allowed to be infinity. Let be the derivative function of a
function so that the following assumptions are satisfied:
A. Particle Motion in a Scalar Field
(A1) , where is a Lipschitz continuous
The center of the platform formation is modeled as a unit function on , and is continuously differ-
mass Newtonian particle with its position represented by . The entiable on ;
system equation for the particle is Newton’s equation . (A2) and if ;
This equation can be written in an equivalent Frenet-Serret form (A3) , , and
which is more convenient for the tracking purpose [14], [15]. such that .
We define the speed of the particle as and the acceleration We design the control law to be
for as

(45)
(49)
Then the equation for speed control is where is a constant control gain. We now assume that
the speed of the center is guaranteed by the speed con-
(46) troller . Then the following proposition can be proved.
Proposition 6.1: Consider a smooth scalar field with bounded
We let . As time , converges to unit Hessian and bounded gradient that satisfies except
speed exponentially with a rate determined by . for a finite number of points where or
We define a unit velocity vector as . We . Under the steering control law given in (49),
define a second unit vector as the vector perpendicular to we have and asymptotically if the initial value
that forms a right handed frame with so that and lie and .
in the plane of the page and the vector points towards Proof: Let a Lyapunov candidate function be
the reader. Then the steering control can be defined as . Then its derivative can be shown
. Using the facts that as . Therefore, if
we have . The value of the Lyapunov function does
(47) not increase. Because our initial condition is such that
and , we have the following equations: , it is impossible for at
any time instant since otherwise goes to infinity.
(48) By the invariance theorem for non-autonomous systems
([55], Theorem 4.4), we conclude that as
The equations (46) and (48) describe the particle motion in the . will not go to because we have shown that
Frenet-Serret form. Equation (47) shows the equivalence be- . This implies that as .
tween Newton’s equation and the Frenet-Serret form when On the other hand, note that .
: once the speed control and the steering control are de- The right hand side is a uniformly continuous function of time
termined, the total force can be determined. since is constant and is a smooth function with bounded
Consider the smooth scalar field in the plane. With the derivatives. Therefore, according to Barbalat’s lemma, must
speed of the particle under control, we design a steering control vanish as . This implies that either or
for the particle so that it will track a level curve of . The . When , we know . According to our

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 661

Fig. 4. Tracking, in simulation, the temperature level curve of 13 C in an esti-

mated temperature field near Monterey Bay, CA on August 13, 2003. For visu-
Fig. 3. Using two sensor platforms to track a level curve. The formation rotates alization purposes, the level curve is accentuated. The trajectory of the center of
at constant angular speed. formation is plotted with snapshots of the formation shown along the trajectory.
The horizontal axis corresponds to longitude and the vertical axis to latitude.

assumption, goes to infinity at . Thus if we start

with , we must have for all time .
Therefore, the only possibility left is which implies
that .

VII. SIMULATION RESULTS

We first demonstrate the cooperative Kalman filtering and
level curve tracking control using two sensor platforms. The
potential field is generated by two identical positive charges in
the plane with added correlated noise. It is desired that the two
sensor platforms keep a distance of 1 unit length and rotate with
a constant angular speed. Fig. 3 shows snapshots of both plat-
forms and the trajectory of the formation center when tracing a
level curve. It can be observed that the trajectory of the center of
the formation is smoother than the actual level curve with spa-
tial noise.
Fig. 5. Half distance (km) between platforms 1 and 2, i.e., the shape variable
The level curve tracking algorithm is applicable to adaptive a, versus time (hours).
sampling using a mobile sensor network in the ocean. Adap-
tive ocean sampling is a central goal of our collaborative Adap-
tive Sampling and Prediction (ASAP) project [56]. The latest Four platforms are employed to track a level curve corre-
ASAP field experiment took place in August 2006 in Monterey sponding to temperature 13 . The trajectory of the formation
Bay, California. Ten gliders were employed under continuous, center and snapshots of the formation are plotted in Fig. 4. The
automated coordinated control to collect maximally informa- center of the formation is controlled to travel at 1 km per hour.
tion-rich data for oceanographic research over the course of one The orientation of the formation is adjusted so that the line con-
month. The success of this field experiment sets a precedent for necting platforms 1 and 2 is aligned with the desired level curve.
the usefulness of the kind of cooperative exploration represented The shape of the formation is adjusted to minimize the Kalman
by level curve tracking. This motivated our second illustration filter error covariance. This can be observed from Fig. 5 where
of level set tracking with four platforms in a model ocean field. we plot the half distance between vehicles 1 and 2, i.e. the shape
In order to test our current algorithms on realistic ocean variable , versus time. Fig. 6 shows the estimates of the tem-
fields, we use a snapshot of the temperature field near Monterey perature at the center of the formation versus time. One can see
Bay produced by the Harvard Ocean Prediction System (HOPS) that the estimates center around 13 with small error.
[57]. This field reflects the temperature at 20 meters below
sea surface on 00:00 GMT August 13th, 2003. This field was VIII. CONCLUSION
produced using remotely observed and in-situ data, including We have developed a cooperative Kalman filter that combines
glider measurements during the Autonomous Ocean Sampling measurements from a small number of mobile sensor platforms
Network (AOSN) field experiment [18], [58]. to cooperatively explore a static planar scalar field. We show that

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
662 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 55, NO. 3, MARCH 2010

[2] D. Marthaler and A. L. Bertozzi, “Tracking environmental level sets

with autonomous vehicles,” in Recent Developments in Cooperative
Control and Optimization, S. Butenko, R. Murphey, and P. Pardalos,
Eds. Norwell, MA: Kluwer Academic Publishers, 2003.
[3] A. L. Bertozzi, M. Kemp, and D. Marthaler, “Determining envi-
ronmental boundaries: Asynchronous communication and physical
scales,” in Cooperative Control, A Post-Workshop Volume: 2003 Block
Island Workshop on Cooperative Control, V. Kumar, N. E. Leonard,
and A. S. Morse, Eds. New York: Springer, 2005, pp. 35–42.
[4] C. H. Hsieh, Z. Jin, D. Marthaler, B. Q. Nguyen, D. J. Tung, A. L.
Bertozzi, and R. M. Murray, “Experimental validation of an algorithm
for cooperative boundary tracking,” in Proc. Amer. Control Conf., Port-
land, OR, 2005, pp. 1078–1083.
[5] D. Zarzhitsky, D. F. Spears, and W. M. Spears, “Swarms for chem-
ical plume tracing,” in Proc. IEEE Symp. Swarm Intell., Pasadena, CA,
2005, pp. 249–256.
[6] F. Zhang, E. Justh, and P. S. Krishnaprasad, “Boundary following using
gyroscopic control,” in Proc. 43rd IEEE Conf. Decision Control, At-
lantis, Paradise Island, Bahamas, 2004, pp. 5204–5209.
[7] J. Clark and R. Fierro, “Cooperative hybrid control of robotic sensors
for perimeter detection and tracking,” in Proc. Amer. Control Conf.,
Portland, OR, Jun. 6–10, 2005, pp. 3500–3505.
Fig. 6. Estimate z ( C) versus time (hour). [8] M. A. Hsieh and V. Kumar, “Pattern generation with multiple robots,”
in Proc. IEEE Int. Conf. Robotics Autom., 2006, pp. 2442–2447.
[9] S. Susca, S. Martíinez, and F. Bullo, “Monitoring environmental
boundaries with a robotic sensor network,” in Proc. Amer. Control
the combined estimates satisfy an information dynamic model Conf., Minneapolis, MN, 2006, pp. 2072–2077.
that does not depend on motion models of the platforms. Based [10] S. B. Andersson and J. Park, “Tip steering for fast imaging in AFM,”
on this model, we have rigorously justified a set of sufficient in Proc. Amer. Control Conf., Portland, OR, Jun. 6–10, 2005, pp.
2469–2474.
conditions that guarantee the convergence of the cooperative [11] F. Zhang and N. Leonard, “Generating contour plots using multiple
Kalman filter. These sufficient conditions provide guidelines on sensor platforms,” in Proc. IEEE Symp. Swarm Intell., Pasadena, CA,
mission design. We show how to adapt the formation shape to 2005, pp. 309–314.
[12] S. Kalantar and U. Zimmer, “Control of open contour formations of
minimize error in the estimates. An algorithm has also been de- autonomous underwater vehicles,” Int. J. Adv. Robot. Syst., vol. 2, no.
signed to estimate the local Hessian, which enables the Kalman 4, pp. 309–316, 2005.
filter and provides curvature estimates for steering control. [13] J. Cortés, S. Martínez, T. Karatas, and F. Bullo, “Coverage control for
mobile sensing networks,” IEEE Trans. Robot. Autom., vol. 20, no. 2,
We take a geometric approach in formation control where re- pp. 243–255, Feb. 2004.
duction is performed on the total configuration space of the for- [14] F. Zhang and N. E. Leonard, “Coordinated patterns of unit speed parti-
mation using Jacobi vectors. The desired formation shape, ori- cles on a closed curve,” Syst. Control Lett., vol. 56, no. 6, pp. 397–407,
2007.
entation and motion can be stabilized using simple controllers [15] F. Zhang, D. M. Fratantoni, D. Paley, J. Lund, and N. E. Leonard, “Con-
with the help of the reduction method and the Jacobi transform. trol of coordinated patterns for ocean sampling,” Int. J. Control, vol. 80,
Both the filter and the formation shape controller are general no. 7, pp. 1186–1199, 2007.
[16] G. J. Pottie and W. J. Kaiser, “Wireless integrated network sensors,”
for any number of platforms and arbitrary formations as long as Commun. ACM, vol. 43, no. 5, pp. 51–58, 2000.
conditions for uniform controllability and observability are sat- [17] D. Culler, D. Estrin, and M. Srivastava, “Overview of sensor networks,”
isfied. The filter and formation shape controller are combined IEEE Comp. Mag., vol. 37, no. 8, pp. 41–49, 2004.
[18] N. E. Leonard, D. Paley, F. Lekien, R. Sepulchre, D. Fratantoni, and
with a steering control law for the center of the formation to per- R. Davis, “Collective motion, sensor networks and ocean sampling,”
form level curve tracking behavior with provable convergence. Proc. IEEE, vol. 95, no. 1, pp. 48–74, Jan. 2007.
There are open questions regarding the current results. The [19] S. Martínez and F. Bullo, “Optimal sensor placement and motion coor-
dination for target tracking,” Automatica, vol. 42, no. 4, pp. 661–668,
error covariance of and are not theoretically character- 2006.
ized and rely on heuristics and simulations, and we have only [20] R. E. Kalman, “When is a linear control system optimal?,” ASME
considered time-invariant fields in the plane. Hence, our on- Trans., J. Basic Eng., vol. 86, pp. 51–60, 1964.
[21] R. S. Bucy, “Global theory of the Riccati equation,” J. Comp. Syst. Sci.,
going work includes addressing the limitations, extending these vol. 1, no. 7, pp. 349–361, 1967.
methods to fast, time-varying scalar fields in 3-D space, and [22] A. H. Jazwinski, Stochastic Processes and Filtering Theory. New
demonstrating the methods in experiments involving robotic York: Academic Press, 1970.
[23] W. W. Hager and L. L. Horowitz, “Convergence and stability properties
mobile sensor platforms. of the discrete Riccati equation and the associated control and filtering
problems,” SIAM J. Control Optim., vol. 14, pp. 295–312, 1976.
ACKNOWLEDGMENT [24] B. D. O. Anderson and J. Moore, “Detectability and stabilizability of
time-varying discrete-time linear systems,” SIAM J. Control Optim.,
The authors wish to thank the MIT and Harvard HOPS group vol. 19, no. 1, pp. 20–32, 1981.
(P. Lermusiaux, A. Robinson, P. Hayley, W. Leslie, in partic- [25] S. W. Chan, G. C. Goodwin, and K. S. Sin, “Convergence properties
ular) for ocean modeling support, and E. Fiorelli, F. Lekien, and of the Riccati difference equation in optimal filtering of nonstabilizable
systems,” IEEE Trans. Autom. Control, vol. AC-29, no. 2, pp. 110–118,
P. Bhatta for collaborations, suggestions, and discussions. Feb. 1984.
[26] R. Olfati-Saber, “Distributed Kalman filter with embedded consensus
REFERENCES filters,” in Proc. 44th IEEE Conf. Decision Control, Seville, Spain,
[1] P. Ogren, E. Fiorelli, and N. E. Leonard, “Cooperative control of mo- 2005, pp. 8179–8184.
bile sensor networks: Adaptive gradient climbing in a distributed envi- [27] R. Olfati-Saber and J. S. Shamma, “Consensus filters for sensor net-
ronment,” IEEE Trans. Autom. Control, vol. 49, no. 8, pp. 1292–1302, works and distributed sensor fusion,” in Proc. 44th IEEE Conf. Deci-
Aug. 2004. sion Control, Seville, Spain, 2005, pp. 6698–6703.

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.
ZHANG AND LEONARD: COOPERATIVE FILTERS AND CONTROL FOR COOPERATIVE EXPLORATION 663

[28] P. Yang, R. A. Freeman, and K. M. Lynch, “Distributed cooperative [52] E. Belbruno, Capture Dynamics and Chaotic Motions in Celestial Me-
active sensing using consensus filters,” in Proc. IEEE Int. Conf. Robot. chanics. Princeton, NJ: Princeton Univ. Press, 2004.
Autom., Rome, Italy, 2007, pp. 405–410. [53] P. Ogren, M. Egerstedt, and X. Hu, “A control Lyapunov function ap-
[29] I. Hussein, “A Kalman filter-based control strategy for dynamic cov- proach to multiagent coordination,” IEEE Trans. Robot. Autom., vol.
erage control,” in Proc. Amer. Control Conf., New York, NY, 2007, pp. 18, no. 5, pp. 847–851, May 2002.
3721–3726. [54] Y. Liu and K. M. Passino, “Stability analysis of m-dimensional asyn-
[30] S. Martínez, “Distributed representation of spatial fields through an chronous swarms with a fixed communication topology,” IEEE Trans.
adaptive interpolation scheme,” in Proc. Amer. Control Conf., New Autom. Control, vol. 48, no. 1, pp. 76–95, Jan. 2003.
York, NY, 2007, pp. 2750–2755. [55] H. Khalil, Nonlinear Systems, 3rd ed. Englewood Cliffs, NJ: Prentice
[31] A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups of mo- Hall, 2001.
bile agents using nearest neighbor rules,” IEEE Trans. Autom. Control, [56] Adaptive Sampling and Prediction (ASAP) Project [Online]. Available:
vol. 48, no. 6, pp. 988–1001, Jun. 2003. http://www.princeton.edu/~dcsl/asap/
[32] Z. Lin, M. Broucke, and B. Francis, “Local control strategies for groups [57] P. Lermusiaux, C. Evangelinos, R. Tian, P. Haley, J. McCarthy, N. Pa-
of mobile autonomous agents,” IEEE Trans. Autom. Control, vol. 49, trikalakis, A. Robinson, and H. Schmidt, “Adaptive coupled physical
no. 4, pp. 622–629, Apr. 2004. and biogeochemical ocean predictions: A conceptual basis,” in Proc.
[33] R. Olfati-Saber and R. M. Murray, “Consensus problems in networks of Computat. Sci. (ICCS’04), Part 3, 2004, vol. 3038, pp. 685–692.
agents with switching topology and time-delays,” IEEE Trans. Autom. [58] S. Ramp, R. E. Davis, N. E. Leonard, I. Shulman, Y. Chao, A. R.
Control, vol. 49, no. 9, pp. 1520–1533, Sep. 2004. Robinson, J. Marsden, P. F. J. Lermusiaux, D. M. Fratantoni, J. D.
[34] L. Moreau, “Stability of multiagent systems with time-dependent com- Paduan, F. P. Chavez, F. L. Bahr, S. Liang, W. Leslie, and Z. Li,
munication links,” IEEE Trans. Autom. Control, vol. 50, no. 2, pp. “Preparing to predict: The Second Autonomous Ocean Sampling
169–182, Feb. 2005. Network (AOSN-II) experiment in Monterey Bay,” Deep-Sea Res. II,
[35] W. Ren and R. W. Beard, “Consensus seeking in multi-agent systems vol. 56, pp. 68–86, 2009.
under dynamically changing interaction topologies,” IEEE Trans.
Autom. Control, vol. 50, no. 5, pp. 655–661, May 2005.
[36] C. G. J. Jacobi, Vorlesungen über Dynamik. Berlin, Germany:
Reimer, 1866.
[37] R. Abraham and J. Marsden, Foundations of Mechanics, 2nd ed. Fumin Zhang (M’04) received the B.S. and M.S. de-
Reading, MA: Addison-Wesley, 1978. grees from Tsinghua University, Beijing, China, in
[38] F. Zhang, M. Goldgeier, and P. S. Krishnaprasad, “Control of small 1995 and 1998, respectively, and the Ph.D. degree
formations using shape coordinates,” in Proc. IEEE Int. Conf. Robot. from the Department of Electrical and Computer En-
Autom., Taipei, Taiwan, 2003, pp. 2510–2515. gineering, University of Maryland, College Park, in
[39] A. Micaelli and C. Samson, “Trajectory Tracking for Unicycle-Type 2004.
and Two-Steering-Wheels Mobile Robots,” INRIA Rep. 2097, 1993. He has been an Assistant Professor in the School
[40] C. Samson, “Control of chained systems: Application to path-following of ECE, Georgia Institute of Technology (Georgia
and time-varying point-stabilization of mobile robots,” IEEE Trans. Tech), Atlanta, since 2007. He was a Lecturer and
Autom. Control, vol. 40, no. 1, pp. 64–77, Jan. 1995. Postdoctoral Research Associate in the Mechanical
[41] K. Li and J. Baillieul, “Data-rate requirements for nonlinear feedback and Aerospace Engineering Department, Princeton
control,” in Proc. 6th IFAC Symp. Nonlin. Control Syst. (NOLCOS), University, Princeton, NJ, from 2004 to 2006. He worked for the Institute
Stuttgart, Germany, 2004, pp. 1277–1282. for Systems Research, University of Maryland. He founded the research
[42] A. Balluchi, A. Bicchi, and P. Soueres, “Path-following with a and teaching program in the fields of robotics and control at Georgia Tech
bounded-curvature vehicle: A hybrid control approach,” Int. J. Con- Savannah Campus. His major research focus includes design and control of
trol, vol. 78, no. 15, pp. 1228–1247, 2005. underwater robots and mobile sensor networks, battery modeling and control,
[43] R. Stengel, Optimal Control and Estimation. New York: Dover, 1994. and theoretical foundations for cyber-physical systems.
[44] B. O. Anderson and J. B. Moore, Optimal Filtering. Englewood
Cliffs, NJ: Prentice-Hall, 1979.
[45] D. Simon, Optimal State Estimation. New York: Wiley, 2006.
[46] A. E. Bryson, Jr. and L. J. Henrickson, “Estimation using sampled data Naomi Ehrich Leonard (F’07) received the B.S.E.
containing sequentially correlated noise,” AIAA J. Spacecraft Robot., degree in mechanical engineering from Princeton
vol. 5, no. 6, pp. 662–665, 1968. University, Princeton, NJ, in 1985, and the M.S.
[47] R. A. Horn and C. R. Johnson, Matrix Analysis. New York: Cam- and Ph.D. degrees in electrical engineering from the
bridge Univ. Press, 1985. University of Maryland, College Park, in 1991 and
[48] F. Zhang, E. Fiorelli, and N. E. Leonard, “Exploring scalar fields using 1994, respectively.
multiple sensor platforms: Tracking level curves,” in Proc. 46th IEEE From 1985 to 1989, she was an Engineer in the
Conf. Decision Control, New Orleans, LA, 2007, pp. 3579–3584. electric power industry. She is currently the Edwin
[49] R. S. Millman and G. D. Parker, Elements of Differential Geometry. S. Wilsey Professor of mechanical and aerospace
Englewood Cliffs, NJ: Prentice-Hall, 1977. engineering and an Associated Faculty Member of
[50] R. Littlejohn and M. Reinsch, “Internal or shape coordinates in the the Program in Applied and Computational Mathe-
n-body problem,” Phys. Rev. A, vol. 52, no. 3, pp. 2035–2051, 1995. matics, Princeton University. Her current research interests include nonlinear
[51] R. Littlejohn and M. Reinsch, “Gauge fields in the separation of rota- control and dynamics, cooperative control for multi-agent systems, mobile
tion and internal motions in the n-body problem,” Rev. Modern Phys., sensor networks, adaptive ocean sampling, collective behavior in fish schools
vol. 69, no. 1, pp. 213–275, 1997. and decision dynamics in mixed human/robot teams.

Authorized licensd use limted to: IE Xplore. Downlade on May 13,20 at 1:503 UTC from IE Xplore. Restricon aply.