To appear in Proc. ICUAS 2012

Observability-based Optimization of CoordinatedSampling Trajectories for Flowfield Estimation

Levi DeVries, Sharanya J. Majumdar, and Derek A. Paley

Abstract— Autonomous vehicles are effective environ-mental sampling platforms whose sampling performancecan be optimized by path-planning algorithms that drivevehicles to specific regions of the operational domain con-taining the most informative data. In this paper, we applytools from nonlinear observability, nonlinear control, andBayesian estimation to derive a multi-vehicle controlalgorithm that steers vehicles to an optimal samplingformation in an estimated flowfield. Sampling trajectoriesare optimized using the empirical observability gramian.We reconstruct the parameters of the flowfield fromnoisy flow measurements collected along the samplingtrajectories using a recursive Bayesian filter.

I. INTRODUCTIONAn autonomous vehicle sampling in an environ-

ment inaccessible to a manned vehicle can shedlight on physical processes in the atmosphere andocean [1],[2],[3]. However, there exists a need tooptimize vehicle sampling trajectories such thatthey provide the most informative data. For ex-ample, large-scale severe weather systems such ashurricanes span tens to hundreds of thousands ofsquare kilometers [4], of which even the longestendurance hurricane-sampling vehicle covers justa small fraction. Automatic control algorithms cansteer multiple vehicles to coordinated sampling tra-jectories that are designed for data collection. Thispaper utilizes tools from nonlinear observabilityto optimize coordinated sampling trajectories forflowfield estimation. The estimator performance is

This work is supported by the National Science Foundation underCMMI Grant Nos. CMMI0928416 and CMMI0928198 and theOffice of Naval Research under Grant No. N00014-09-1-1058.

Levi DeVries is a graduate student in the Department ofAerospace Engineering at the University of Maryland, College Park,MD 20742 lddevrie@umd.edu

Sharanya J. Majumdar is an associate professor in the RosenstielSchool of Marine and Atmospheric Science at the University ofMiami, Miami, FL 33149 smajumdar@rsmas.miami.edu

Derek A. Paley is an assistant professor in the Department ofAerospace Engineering at the University of Maryland, College Park,MD 20742 dpaley@umd.edu

improved by maximizing the observability of flow-field parameters over a candidate set of coordinatedtrajectories [5].

This work falls within the general context of(mobile) sensor placement for estimation of an un-known field [6],[7],[8]. The goal is to place a finitenumber of sensors in a possibly mobile configu-ration that maximizes an associated cost function.We use the empirical observability gramian [7], [9]to evaluate candidate sampling trajectories and im-plement a recursive Bayesian filter to estimate anunknown flowfield from noisy measurements col-lected along the trajectories. The empirical observ-ability gramian has been previously employed forsensor placement in monitoring chemical reactions[7] and for model reduction of high-dimensionalnonlinear systems [10]. Related work [6] providesplacement strategies for static sensors in order toachieve optimal coverage of a time-invariant field.In distributed parameter estimation [11], [12], and[13], one assumes the unknown field follows aknown process model such as a partial differentialequation and estimates the unknown parametersassociated with the model. An alternate strategyusing sensor platforms with time-varying com-munication is a distributed information-consensusfilter [14].

Techniques exist to estimate wind fields usingunmanned aircraft. The authors in [15] imple-mented an unscented Kalman filter to estimatewind disturbances affecting microscale unmannedaerial vehicles. In [16] the authors provide amethod for estimating wind fields for small andmini unmanned aerial vehicles. At the macroscale,the authors in [17] implemented an extendedKalman filter to provide wind shear estimates foruse in feedback control of aircraft. In [8] the au-thors used Gaussian process regression to estimatea wind field for exploration and exploitation by

gliding UAV’s.A tool well suited to evaluate the effectiveness

of candidate sensor measurements is nonlinearobservability. The authors in [9] used the nonlinearobservability rank condition [18] to evaluate theeffectiveness of Lagrangian drifter sensors for es-timating point vortex flows, whereas the authors in[19] assimilated Lagrangian drifter measurementsin an extended Kalman filter to estimate oceanflows. In this paper we calculate the empirical ob-servability gramian resulting from measurementscollected by each sampling vehicle. The empiricalobservability gramian is proportional to the Fisherinformation matrix [20], differing by only themeasurement noise covariance. The singular valuesof the observability gramian provide a measure offlowfield observability along a candidate samplingtrajectory. Observability of the flowfield is opti-mized here by maximizing the smallest singularvalue of the observability gramian over a set ofcandidate trajectories, although other metrics exist[7], [20].

We take the following technical approach todesign an observability-based sampling algorithmand apply it to sampling an idealized vortex model.We assume the vehicles collect noisy measure-ments of the flowfield and implement a recursiveBayesian filter to estimate the flowfield parameters.Using the estimated flowfield, we calculate thesmallest singular value of the empirical observ-ability gramian to optimize sampling trajectoriesover a given time interval. The optimal samplingtrajectories and the estimated flowfield are thenimplemented in a decentralized nonlinear, multi-vehicle control algorithm to steer vehicles alongoptimized sampling trajectories, which in this caseare circular formations.

The contributions of this work are (1) a frame-work to evaluate candidate sampling trajectories ina time-invariant flowfield using flowfield parameterobservability as a scoring metric; (2) a decen-tralized multi-vehicle control to steer vehicles tocircular formations that are a specified distancefrom a reference position, which constitute the setof candidate sampling trajectories; and (3) a multi-vehicle control algorithm using nonlinear observ-ability and recursive Bayesian estimation to recon-struct a time-invariant flowfield from noisy flow

measurements. The contributions are illustrated vianumerical simulations in a Rankine vortex flow.

Section II describes a self-propelled particlemodel of vehicles in a moderate flowfield; nonlin-ear observability measures; and recursive Bayesianestimation. Section III provides a decentralizedmulti-vehicle control that steers vehicles to circularformations at a specified distance from the origin.In addition, Section III applies nonlinear observ-ability measures to evaluate candidate samplingtrajectories in a parameterized flowfield. SectionIV presents a multi-vehicle control algorithm foroptimal sampling of a parameterized flowfield andprovides numerical simulations of the theoreticalresults. Section V summarizes the paper and de-scribes ongoing research.

