Distributed Estimation for Motion

Coordination in an Unknown Spatially

Varying Flowfield

Cameron K. Peterson∗ and Derek A. Paley†

University of Maryland, College Park, MD, 20742, USA

I. Introduction

This note addresses the development of decentralized motion coordination algorithms in

the presence of an unknown, spatially varying flowfield. Spatiotemporal flowfields are difficult

to model and may contribute to a signification portion of the vehicle’s inertial velocity. Some

existing algorithms support operation in an unknown uniform flow, though the authors are

not aware of any previous result for cooperative control in an arbitrary, unknown flowfield

that varies smoothly in space. Summers et al. account for constant-velocity wind using

adaptive estimates to drive cooperative vehicles in a loiter circle.1 A uniform flowfield with

an added singular point was mapped by Petrich et al. to improve navigation of shallow-water

underwater autonomous vehicles using only a sparse set of GPS measurements, assuming all

measurements are shared in a centralized fashion.2 Lawrance et al. mapped out a flowfield

using Gaussian process regression for a single vehicle.3 Also for a single vehicle, Langelaan

et al. computed a 3D wind estimate and the wind acceleration using sensors typically found

on a small unmanned aerial vehicle.4

The distributed control algorithm described in this note is achieved using a combined

information and consensus filter.5,6 Lynch et al. first used an information-consensus fil-

ter to estimate spatially varying environmental fields such as temperature.7 A consensus

filter asymptotically convergences to the average of the consensus inputs for either a con-

nected undirected graph5 or a strongly connected and balanced digraph.6 Casbeer and

∗Graduate student, Department of Aerospace Engineering; cammykai@yahoo.com. AIAA Student Mem-ber.†Associate Professor, Department of Aerospace Engineering and Institute for Systems Research;

dpaley@umd.edu. AIAA Associate Fellow.

1 of 11

Beard showed that the state of a dynamical system that is estimated using an information-

consensus filter is comparable with one obtained from a centralized estimator.8 Olfati-Saber

also provided decentralized Kalman filter formulations,9,10,11,12 developed techniques appli-

cable to a heterogeneous group of sensors,9 and established the properties of stability for the

information-consensus filter.12

In this note, a distributed information-consensus filter is implemented to estimate the

coefficients of a parameterized flowfield. The inter-vehicle communication constraints may

be directed, provided they are strongly connected and balanced.6 (A strongly connected

graph ensures that a communication path that obeys edge direction may be found from

any vehicle to any other vehicle; the communication graph is balanced if, for each vehicle,

the number of incoming connections is equal to the number of outgoing connections.) The

estimated flowfield at the vehicle locations are fed into a decentralized multi-vehicle control

law that cooperatively stabilizes vehicles to a moving formation.

The contribution of this note is a distributed, observer-based control algorithm for the

stabilization of a circular formation in an estimated spatially varying flowfield using a de-

centralized information-consensus filter with noisy position measurements. It is the first

publication that the authors are aware of to consider motion coordination with distributed

estimation of a spatially varying flowfield. In the authors’ previous work,13 the vehicles op-

erated in a spatially uniform flowfield and the estimation scheme was not distributed. In

other previous work,14 the vehicles operated in a flowfield that was entirely known a priori.

The contribution of this note enables operation of multiple vehicles in an arbitrary, random

flowfield assuming only noisy position measurements. Although we present the results for

circular formations of a self-propelled particle model, we expect that the general framework

would apply to other multi-vehicle control systems.

The note proceeds as follows. Section II introduces the vehicle model and outlines an

algorithm for decentralized estimation of a scalar field. Section III proposes a control

algorithm that uses an information-consensus filter to stabilize a circular formation first in

a parametrized flowfield and then in an arbitrary, random flowfield. Section IV provides

concluding remarks.

II. Multi-Vehicle Control and Distributed Estimation

Each vehicle is modeled as a self-propelled Newtonian particle, where the position of