II. CONTROL OBJECTIVE AND BACKGROUND

The general control problem we address hereis the optimization of an observer-based feedbackcontroller using observability measures as a de-sign metric. We assume there is a fleet of Nsampling vehicles collecting measurements of aknown, time-invariant flowfield characterized byM parameters. The vehicles are steered by a de-centralized feedback controller parameterized byQ control parameters. The overall system, whichmay be nonlinear, is1

z = g(z,u(z,ΩΩΩ; χχχ);ΩΩΩ)

ααα = h(z,u(z,ΩΩΩ; χχχ);ΩΩΩ),(1)

where z contains the states of all N vehicles. Thedynamic control u is a function of the vehiclestates z augmented by the estimated flowfieldparameters ΩΩΩ∈RM and parameterized by χχχ ∈RQ.The function g(·) represents the vehicle dynamicsthat are affected by the flow and therefore dependon the flowfield parameters ΩΩΩ. The outputs ααα aremeasurements of the flowfield.

When we assume vehicles collect noisy mea-surements of the flow, an observer is implementedto provide estimates of the flowfield parameters ΩΩΩ.The observer dynamics are

˙ΩΩΩ = w(z,ΩΩΩ, ααα), (2)

1The notation g(a,b;α,β ) represents a function g(·) that dependson the state variables a,b and the parameters α and β . We usebold fonts to represent either a column matrix, e.g., of vehiclepositions r = [r1 r2 ... rN ]

T , or a set of parameters, e.g., ΩΩΩ =(Ω1,Ω2, . . . ,ΩM).

θk!e1

!e2

sin θk

cos θk

!fk

!rk

φk

(a)

γk θk!e1

!e2

!fk

sk cos γk

sk sin γk!fk

(b)

Fig. 1. Model of (a) flow-relative velocity and (b) total velocity.

where ααα = ααα + ηηη are measurements corruptedby noise ηηη . In this paper we implement (2) indiscrete time using a Bayesian filter. The goal isto optimize the control parameters χχχ using theflowfield estimates ΩΩΩ such that the ΩΩΩ convergesto ΩΩΩ.

The remainder of this section reviews contribu-tions from prior work that are utilized in SectionsIII and IV. First, we review a planar model ofself-propelled particle motion in a moderate time-invariant flowfield in which the flow magnitude isstrictly less than the vehicle speed. Then we definemeasures of the observability of nonlinear systems.Finally, we describe a framework for recursiveBayesian estimation of a time-invariant flowfield.

A. Vehicle Dynamics in a Moderate Flowfield

This section reviews a two-dimensional modelof self-propelled particle motion in a moder-ate flowfield [21],[22],[23],[24],[25],[26], in whicheach particle assumes control of its turn raterelative to the flow. Control of the total-velocityturn rate is assumed by utilizing a coordinatetransformation [21] that is nonsingular as long asthe flow speed does not exceed the vehicle speed.

Consider a collection of N planar Newtonianparticles each able to control its turn rate. Thekth particle’s position with respect to a ground-fixed inertial reference frame I = (O,~e1,~e2) isrepresented by the vector ~rk = xk~e1 + yk~e2 ∈ R2

as shown in Figure 1(a). For ||~rk|| 6= 0 the polarcoordinates (rk,φk) of the kth particle’s positionare

rk =√

x2k + y2

k (3)

φk = tan−1(yk/xk). (4)

Each particle’s motion is controlled by a gyro-scopic steering force normal to its velocity ~rk =xk~e1+ yk~e2. In the absence of flow, ~rk has constantmagnitude, which we assume to be unity withoutloss of generality. The orientation of the kth parti-cle’s velocity is a point θk on the unit circle, suchthat xk = cosθk and yk = sinθk. The decentralizedsteering control θk = uk(r,θθθ ; χχχ) is designed usingstate-feedback control when the states are knownand observer-based feedback otherwise. The flow-free equations of motion of the kth particle are [22]

xk = cosθkyk = sinθkθk = uk.

(5)

Next we augment the model (5) by includinga time-invariant, spatially varying flowfield ~fk =fx,k~e1+ fy,k~e2 whose magnitude is strictly less thanone. The flowfield is parameterized by ΩΩΩ ∈ RM,i.e.,

fx,k = fx(xk,yk;ΩΩΩ) ∈ Rfy,k = fy(xk,yk;ΩΩΩ) ∈ R. (6)

For example, consider the following Rankine vor-tex model [9], for which ΩΩΩ = (rmax,vmax,µ):

fx,k =

−vmax (rk/rmax)sinφk, 0<rk<rmax−vmax (rk/rmax)

−µsinφk, rk>rmax,(7)

and

fy,k =

vmax (rk/rmax)cosφk, 0<rk<rmaxvmax (rk/rmax)

−µcosφk, rk>rmax.(8)

In the presence of flowfield ~fk, each particle’svelocity is represented by the vector sum of itsvelocity relative to the flow and the flow velocityrelative to the ground, as shown in Figure 1(b).The equations of motion of the kth particle become[21], [27]

xk = cosθk + fx,kyk = sinθk + fy,kθk = uk.

(9)

For the purpose of path planning in a flowfield,we desire control of the inertial velocity rather thanthe velocity relative to the flow. For this reason, wedefine the magnitude and orientation of the totalvelocity

sk ,√

x2k + y2

k (10)

γk , tan−1(yk/xk), (11)

such that, from Figure 1(b),

sk cosγk = cosθk + fx,k (12)sk sinγk = sinθk + fy,k. (13)

The time derivative of (12) and (13) is [21]

sk cosγk− skγk sinγk = −θk sinθk + fx,k (14)sk sinγk− skγk cosγk = θk cosθk + fy,k, (15)