particle k = 1, . . . , N is denoted by rk. The particle travels in a plane at constant, unit speed

relative to an ambient flowfield fk = f(rk) and is subject to a steering control perpendicular

to the velocity eiθk of the vehicle relative to the flowfield. The particle’s inertial velocity is

thus rk = eiθk + fk.14 The steering control uk = θk is the turn rate of the orientation θk

2 of 11

of the velocity relative to the flow. In terms of the inertial speed sk = |rk| and orientation

γk = arg(rk) the equations of motion are

rk = sk(t)eiγk

γk = νk,(1)

where νk is the angular rate of change of the inertial velocity orientation of particle k.

A circular formation is obtained when the instantaneous center of rotation14 ck = rk +

ω−10 ieiγk of each vehicle’s trajectory is fixed and identical. Steering control νk = ω0sk(t)

ensures particle k will traverse a circle with a fixed center ck(0) and a constant radius

|ω0|−1 = |ck(0) − rk(0)|.14 In a circular formation, cj = ck for all pairs j and k and the

vector c = [c1, . . . , cN ]T of circle centers is in the null space of the projection matrix P =

diag{1} − (1/N)11 T , where 1 , (1, . . . , 1)T ∈ RN . Given a known flowfield, one can use

Lyapunov function S(r,γ) , 1/2〈c, Pc〉15 to show that the controla

νk = ω0(sk +K〈Pkc, eiγk〉), K > 0, (2)

stabilizes N vehicles to a fixed circular formation [13, Theorem 1]. The time derivative of S

along solutions of (1) is negative semi-definite with S = 0 occurring when (2) evaluates to

νk = ω0sk(t) and Pc = 0, i.e., the vehicles each travel around a circle, and the circle centers

are identical. (Details of the proof are omitted due to space constraints.) The use of (2)

to stabilize a circular formation in an unknown spatially-varying flowfield is illustrated in

Section III.

In this note, it is assumed that each vehicle can measure or approximate the local flowfield

at its current position. The measurements are incorporated into an information-consensus

filter to estimate the parameters of a global, spatially varying flowfield model. This note

follows Lynch et al.7 in which an information filter and a consensus filter were used to estimate

a scalar environmental field from measurements collected by multiple vehicles. The field fk

at position rk is modeled as7

fk = f(rk) =m∑m=1

n∑n=1

am,nψm,n(rk),

where ψk , ψ(rk) = [ψ1,1(rk), ψ1,2(rk), ..., ψm,n(rk)]T is the vector of l = m× n known basis

functions evaluated at rk and a = [a1,1, a1,2, ..., am,n]T is the set of unknown coefficients to

aPk denotes the kth row of P .

3 of 11

be estimated. This note adopts the following basis vectors:

ψm,n(rk) = cos(πmY

Im(rk))

cos(πnX

Re(rk)), (3)

where m = 1, . . . , m, n = 1, . . . , n, and X and Y represent the dimensions of the domain in

the complex plane.

In this note, it is assumed that the coefficients are constant, i.e., am,n = 0 for all m and

n; they are estimated using either noisy measurements of the flowfield or local approxima-

tions of the flowfield derived from noisy measurements of vehicle position. Each flowfield

measurement fk is corrupted by Gaussian, zero-mean measurement noise vk with variance

Rk ∈ C, so that7

fk = ψTka+ vk. (4)

The information filter is a variation of the Kalman filter that propagates forward the

inverse of the error covariance.16 Let a be the estimated flowfield coefficients and M =

E[(a−a)(a−a)T ] be the coefficient error covariance.b The inverse error covariance M−1 , I

is called the information matrix and i = Ia is the information measurement.7 This note

implements a discrete form of the information filter. Let t be the current time and ∆t

the time step. The superscript (−) denotes the prior estimates and (+) denotes the updated

estimates. The information filter equations are simplified under the assumption that the state

a is constant and does not have process noise. These conditions imply that the predicted

information covariance and information state at time t are equal to the prior values, i.e.,