where fi,k =∂ fi,k∂xk

xk +∂ fi,k∂yk

yk. Solving for sk in (14)and substituting the result into (15) to solve for γkyields [21]

γk = (1− s−1k ( fx,k cosγk + fy,k sinγk))uk

+( fy,k cosγk− fx,k sinγk), νk,(16)

where νk is the steering control of the orientationof the total velocity. From (12), (13), and (16) weachieve the equations of motion for the kth particlein a flowfield subject to steering control νk [21]:

xk = sk cosγkyk = sk sinγkγk = νk, k = 1, . . . ,N.

(17)

In a moderate flow, the transformation from νk touk (16) is nonsingular and the speed is [21]

sk = fx,k cosγk + fy,k sinγk

+√

1− ( fy,k cosγk− fx,k sinγk)2.(18)

The extension to a strong flowfield, i.e., one inwhich the flow magnitude can exceed the particlespeed, is possible [27] but outside the scope of thispaper where we assume a moderate flow.

B. Unobservability Measures

In linear systems theory, the singular values σ jof the observability gramian determine the relativeease in determining the initial states of a linear sys-tem from the outputs generated over a time interval[28, p. 125-126]. Large singular values imply thatit is easy to invert the mapping from outputs toinitial states [9]. The reciprocal of the smallestsingular value σmin of the observability gramian,called the unobservability index, is a measure ofthe relative ease in which an estimation schemecan determine the initial state of a system [9]; largevalues imply that the system is difficult to observe,

whereas small values indicate the opposite. Theunobservability index is [9]

ξ , 1/σmin. (19)

In stochastic estimation, a large value of ξ

implies that noise in the measurements will sig-nificantly impact the estimate error. Conversely, asmall value of ξ implies that the estimation errormay not be susceptible to measurement noise [9].

While the unobservability index is used to eval-uate candidate sampling trajectories in this work, itis one of many metrics providing a measure of ob-servability. For instance, the estimation conditionnumber [7], [9]

λ = σmaxσmin

= σmaxξ , (20)

reflects the degree of variability in the observabil-ity of the system. A large λ implies that a smallperturbation in one direction may have a morepronounced effect on the output than a large per-turbation in another direction, which implies thatthe observability of the system is sensitive to theperturbation direction and the estimation problemmay be ill-conditioned [9]. Other metrics of theobservability gramian include the trace, maximumsingular value, and determinant [7], [20]. In thispaper we choose the unobservability index (19)since it measures the least observable state.

Using the observability gramian on a nonlin-ear system to compute the unobservability indexand the estimation condition number requires lin-earization about an equilibrium point. However,linearization may not adequately capture the input-output behavior of the nonlinear system over adesired operating region. Moreover, nonlinear ob-servability analysis [18], [29] can be difficult toapply. One alternative for determining the ob-servability of a nonlinear system is to use theempirical observability gramian [9], also known asthe observability covariance matrix [10],[30]. Theempirical observability gramian is useful becauseit approximates the input-output behavior of a non-linear system and is equivalent to the observabilitygramian for a linear system.

The empirical observability gramian maps theinput-to-state and state-to-output behavior of anonlinear system more accurately than the observ-ability gramian found by linearization [30]. It is

defined as follows. Let εiei be a small displacementof the initial parameter along the ith unit vector ei ∈RM and let ΩΩΩ ∈RM be the initial parameter state.The (i, j)th component of the M ×M empiricalobservability gramian WO is [9]

WO(i, j) = 14εiε j

∫ T0[ααα+i(τ)−ααα−i(τ)

]T[ααα+ j(τ)−ααα− j(τ)

]dτ,

i = 1, . . . ,M, j = 1, . . . ,M,(21)

where ΩΩΩ±i = ΩΩΩ± εiei, produces the output ααα±i =

h(z,u;ΩΩΩ±i). Measures of the observability of a

nonlinear system can be obtained by applying (19)and (20) to WO.

Returning to the Rankine vortex example, as-sume each vehicle collects a (noise-free) flowmeasurement

αααk = [ fx,k, fy,k]T = ~fk ∈ R2. (22)

such that ααα = [αααT1 ,ααα

T2 , . . . ,ααα

TN ]

T ∈R2N . The equa-tions of motion in (1) with ααα given by (22) producea nonlinear input-output system.

Calculating the empirical observability gramianvia integration of (1) with g(·) given by (17)reveals the input-output observability of the flow-field parameters ΩΩΩ over a given particle trajectory.The steering control input νk dictates the samplingtrajectory and is designed to stabilize optimal sam-pling trajectories. The perturbation size is chosento be a fixed percentage of the nominal state size.For example, a 20% perturbation for rmax = 30 isε1 = 6.

C. Bayesian Estimation of a Parameterized Flow-field

We adopt a nonlinear estimation scheme to en-able each particle to estimate the unknown parame-ters of a model flowfield. These estimates are usedby each particle to compute its steering control. Inthis section we describe a recursive Bayesian filterformulation for parameter estimation.

Estimation of a spatiotemporal flowfield ~f of theform (6) using noisy observations of the flow canbe accomplished by assimilating the observationsusing a recursive Bayesian filter. For linear systemswith Gaussian noise, the optimal Bayesian filter isthe Kalman filter, whereas for nonlinear systemswith nonlinear noise models, a common Bayesianfilter is a particle filter [31]. In either case, the flow

estimate is encapsulated in a state vector, which forexample may contain the flow velocity ~f at eachone of P grid points. An alternative we pursue isa state vector ΩΩΩ that contains only a set of M Pparameters, from which the flowfield ~f can be re-constructed. For example, the model (6) is definedby the M = 3 parameters ΩΩΩ = (rmax,vmax,µ), suchthat the flowfield at~rk is ~fk(ΩΩΩ) = ~f (xk,yk;ΩΩΩ). Thisrepresentation provides a significant reduction incomputations, making it attractive for use in anoptimal sampling scheme. Note this representationis only possible for a parameterized flowfield.