I−(t) = I+(t−∆t) and i−(t) = i+(t−∆t). The measurement update equations for particle

k are16,7

I+k = I−k +ψkR

−1k ψ

Tk

i+k = i−k +ψkR−1k fk.

Rewriting these equations using Ck , ψkR−1k ψ

Tk and yk , ψkR

−1k fk yields7

I+k = I−k + Ck

i+k = i−k + yk.(5)

The matrix Ck and vector yk represent the information gained from particle k in a single

update measurement. The estimated coefficients ak for particle k are obtained from the

information matrix using ak = I−1k ik.

7 An advantage of using the information filter is that

measurement updates are simply added to the predicted information matrix and predicted

information measurement.8 Measurements from N vehicles can be incorporated in a single

bE[·] is the expected value of [·].

4 of 11

update step using the following sums:7

C ,N∑k=1

Ck =N∑k=1

ψkR−1k ψ

Tk (6)

and

y ,N∑k=1

yk =N∑k=1

ψkR−1k fk. (7)

The measurement-update equations that incorporate the information from all particles are

I+ = I− + C

i+ = i− + y,(8)

with the estimated coefficients a = I−1i.

A centralized information filter can be used directly to estimate a when all-to-all commu-

nication is available. When all-to-all communication is not available, the information filter

is supplemented by a consensus filter. A consensus filter approximates the average value

of an input parameter and converges to the true average as long as the (directed) vehicle

communication topology is strongly connected and balanced.6 The information-consensus

filter allows vehicle k to approximate C and y using information from its neighbor set Nk,where j ∈ Nk indicates that vehicle k receives communication from vehicle j. Let C(i,j),k

indicate the entry in the ith row and jth column of Ck. Likewise yn,k is the nth entry of

vehicle k’s measurement vector. Let τ0,k be particle k’s input to the estimated value. That is,

τ0,k = C(i,j),k where i, j = 1, . . . , l or τ0,k = yn,k where n = 1, . . . , l. The proportional-integral

(PI) consensus filter is7

τk = ξ(τ0,k − τk)−KP

∑j∈Nk

(τk − τj) +KI

∑j∈Nk

(ηk − ηj)ηk = −KI

∑j∈Nk

(τk − τj).(9)

The gain ξ > 0 determines how much the consensus filter relies upon its own input relative to

the inputs from other connected particles. τk is the consensus variable, i.e., the approximate

average of C(i,j),k or yn,k, ηk is an integrator variable, and KP and KI are the proportional

and integral gains, respectively. The sums in (9) are computed for all the particles in the

neighbor set of k.

The next section provides a distributed motion coordination algorithm that estimates a

spatially varying flowfield using (8) with inputs C and y approximated by the consensus

filter (9).

5 of 11

Nf~

.

.

.

IF

Control

Nν

2ν...

2~f

1~f 1ν

1r

2r

Nr

+

NN yC ,

22 , yC

11, yC

yC, f

(a) Centralized architecture

.

.

.

Control

Nν

2ν...

1~f 1ν

1m

2m

Nm

11, yC 11, yNCN

Nf

.

.

.

.

.

.

CF

CF

CF

IF

IF

IF

2f

1f

22 , yNCN

NN yNCN ,

22 , yC

NN yC ,

2~f

Nf~

(b) Decentralized architecture

Figure 1. Flowfield estimation and multivehicle control architectures.

III. Stabilization of Circular Formations in an Estimated

Spatially Varying Flowfield

Flowfield estimation and control may be accomplished in a centralized manner using

an information filter or in a decentralized manner using an information-consensus filter.

Figure 1 illustrates both designs. In Figure 1(a), the flowfield fk is approximated by a set

of basis functions as in (4), where the basis vector is known and the flowfield coefficients

are estimated. At each time step, vehicle k measures the local flowfield at its position rk.

Equations (6) and (7) are used to obtain C and y, which represent the information gained

from the local flowfield measurement. The centralized filter uses C and y to compute the

global flowfield estimate f . The estimate f (and its directional derivative) are fed into each

particle’s steering controller. This process is repeated at each time step and the global

flowfield estimate improves from the additional measurements.

The decentralized algorithm, depicted in Figure 1(b), uses a PI consensus filter (9) to

calculate Ck, the approximate average of matrix (6), and yk, the approximate average of

measurement vector (7). This note also relaxes the assumption that each particle measures

the local flowfield and instead requires only noisy position measurements as described next.

Let mk(t) be the change in vehicle position from t−∆t to t, subject to measurement error

gk(t):

mk(t) = rk(t) + gk(t)− rk(t−∆t)− gk(t−∆t). (10)

Using rk(t) = lim∆t→∞(rk(t)− rk(t−∆t))/∆t in (10) yields

mk(t) ≈ rk∆t+ gk(t)− gk(t−∆t)

≈ [eiθk(t) + fk(t)]∆t+ gk(t)− gk(t−∆t).

For a sufficiently small ∆t, one can assume θk is constant without loss of generality. The

6 of 11

Table 1. Decentralized Information-Consensus Filter Cooperative Control Algorithm

Input: Basis vector ψ, sensor variances Rk, circle formation radius |ω0|−1, and a stronglyconnected and balanced communication topology

At each time step t, each particle k = 1, . . . , N performs the following steps:

1: Take a noisy position measurement rk

2: Use the difference between the previous and current position measurement to approxi-mate the local flowfield measurement using (11)

3: Evaluate the basis vector at the measured position: ψk , ψ(rk)

4: For n = 1, . . . , p, where p is the number of consensus filter iterations, repeat:

4a: Use the consensus filter to estimate the components of C and y

5: Update the approximate prior information matrix I− and measurement i− using (8)and determine the estimated coefficients ak = I−1i

6: Compute the estimated flowfield fk = ψkak = ψkI−1i

7: Compute control νk using (2) with sk, c and γk replaced by their estimated values

approximate local flowfield measurement fk(t) at time t is thus

fk(t) = mk(t)∆t− eiθk(t) + gk(t)−gk(t−∆t)

∆t. (11)

Note time step ∆t must be small enough so that θk can be considered constant, but not so

small that the change in position measurement error from t − ∆ to t dominates (11). In

order to approximate a local flowfield each vehicle also needs to know the orientation θk of

its velocity relative to the flow. (The speed relative to the flow is one by assumption.)

Communicating only with vehicles in its neighbor set, each particle uses a consensus

filter to determine an approximate average of Ck and yk. These values are multiplied by

the number of particles N to approximate C and y, which are used by the information

filters to generate estimates of the flowfield coefficients. Each particle may have different

coefficient estimates due to variances in the approximate average of the covariance and

measurement matrix. The estimated flowfield is used in each vehicle’s steering control and

the process is repeated at the next time step. Table 1 and the following proposition present

the information-consensus filter algorithm.

Proposition 1. Let f(rk) =∑m

m=1

∑nn=1 am,nψm,n(rk) be a spatially varying flowfield where

the ψm,n(rk) are known basis vectors and the coefficients am,n are unknown. Using the algo-

rithm described in Table 1 forces convergence of solutions of model (1) to the set of a circular

7 of 11

formations with radius |ω0|−1 and direction of rotation determined by the sign of ω0.

Proposition 1 is justified because the estimation stage of the algorithm in Table 1 (steps

3–6) is separate from the control dynamics. The combined information-consensus filter

behaves like the centralized information filter, provided it is given time to converge to the

average. Convergence of the consensus filter is assured for strongly connected and balanced

communication topologies. Due to the stability properties of the information filter12 the

flowfield estimate will improve independently from the steering control. When the time step

is sufficiently small to ensure that the local flowfield is approximately uniform, then (11)

is an adequate replacement for the noisy flowfield measurement of Table 1. The control

becomes more accurate as the estimated flowfield converges to the true flowfield. Once the