The discrete-time Bayesian formalism proceedsas follows [31]. As in (2), let ΩΩΩ(t) denote thestate estimate at time t, αααk(t) denote the kth

vehicle’s noisy observation at time t, and Ak(t) =αααk(1), . . . , αααk(t) denote the set of observationsup to time t. The posterior probability of the stateΩΩΩ(t) given Ak(t) is

p(ΩΩΩ(t)|Ak(t)) = β p(αααk(t)|ΩΩΩ(t))p(ΩΩΩ(t)|Ak(t−∆t)),

(23)

where the coefficient β is chosen so thatp(ΩΩΩ(t)|Ak(t)) has unit integral over the statespace. The conditional probability p(αααk(t)|ΩΩΩ(t))is a likelihood function that represents the prob-ability that the state ΩΩΩ(t) generated the observa-tion αααk(t). Note that p(ΩΩΩ(0)|Ak(0)) is the priorprobability, which we assume to be uniform inthe absence of any information other than theparameter lower and upper bounds.

Returning to the Rankine vortex example, sup-pose the kth particle obtains the following noisymeasurement of the flow at time t and location rk:

αααk(t) = [ fx,k, fy,k]T +[ηx,k(t),ηy,k(t)]T ∈ R2,

where the noise ηi,k(t) ∼ N (0,σ2i ) is normally

distributed with zero mean and variance σ2i for

i = x,y. For each point ΩΩΩ(t) in the M-dimensionalstate space, we choose the likelihood function tobe a multi-variate Gaussian, i.e.,

p(αααk(t)|ΩΩΩ(t)) = 12π

exp[−12 [~fk(ΩΩΩ(t))− αααk(t)]T

Σ−1[~fk(ΩΩΩ(t))− αααk(t)]],(24)

where Σ = diag(σ2x ,σ

2y ).

Assuming measurements are taken at each timestep, the conditional probability density of the state

ΩΩΩ(t) is updated with the equations of motion ofthe kth particle in an Euler integration scheme suchthat the dynamics in (2) take the discrete form

p(ΩΩΩ(t)|A(t))=β

(N

∏k=1

p(αααk(t)|ΩΩΩ(t))

)p(ΩΩΩ(t)|A(t−∆t)),

(25)

where p(αααk(t)|ΩΩΩ(t)) is given by (24) andp(ΩΩΩ(t)|AAA(t−∆t)) is the prior probability. We takethe point in parameter space corresponding tothe maximum of the posterior probability densityp(ΩΩΩ(t)|A(t)) to be the best estimate of the flow-field parameters ΩΩΩ. Note that (25) assumes thatthe kth particle communicates its measurement toeither a central hub or to every other particle suchthat all vehicles have knowledge of p(ΩΩΩ(t)|A(t)).Distributed versions of (25) are possible [14], butbeyond the scope of this paper.

III. DECENTRALIZED MULTI-VEHICLECONTROL AND OBSERVABILITY-BASED

SAMPLING

In this section we derive controllers to stabilizemulti-vehicle sampling trajectories that optimizethe observability of a parameterized flowfield.Sampling trajectories are chosen from a familyof sampling formations parameterized by χχχ . Forexample, the family of circular sampling patternsis parameterized by the position of the circle’scenter and its radius. For flowfields with azimuthalsymmetry such as a Rankine vortex model (7)–(8), the azimuth of the circular pattern’s centeris irrelevant and the family of circular formationsis parameterized by the distance ρ of the centerfrom the origin and the radius of the circle |ω0|−1.This section presents a decentralized multi-vehiclecontrol algorithm to steer vehicles to circular for-mations parameterized by χχχ = (ρ, |ω0|−1). Theempirical observability gramian is used to evaluatethe family of candidate sampling trajectories andan observability-based algorithm is presented tosteer vehicles to optimal sampling formations.

A. Stabilization of Circular Formations with Arbi-trary Azimuth

This subsection uses Lyapunov-based controltechniques [32] to derive a decentralized con-trol algorithm that steers vehicles to a circular

formation parameterized by χχχ = (ρ, |ω0|−1). Thecontrol laws are derived assuming the particlestravel through a known, time-invariant flowfield.We relax the known-flowfield assumption in Sec-tion IV, using instead flowfield estimates providedby a recursive Bayesian filter.

A particle traveling in a circle rotates about afixed point in space. Let the instantaneous circlecenter [21], [22] be defined by

~ck =~rk +(ω0sk)−1[−yk, xk]

T = [cx,k,cy,k]T ∈ R2,

(26)where |ω0|−1 is the radius of the circle. From thetime derivative of (26),

~ck =~rk(1− (ω0sk)−1νk), (27)

we see that ~ck = 0 for the control νk = ω0sk,which steers the kth particle about a fixed center~ck =~ck(0). The following control law forces thekth particle to converge to a circular trajectory inwhich the steady-state value ||~ck||= ρ is specified.

Theorem 1: The particle model (17) with con-trol

νk = ω0sk[1+K(~rTk~rk)(1−ρ||~ck||−1)], K > 0,

(28)stabilizes the set of circular trajectories with radius|ω0|−1 centered at distance ρ from the origin ofreference frame I .

Proof: Consider the potential function

V =N

∑k=1

12(||~ck||−ρ)2, (29)

where ||~ck|| =√~cT

k~ck is the distance of circlecenter k from origin O. The time derivative of (29)along solutions to the equations of motion (17) is

V =N

∑k=1

(1−ρ||~ck||−1)(~rTk~rk)(1− (ω0sk)

−1νk).

(30)Substituting the control (28) into (30) gives

V = −KN

∑k=1

[~rTk~rk(1−ρ||~ck||−1)]2 ≤ 0. (31)

Equation (31) implies that V is negative semi-definite with V = 0 occurring when ||~ck|| = ρ or~rT

k~rk = 0 for all k, which occurs when ||~ck|| =0 since ~rk 6= 0. The largest invariant set Λ for

−60 −40 −20 0 20 40 60−60