flowfield is estimated, as discussed in Section II, steering control (2) drives all particles to a

circular formation with identical center points and radii [13, Theorem 1] .

Figure 2 illustrates the results of simulating the decentralized information-consensus algo-

rithm using noisy position measurements in flowfield (3) with l = 10 known basis functions.

Figure 2(a) depicts the vehicles (red circles), the vehicle trajectories (blue tracks), and the

inertial velocities (black arrows) at t = 500 seconds. The actual flowfield is depicted by the

gray vector field in the background. The vehicles were commanded to a circular formation

with radius |ω0|−1 = 10. Each particle approximates the local flowfield using (11) and uses

that approximation in an information-consensus filter to estimate the global flowfield. The

particles have a strongly connected and balanced topology, communicating with only four

neighbors, such that particle k receives communication directly from particles k − 2, k − 1,

k + 1 and k + 2, modulus N . We set KI = 0.05, KP = 0.5, ξ = 0.01, and sensor variance

Rk = 0.01. Figure 2(b) shows the error in each basis coefficient estimate converging to zero

for particle k = 15. The root mean square error (RMSE) of the coefficient estimates is

depicted by the black line. The error values for the decentralized consensus filter take longer

to converge than the centralized implementation, because the imperfect estimates of C and

y increase the duration of the transient.

Figure 3 illustrates the results of simulating the decentralized information-consensus al-

gorithm using noisy position measurements in an arbitrary, random flowfield modeled by

(but not necessary spanned by) l = 10 basis functions of the form (3). Figure 3(a) depicts

the particles as they are converging to a circular formation with the actual (gray arrows)

and estimated (gold arrows) flowfield as background vectors. Figure 3(c) shows the particles

at time t = 500 seconds, after they have achieved a circular formation. Figures 3(b) and

3(d) depict the percent flowfield error between the actual and estimated flowfield for particle

k = 15 at times t = 100 and t = 500 seconds. As time progresses, the estimate of the global

flowfield improves, although the estimation errors are higher toward the edges of the region

where fewer measurements were collected.

8 of 11

0 10 20 30 40 500

10

20

30

40

50

Re(r)

Im(r

)

(a) t =[0, 500] s.

0 50 100 1500

0.5

1

1.5

Time (s)

Coe

ffici

ent E

rror

(b) Flowfield coefficient errors, k = 15.

Figure 2. Stabilization of a circular formation in an estimated spatially varying flowfield usingan information-consensus filter driven by noisy position measurements.

IV. Conclusion

This note describes the design of a decentralized control algorithm for autonomous ve-

hicles that operate in an estimated spatially varying flowfield. The authors provide a dis-

tributed motion coordination algorithm that estimates a spatially varying flowfield and uses

the estimate in a closed-loop multi-vehicle control. Each vehicle implements an information-

consensus filter to reconstruct the flowfield parameters from noisy position measurements.

Simulations are provided to illustrate the performance of the estimated control algorithms in

stabilizing a circular formation in a parametrized spatially variable flowfield. This approach

is shown to be viable in an arbitrary, random flowfield.

Acknowledgments

This material is based upon work supported by the National Science Foundation under

Grant No. CMMI0928416 and the Office of Naval Research under Grant No. N00014-09-1-

1058. The authors would like to thank Sharan Majumdar, Kayo Ide, and Pat Murphy for

discussions related to this note and the comments of the reviewers.

References

1Summers, T. H., Akella, M. R., and Mears, M. J., “Coordinated Standoff Tracking of Moving Targets:

Control Laws and Information Architectures,” Journal Guidance, Control, and Dynamics, Vol. 32, No. 1,

2009, pp. 56–69.

9 of 11

0 10 20 30 40 500

10

20

30

40

50

Re(r)

Im(r

)

(a) t =[0, 100] s.

Re(r)

Im(r

)

0 10 20 30 40 500

10

20

30

40

50

0

10

20

30

40

(b) Flowfield percent errors, t = 100 s.

0 10 20 30 40 500

10

20

30

40

50

Re(r)

Im(r

)

(c) t =[0, 500] s.

Re(r)

Im(r

)

0 10 20 30 40 500

10

20

30

40

50

0

10

20

30

40

(d) Flowfield percent errors, t = 500 s.

Figure 3. Stabilization of a circular formation in an estimated spatially varying flowfield usinga decentralized information-consensus filter in a randomly generated flowfield.

2Petrich, J., Woolsey, C. A., and Stilwell, D. J., “Planar Flow Model Identification for Improved

Navigation of Small AUVs,” Ocean Engineering , Vol. 36, No. 1, Jan. 2009, pp. 119–131.

3Lawrance, N. R. J. and Sukkarieh, S., “Autonomous Exploration of a Wind Field with a Gliding

Aircraft,” Journal of Guidance, Control, and Dynamics, Vol. 34, No. 3, 2011, pp. 719–733.

4Langelaan, J. W., Alley, N., and Neidhoefer, J., “Wind Field Estimation for Small Unmanned Aerial

Vehicles,” Journal of Guidance, Control, and Dynamics, Vol. 34, No. 4, 2011, pp. 1016–1030.

5Olfati-Saber, R., Fax, J. A., and Murray, R. M., “Consensus and Cooperation in Networked Multi-

Agent Systems,” Proceedings of the IEEE , Vol. 95, No. 1, 2007, pp. 215–233.

6Ren, W., Beard, R. W., and Atkins, E. M. A., “A Survey of Consensus Problems in Multi-Agent

Coordination,” American Control Conference, 2005. Proceedings of the 2005 , IEEE, Portland, Oregon, June

2005, pp. 1859–1864.

7Lynch, K. M., Schwartz, I. B., Yang, P., and Freeman, R. A., “Decentralized Environmental Modeling

by Mobile Sensor Networks,” IEEE Transactions on Robotics, Vol. 24, No. 3, 2008, pp. 710–724.

10 of 11

8Casbeer, D. W. and Beard, R. W., “Distributed Information Filtering using Consensus Filters,” Amer-

ican Control Conference, St. Louis, Missouri, 2009, pp. 1882–1887.

9Olfati-Saber, R., “Distributed Tracking for Mobile Sensor Networks with Information-Driven Mobil-

ity,” American Control Conference, 2007, pp. 4606–4612.

10Olfati-Saber, R. and Shamma, J. S., “Consensus Filters for Sensor Networks and Distributed Sensor

Fusion,” IEEE Conference on Decision and Control , Seville, Spain, Dec. 2005, pp. 6698–6703.

11Olfati-Saber, R., “Ultrafast Consensus in Small-World Networks,” American Control Conference, Port-

land, Oregon, June 2005, pp. 2371–2378.

12Olfati-Saber, R., “Kalman-Consensus Filter: Optimality, Stability, and Performance,” IEEE Confer-

ence on Decision and Control held jointly with Chinese Control Conference, Dec. 2009, pp. 7036–7042.

13Peterson, C. K. and Paley, D. A., “Multivehicle Coordination in an Estimated Time-Varying Flow-

field,” Journal of Guidance, Control, and Dynamics, Vol. 34, No. 1, Jan. 2011, pp. 177–191.

14Paley, D. A. and Peterson, C., “Stabilization of Collective Motion in a Time-Invariant Flow Field,”

Journal of Guidance, Control, and Dynamics, Vol. 32, No. 3, 2009, pp. 771–779.

15Sepulchre, R., Paley, D. A., and Leonard, N. E., “Stabilization of Planar Collective Motion: All-to-All

Communication,” IEEE Transactions on Automatic Control , Vol. 52, No. 5, 2007, pp. 811–824.

16Simon, D., Optimal State Estimation: Kalman, H [infinity] and nonlinear approaches, John Wiley &

Sons, Inc., New Jersey, 2006.

11 of 11