−40

−20

0

20

40

60

x

y

(a)

−60 −40 −20 0 20 40 60−60

−40

−20

0

20

40

60

x

y

(b)

Fig. 2. Simulation of N = 6 particles in a Rankine vortex with (a)control algorithm (28) and (b) steering control algorithm (33).

which V = 0 contains circular trajectories with||~ck|| = ρ or ||~ck|| = 0. The invariance principle[32] stipulates that particle k converges to Λ.

Figure 2(a) shows a simulation of the controllaw (28) with N = 6 particles in a Rankine vortexdescribed by (7)–(8) with rmax = 30, vmax = 0.7,and µ = 0.6. Note that each particle converges toa circular trajectory of radius |ω0|−1 = 10 whosecenter lies on the dashed line corresponding to ρ =30. The control gain is K = 0.1.

To guarantee particle convergence to a singlecircular formation whose center is a desired dis-tance from the origin, we utilize a composite po-tential. Let ~c = [~c1,~c2, . . . ,~cN ]

T ∈ RN×2. Considerthe potential

V =12

N

∑k=1

[~cT

k (Pk~c)T +ak0(||ck||−ρ)2] , (32)

where ak0 = 1 if the kth particle is informed ofρ and zero otherwise and Pk is the kth row of theN×N projection matrix P= IN×N−(1/N)11T . Wehave the following result.

Theorem 2: The particle model (17) with con-trol

νk = ω0sk(1+K[~rTk (Pk~c)T

+ak0(1−ρ||~ck||−1)~rTk~rk])

(33)

stabilizes the set of circular formations with radius|ω0|−1 whose center is ρ units from the origin.

Proof: The time derivative of (32) alongsolutions of (17) is

V =N

∑k=1

[~rT

k (Pk~c)T +ak0(1−ρ||~ck||−1)~rTk~rk]

(1− (ω0sk)−1

νk).

(34)

Substituting (33) into (34) gives V ≤ 0 since

V=−KN

∑k=1

[~rT

k (Pk~c)T+ak0(1−ρ||~ck||−1)~rTk~rk]2.

(35)Equation (35) implies that the vehicles convergeto the largest invariant set Λ for which

~rTk (Pk~c)T +ak0(1−ρ||~ck||−1)~rT

k~rk ≡ 0. (36)

Since ||~rk|| 6= 0, (36) is satisfied only when Pk~c= 0and ||~ck|| = ρ . The quantity Pk~c = 0 only when~ck = ~c j for all pairs j,k [22]. By the invarianceprinciple [32] solutions converge to Λ, which con-tains the desired set of circular formations.

Simulation of the control algorithm (33) withN = 6 particles in a Rankine vortex is shown inFigure 2(b) with |ω0|−1 = 10, K = 0.1, a1,0 =a2,0 = a3,0 = 1, and a4,0 = a5,0 = a6,0 = 0. Theparticles converge to a circular formation withdistance ρ = 30 from the origin, shown by thedashed line.

B. Observability-based Optimization of ControlParameters

In this section we present an algorithm that usesnonlinear observability to optimize the parametricinputs to a coordinated sampling formation. Inmany environmental sampling applications, vehi-cles are driven in repeating patterns to collect mea-surements within a spatiotemporal volume [33].The control algorithms driving vehicles to theseformations require a set of parameter inputs χχχ ,such as χχχ = (ρ, |ω0|−1) for the circular formationsin Section III-A. Here we use the unobservabilityindex (19) as a metric to evaluate the ability ofmembers of the formation parameter space χχχ toobserve a flowfield parameterized by ΩΩΩ.

In a known flowfield, a set of sampling param-eters χχχ produces a corresponding unobservabilityindex ξ (χχχ) calculated from (1), (19), and (21). Theoptimal trajectory is found by optimizing over thespace of sampling parameters

χχχ∗ = argmin ξ (χχχ). (37)

Since this optimization technique iterates only overthe low-dimensional sampling parameter space,rather than the space of all possible samplingtrajectories, it can be computed rapidly even byan exhaustive search.

0 10 20 30 40 50 600

0.5

1

1.5

2

2.5

3

3.5

4

4.5

Distance of Circle Center from Origin

Un

ob

se

rva

bili

ty I

nd

ex (

log

10)

(a)

−60 −40 −20 0 20 40 60−60

−40

−20

0

20

40

60

x

y

(b)

Fig. 3. (a) Unobservability index versus distance of circularsampling formation from vortex origin. (b) A sample of controlledtrajectories. The red circle centers at y = 40 and y = 60 correspondto the red data points of (a).

Figure 3 shows analysis of circular trajec-tory optimization in a Rankine vortex (7)–(8)parameterized by the flowfield parameters ΩΩΩ =(rmax,vmax,µ). The sampling parameter space χχχ =(ρ,10) is the set of circular formation centerdistances ρ from the origin, assuming circular for-mations of radius |ω0|−1 = 10. Figure 3(a) showsthe log of the unobservability index as a functionof the sampling parameter ρ . Figure 3(b) showsfour sampling trajectories corresponding to the reddata points of Figure 3(a). In each case, the initialposition of the vehicle is located on the desiredcircle to eliminate the effect of transient behav-ior on the unobservability analysis. The flowfieldparameters are ΩΩΩ = (30,0.7,0.6). rmax is shownby the dashed line in both figures. Notice thatthe unobservability is minimized by traveling ina circle with ρ∗ = 35. For ρ < 20 = rmax−|ω0|−1

there is an increase in unobservability because theflowfield parameter µ , corresponding to the decayin flowspeed outside of rmax, is unobservable. Forρ > 40 the parameters are less observable becausethe flow strength decreases exponentially outsidethe radius of maximum wind.

Figure 4 shows four pattern subsets of the sam-pling parameter space that provide significantlydifferent observability. Pattern subset #1 (PS1)contains circular trajectories that lie entirely insidermax. PS2 trajectories cross rmax. PS3 lies entirelyoutside rmax with |ω0|−1 < rmax. PS4 contains rmax.Since the Rankine vortex model is azimuthallysymmetric, the circle centers are constrained to they-axis without loss of generality.

Figure 5 shows the results of unobservabil-ity analysis over χχχ = (ρ, |ω0|−1), in which the

(PS1) (PS2) (PS3) (PS4)

Fig. 4. Pattern subsets associated with a circular samplingformation in a Rankine vortex. The dotted line represents rmax.

parameter-space boundaries of the pattern subsetsare evident. Figure 5(a) depicts example trajec-tories from within each subset as well as thesubset boundaries, which are found analytically.Figure 5(b) shows the unobservability index overthe parameter space. In both figures ρ and |ω0|−1

are normalized by rmax. Note that areas of lowunobservability correspond to trajectories crossingrmax (PS2), whereas highly unobservable trajecto-ries remain entirely inside (PS1) or outside (PS4)rmax. Trajectories in pattern subset (PS3) are lessobservable than (PS2), but more observable than(PS1) and (PS4). The minimum of the unobserv-ability index is shown by the white dot in Figure5(b), along with a scaled image of the optimaltrajectory.

IV. OBSERVABILITY-BASED SAMPLING IN ANESTIMATED FLOWFIELD

In this section we combine the results of Sec-tions II and III in an optimal sampling algorithmand provide a numerical example of vehicles sam-pling in a Rankine vortex. A recursive Bayesianfilter estimates the flowfield parameters, which areutilized to determine optimal sampling parametersover a time interval Topt . The multi-vehicle controlalgorithm (33) is implemented using the flowfieldestimate and the optimal sampling parameters,such that vehicles are steered to trajectories withoptimal observability.

The sampling algorithm proceeds as follows.First, the probability density function for the flow-field parameters ΩΩΩ is initialized either uniformlywithin known bounds or from a known prior dis-tribution. The maximum of the probability densityfunction corresponds to the initial flowfield param-eters ΩΩΩ(0) upon which the initial flowfield estimatefk = f (xk,yk;ΩΩΩ(0)) is based. The initial estimateis utilized in (22) to calculate WO(χχχ) using (21)

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

PS1

PS2

PS3

PS4

Circle Center (rmax

)

Circle

Ra

diu

s (

r ma

x)

(a)

PS1

PS2

PS3

PS4

Circle Center (rmax

)

Circle

Ra

diu

s (

r ma

x)

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

Un

ob

se

rva

bili

ty I

nd

ex

1

1.5

2

2.5

3

3.5

4

4.5

5

(b)

Fig. 5. Observability of a Rankine vortex. (a) Pattern subsets over the parameter space χχχ = (ρ, |ω0|−1) normalized by rmax. (b) Theunobservability index ξ (χχχ) with minimum denoted by the white dot.

over a time horizon Topt and sampling parametersχχχ . (Note that in this step the transient behavior ofthe vehicles as they converge to the formation χχχ

is taken into account; the initial positions of eachparticle are used to calculate (21).) The choice ofhorizon time depends on the expected accuracyof the flowfield estimate. For instance, one maychoose Topt to be relatively short initially sincethe vehicles have yet to collect measurements. Inthis work, we assume a constant horizon time ofTopt = 50 seconds.

The optimal sampling parameters χχχ∗ over Toptfound using (37) are implemented in the controlalgorithm νk = νk(xk,yk, fk; χχχ∗), where νk is givenby (33). Particles travel along the closed-loop tra-jectories, collecting noisy measurements αααk(t) andusing the flowfield estimates ~fk = f (xk,yk;ΩΩΩ(t))in the decentralized control. After time Topt haselapsed, the process is repeated on Topt intervalsto produce an updated set of optimal samplingparameters χχχ∗ from the new flowfield parametersΩΩΩ(t) until the mission completes. An overview ofthe sampling algorithm is shown in Table 1 withan associated block diagram shown in Figure 6.

We simulated the observability-based samplingalgorithm with N = 5 vehicles to estimate the flow-field parameters ΩΩΩ= (rmax,vmax,µ) of the Rankinevortex model (7)–(8). The initial vehicle positionswere clustered outside rmax and the flowfield wasassumed to have ground-truth parameters rmax =

Table 1 Observability-based Sampling Algorithm.

Inputs: probability density p(ΩΩΩ(t)|ααα(0))producing initial flowfield estimate ΩΩΩ(0);χχχ(0)∗ = argmin ξ (χχχ;ΩΩΩ(0),Topt) using observabilityanalysis and initial vehicle positions over iteration timeTopt ; Tf inal ; Topt .for t < Tf inal do

1. Generate an estimated flowfield using estimatedparameters, ~fk(t) = ~f (xk(t),yk(t);ΩΩΩ(t)).2. Use the estimated flowfield and the current optimalsampling parameters to calculate the steering controlνk(t) = ν(xxx(t),yyy(t),~fff (t); χχχ∗).3. Update the flowfield parameter estimate by assimi-lating measurements, so that ΩΩΩ(t)= sup p(ΩΩΩ(t)|A(t)).if t mod Topt = 0 then

Find the optimal sampling parameters over the ob-servability iteration time using the current particlepositions and current flowfield parameter estimate,χχχ∗ = argminξ (χχχ;ΩΩΩ(t),Topt).

end if4. Set t = t +∆t

end for

30, vmax = 0.5, and µ = 0.8. The duration of thesampling mission was 400 seconds with observ-ability iteration occurring every Topt = 50 seconds.The probability density function was initializeduniformly over the parameter space rmax ∈ [0,100],vmax ∈ [0.01,1], and µ ∈ [0,1], and the initial flow-field parameter estimate was selected randomlywithin the space. For simplicity we assumed theradius of the desired circular formation was held

Flowfield

Recursive Bayesian Filter

Multi-vehicle Control

Observability Optimization

Ω

χ∗

α

Vehicle Dynamics

α

+ η

ν

z

Ω

Fig. 6. A schematic diagram of the observability-based samplingalgorithm is shown. A recursive Bayesian filter provides flowfieldparameter estimates ΩΩΩ from noisy flow measurements ααα . Theestimated flowfield parameters are used to calculate observabilityoptimizing control parameters χχχ∗ that characterize the multi-vehiclesampling formation.

constant at |ω0|−1 = 10 so the observability analy-sis takes place over the sampling parameter spaceχχχ = (ρ,10).

Figure 7 shows a snapshot of the samplingalgorithm simulation at t = 20 seconds. Figures7(a) and 7(b) show the log of two marginalprobability densities2 over the flowfield parameterspace rmax, vmax, and µ . In each figure the blackdot corresponds to the ground truth parametersand the magenta dot corresponds to the currentestimate. Figure 7(c) shows the result of the initialunobservability analysis. The red dot correspondsto the sampling parameter with the highest ob-servability; the black and magenta dashed linescorrespond to the true and estimated rmax values,respectively. Figure 7(d) shows the position andvelocity orientation of each particle after t = 20seconds. The black and magenta dashed circlescorrespond to the ground truth and estimated rmaxvalues, respectively. The true flowfield is plottedin black while the estimated flowfield is shownin grey. The red circle corresponds to the optimalcircle center distance ρ∗.

Figure 8 shows a snapshot of the sampling mis-sion at t = 400 seconds. Figure 8(d) shows the fullparticle trajectory in light blue while the convergedformation is in dark blue. Notice that the mode ofthe posterior of the Bayesian filter has convergedto the ground-truth. The unobservability analysis inFigure 8(c) agrees with the predictions in Figure5(b) in the following sense: for fixed circle radius

2The marginal probability density is the sum of a multi-dimensional probability density function over a single dimension.

|ω0|−1 = 10 corresponding to |ω0|−1/rmax = 0.33,the optimal circular formation has center distanceρ/rmax = 0.85, corresponding to the minimum ofFigure 5(b) for that circle radius.

V. CONCLUSION

This paper presents a multi-vehicle samplingalgorithm that maximizes the observability of anestimated flowfield. The sampling algorithm steersvehicles to optimal sampling trajectories selectedfrom a parameterized space of candidate samplingtrajectories. The observability of the parametersdefining a flowfield model is evaluated along can-didate trajectories using the empirical observabilitygramian. The optimal flowfield observability isachieved by minimizing the unobservability index,which is the reciprocal of the smallest singularvalue of the empirical observability gramian. Weemploy a recursive Bayesian filter to provide esti-mates of the flow to the steering control algorithm.Numerical simulations of the algorithm in a Rank-ine vortex suggest that the sampling algorithmestimates the flowfield parameters even when noisymeasurements of the flow are used. In ongoingresearch we are expanding this sampling algorithmto higher fidelity flowfield and vehicle models,including flowfields in which vehicles may not beable to maintain forward progress against the flow.

VI. ACKNOWLEDGMENTS

The authors would like to acknowledge KayoIde, Sean Humbert, Doug Koch, and Angela Makifor discussions related to this work.

REFERENCES

[1] J. Elston, B. Argrow, E. Frew, and A. Houston, “Evaluationof UAS concepts of operation for severe storm penetrationusing hardware-in-the-loop simulations,” in AIAA Guidance,Navigation, and Control Conference, Toronto, Canada, Au-gust 2010, pp. 8178–8193.

[2] N. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. Fratan-toni, and R. Davis, “Collective motion, sensor networks, andocean sampling,” Proceedings of the IEEE, vol. 95, no. 1, pp.48–74, 2007.

[3] P. Lin, “Observations: The first successful typhoon eyewall-penetration reconnaissance flight mission conducted by theunmanned aerial vehicle, Aerosonde,” Bulletin of the Ameri-can Meteorological Society, vol. 87, pp. 1481–1483, 2006.

[4] “Hurricane basics,” National Oceanographicand Atmospheric Administration (NOAA),http://hurricanes.noaa.gov/pdf/hurricanebook.pdf, May1999.

rmax

vm

ax

20 40 60 80 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a)

rmax

µ

20 40 60 80 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(b)

0 10 20 30 40 50 60 700

0.5

1

1.5

2

2.5

3

3.5

4

4.5

Distance of Circle Center from Origin

Un

ob

se

rva

bili

ty I

nd

ex (

log

10)

(c)

−80 −60 −40 −20 0 20 40 60 80−80

−60

−40

−20

0

20

40

60

80

x

y

(d)

Fig. 7. Snapshot of the observability-based sampling algorithm at t = 20 seconds. (a)–(b) The log of marginal probability densities overthe parameter space ΩΩΩ. (c) The unobservability index versus ρ with the minimum shown by the red data point. (d) Particle trajectoriesare shown in a Rankine vortex with the desired circle center radius ρ shown by the red circle.

[5] A. J. Krener, “Eulerian and lagrangian observability of pointvortex flows,” Tellus, vol. 60, pp. 1089–1102, 2008.

[6] J. Cortes, S. Martinez, T. Karatas, and F. Bullo, “Coveragecontrol for mobile sensing networks,” IEEE Transactions onRobotics and Automation, vol. 20, no. 2, pp. 243–255, April2004.

[7] A. K. Singh and J. Hahn, “Determining optimal sensor loca-tions for state and parameter estimation for stable nonlinearsystems,” Industrial and Engineering Chemistry Research,vol. 44, no. 15, pp. 5645–5659, June 2005.

[8] N. R. Lawrance and S. Sukkarieh, “Autonomous explorationof a wind field with a gliding aircraft,” Journal of Guidance,Control, and Dynamics, vol. 34, no. 3, pp. 719–733, May-June2011.

[9] A. Krener and K. Ide, “Measures of unobservability,” inIEEE Conference on Decision and Control, Shanghai, China,December 2009, pp. 6401–6406.

[10] S. Lall, J. Marsden, and S. A. Glavaski, “A subspace approachto balanced truncation for model reduction of nonlinear con-trol systems,” International Journal of Robust and Nonlinear

Control, vol. 12, no. 6, pp. 519–535, 2002.[11] M. A. Demetriou, “Guidance of mobile actuator-plus-sensor

networks for improved control and estimation of distributedparameter systems,” IEEE Transactions on Automatic Control,vol. 55, no. 7, pp. 1570–1584, July 2010.

[12] Z. Song, Y. Chen, J. Liang, and D. Ucinski, “Optimal mobilesensor motion planning under nonholonomic constraints forparameter estimation of distributed systems,” in In Proc.of IEEE International Conference on Intelligent Robots andSystems, Edmonton, Alberta, Canada, August 2005, pp. 3163–3168.

[13] H. Chao and Y. Chen, “Surface wind profile measurementusing multiple small unmanned aerial vehicles,” in In Proc.of the 2010 American Control Conference, June-July 2010,pp. 4133–4138.

[14] K. M. Lynch, I. B. Schwartz, P. Yang, and R. A. Freeman,“Decentralized environmental modeling by mobile sensornetworks,” IEEE Transactions on Robotics, vol. 24, no. 3,pp. 710–724, 2008.

[15] H. J. Palanthandalam-Madapusi, A. Girard, and D. S. Bern-

rmax

vm

ax

20 40 60 80 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a)

rmax

µ

20 40 60 80 100

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(b)

0 10 20 30 40 50 60

1

1.5

2

2.5

3

3.5

4

4.5

Distance of Circle Center from Origin

Un

ob

se

rva

bili

ty I

nd

ex (

log

10)

(c)

−80 −60 −40 −20 0 20 40 60 80−80

−60

−40

−20

0

20

40

60

80

x

y

(d)

Fig. 8. Snapshot of the observability-based sampling algorithm at t = 400 seconds. (a)–(b) The log of marginal probability densities overthe parameter space ΩΩΩ. (c) The unobservability index versus ρ with the minimum shown by the red data point. (d) Particle trajectoriesare shown in light blue with the converged formation shown in dark blue.

stein, “Wind-field reconstruction using flight data,” in Proc.of the 2008 American Control Conference, 2008, pp. 1863–1868.

[16] J. W. Langelaan, N. Alley, and J. Neidhoefer, “Wind fieldestimation for small unmanned aerial vehicles,” Journal ofGuidance, Control, and Dynamics, vol. 34, no. 4, pp. 1016–1030, July-August 2011.

[17] S. Mulgund and R. F. Stengel, “Optimal nonlinear estimationfor aircraft flight control in wind shear,” Automatica, vol. 32,no. 1, pp. 3–13, 1996.

[18] R. Hermann and A. J. Krener, “Nonlinear controllabilityand observability,” IEEE Transactions on Automatic Control,vol. 22, no. 5, pp. 728–740, 1977.

[19] H. Salman, L. Kuznetsov, C. K. R. T. Jones, and K. Ide, “Amethod of assimilating Lagrangian data into a shallow-water-equation ocean model,” Monthly Weather Review, vol. 134,pp. 1081–1101, April 2006.

[20] A. V. Wouwer, N. Point, S. Porteman, and M. Remy, “Anapproach to the selection of optimal sensor locations indistributed parameter systems,” Journal of Process Control,vol. 10, no. 4, pp. 291–300, August 2000.

[21] D. A. Paley and C. Peterson, “Stabilization of collective mo-tion in a time-invariant flowfield,” AIAA Journal of Guidance,Control, and Dynamics, vol. 32, pp. 771–779, June 2009.

[22] R. Sepulchre, D. A. Paley, and N. E. Leonard, “Stabilizationof planar collective motion: All-to-all communication,” IEEETransactions on Automatic Control, vol. 52, pp. 811–824,May 2007.

[23] L. Techy, D. A. Paley, and C. Woolsey, “UAV coordinationon closed convex paths in wind,” AIAA Journal of Guidance,Control, and Dynamics, vol. 33, pp. 1946–1951, 2010.

[24] L. Arranz, A. Seuret, and C. C. de Wit, “Contraction controlof a fleet circular formation of AUVs under limited commu-nication range,” in American Control Conference, Baltimore,MD, June 2010, pp. 5991–5996.

[25] R. Sepulchre, D. A. Paley, and N. E. Leonard, “Stabilizationof planar collective motion with limited communication,”IEEE Transactions on Automatic Control, vol. 53, pp. 706–719, April 2008.

[26] N. L. R. Sepulchre, D. A. Paley, “Stabilization of collectivemotion of self-propelled particles,” Proc. 16th Int. Symp.Mathematical Theory of Networks and Systems, p. 10, July

2004.[27] L. DeVries and D. A. Paley, “Multi-vehicle control in a strong

flowfield with application to hurricane sampling,” Journal ofGuidance, Control, and Dynamics, vol. 35, no. 3, pp. 794–806, May-June 2012.

[28] S. Skogestad and I. Postlethwaite, Multivariable FeedbackControl: Analysis and Design. West Sussex, England: JohnWiley and Sons, 1996.

[29] U. Vaidya, “Observability gramian for nonlinear systems,” inProc. of the 46th IEEE Conf. on Decision and Control (CDC),New Orleans, LA, Dec. 2007, pp. 3357–3362.

[30] J. Hahn and T. F. Edgar, “A gramian based approach tononlinear quantification and model classification,” Industrialand Engineering Chemistry Research, vol. 40, pp. 5724–5731,2001.

[31] N. Bergman, “Recursive Bayesian estimation navigation andtracking applications,” Ph.D. dissertation, Department of Elec-trical Engineering, Linkoping University, Sweden, 1999.

[32] H. K. Khalil, Nonlinear Systems, 3rd ed. Prentice Hall, 2002.[33] D. A. Paley, F. Zhang, and N. E. Leonard, “Cooperative

control for ocean sampling: The Glider Coordinated ControlSystem,” IEEE Transactions on Control Systems Technology,vol. 16, no. 4, pp. 735–744, 2008.