Distributed Estimation for Motion Coordination in an Unknown Spatially Varying Flowfield

Distributed Estimation for Motion Coordination in an Unknown Spatially Varying Flowfield Cameron K Peterson and Derek A Paley University of Maryland, College Park, MD, 742, 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 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 using Gaussian process regression for a single vehicle 3 Lawrance et al mapped out a flowfield 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 filter 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 connected undirected graph 5 or a strongly connected and balanced digraph 6 Casbeer and Graduate student, Department of Aerospace Engineering; cammykai@yahoocom AIAA Student Member Associate Professor, Department of Aerospace Engineering and Institute for Systems Research; dpaley@umdedu AIAA Associate Fellow of

Beard showed that the state of a dynamical system that is estimated using an informationconsensus filter is comparable with one obtained from a centralized estimator 8 Olfati-Saber also provided decentralized Kalman filter formulations, 9,,, 2 developed techniques applicable to a heterogeneous group of sensors, 9 and established the properties of stability for the information-consensus filter 2 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 decentralized 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, 3 the vehicles operated in a spatially uniform flowfield and the estimation scheme was not distributed In other previous work, 4 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 =,, N is denoted by r k The particle travels in a plane at constant, unit speed relative to an ambient flowfield f k = f(r k ) and is subject to a steering control perpendicular to the velocity e iθ k of the vehicle relative to the flowfield The particle s inertial velocity is thus ṙ k = e iθ k + fk 4 The steering control u k = θ k is the turn rate of the orientation θ k 2 of

of the velocity relative to the flow In terms of the inertial speed s k = ṙ k and orientation γ k = arg(ṙ k ) the equations of motion are ṙ k = s k (t)e iγ k γ k = ν k, () 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 rotation 4 c k = r k + ω ie iγ k of each vehicle s trajectory is fixed and identical Steering control νk = ω s k (t) ensures particle k will traverse a circle with a fixed center c k () and a constant radius ω = c k () r k () 4 In a circular formation, c j = c k for all pairs j and k and the vector c = [c,, c N ] T of circle centers is in the null space of the projection matrix P = diag{ } (/N) T, where (,, ) T R N Given a known flowfield, one can use Lyapunov function S(r, γ) /2 c, P c 5 to show that the control a ν k = ω (s k + K P k c, e iγ k ), K >, (2) stabilizes N vehicles to a fixed circular formation [3, Theorem ] The time derivative of S along solutions of () is negative semi-definite with Ṡ = occurring when (2) evaluates to ν k = ω s k (t) and P c =, ie, 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 f k at position r k is modeled as 7 f k = f(r k ) = m n m= n= a m,n ψ m,n (r k ), where ψ k ψ(r k ) = [ψ, (r k ), ψ,2 (r k ),, ψ m, n (r k )] T is the vector of l = m n known basis functions evaluated at r k and a = [a,, a,2,, a m, n ] T is the set of unknown coefficients to a P k denotes the kth row of P 3 of

be estimated This note adopts the following basis vectors: ψ m,n (r k ) = cos ( πm Im(r Y k) ) cos ( πn Re(r X k) ), (3) where m =,, m, n =,, 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, ie, ȧ m,n = for all m and n; they are estimated using either noisy measurements of the flowfield or local approximations of the flowfield derived from noisy measurements of vehicle position Each flowfield measurement f k is corrupted by Gaussian, zero-mean measurement noise v k with variance R k C, so that 7 fk = ψ T k a + v k (4) The information filter is a variation of the Kalman filter that propagates forward the inverse of the error covariance 6 Let â be the estimated flowfield coefficients and M = E[(a â)(a â) T ] be the coefficient error covariance b The inverse error covariance M I is called the information matrix and i = Iâ is the information measurement 7 implements a discrete form of the information filter This note 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, ie, I (t) = I + (t t) and i (t) = i + (t t) The measurement update equations for particle k are 6, 7 I + k = I k + ψ kr k ψt k i + k = i k + ψ kr k f k Rewriting these equations using C k ψ k R k ψt k and y k ψ k R k f k yields 7 I + k = I k + C k i + k = i k + y k (5) The matrix C k and vector y k represent the information gained from particle k in a single update measurement The estimated coefficients â k for particle k are obtained from the information matrix using â k = I k i k 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 b E[ ] is the expected value of [ ] Measurements from N vehicles can be incorporated in a single 4 of

update step using the following sums: 7 C N C k = k= N k= ψ k R k ψt k (6) and y N y k = k= N k= ψ k R k f k (7) The measurement-update equations that incorporate the information from all particles are I + = I + C i + = i + y, (8) with the estimated coefficients â = I i A centralized information filter can be used directly to estimate â when all-to-all communication 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 N k, where j N k 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 C k Likewise y n,k is the nth entry of vehicle k s measurement vector Let τ,k be particle k s input to the estimated value That is, τ,k = C (i,j),k where i, j =,, l or τ,k = y n,k where n =,, l The proportional-integral (PI) consensus filter is 7 τ k = ξ(τ,k τ k ) K P j N k (τ k τ j ) + K I j N k (η k η j ) η k = K I j N k (τ k τ j ) (9) The gain ξ > determines how much the consensus filter relies upon its own input relative to the inputs from other connected particles τ k is the consensus variable, ie, the approximate average of C (i,j),k or y n,k, η k is an integrator variable, and K P and K I 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

r r 2 2 C, y fˆ IF Control ~ C f, y ν ~ C f 2, y 2 + ν 2 m m 2 Control ~ C f, y NC, Ny ˆf CF IF ν ~ f2 C 2, y 2 CF NC, Ny 2 2 IF ˆf 2 ν 2 r N ~ f N C, y N N ν N m N ~ f N C, y N N CF N C, N N y N IF fˆn ν N (a) Centralized architecture (b) Decentralized architecture Figure 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 illustrates both designs In Figure (a), the flowfield f k 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 r k 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 (b), uses a PI consensus filter (9) to calculate C k, the approximate average of matrix (6), and ȳ k, 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 m k (t) be the change in vehicle position from t t to t, subject to measurement error g k (t): m k (t) = r k (t) + g k (t) r k (t t) g k (t t) () Using ṙ k (t) = lim t (r k (t) r k (t t))/ t in () yields m k (t) ṙ k t + g k (t) g k (t t) [e iθk(t) + f k (t)] t + g k (t) g k (t t) For a sufficiently small t, one can assume θ k is constant without loss of generality The 6 of

Table Decentralized Information-Consensus Filter Cooperative Control Algorithm Input: Basis vector ψ, sensor variances R k, circle formation radius ω, and a strongly connected and balanced communication topology At each time step t, each particle k =,, N performs the following steps: : Take a noisy position measurement r k 2: Use the difference between the previous and current position measurement to approximate the local flowfield measurement using () 3: Evaluate the basis vector at the measured position: ψk ψ( r k ) 4: For n =,, 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 â k = I i 6: Compute the estimated flowfield ˆf k = ψ k â k = ψ k I i 7: Compute control ν k using (2) with s k, c and γ k replaced by their estimated values approximate local flowfield measurement f k (t) at time t is thus f k (t) = m k(t) t e iθ k(t) + g k(t) g k (t t) t () 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 () 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 ȳ k 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 and the following proposition present the information-consensus filter algorithm Proposition Let f(r k ) = m m= n n= a m,nψ m,n (r k ) be a spatially varying flowfield where the ψ m,n (r k ) are known basis vectors and the coefficients a m,n are unknown Using the algorithm described in Table forces convergence of solutions of model () to the set of a circular 7 of

formations with radius ω and direction of rotation determined by the sign of ω Proposition is justified because the estimation stage of the algorithm in Table (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 filter 2 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 () is an adequate replacement for the noisy flowfield measurement of Table 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 [3, Theorem ] Figure 2 illustrates the results of simulating the decentralized information-consensus algorithm using noisy position measurements in flowfield (3) with l = known basis functions Figure 2(a) depicts the vehicles (red circles), the vehicle trajectories (blue tracks), and the inertial velocities (black arrows) at t = 5 seconds The actual flowfield is depicted by the gray vector field in the background The vehicles were commanded to a circular formation with radius ω = Each particle approximates the local flowfield using () 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, k + and k + 2, modulus N We set K I = 5, K P = 5, ξ =, and sensor variance R k = Figure 2(b) shows the error in each basis coefficient estimate converging to zero for particle k = 5 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 algorithm using noisy position measurements in an arbitrary, random flowfield modeled by (but not necessary spanned by) l = 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 = 5 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 = 5 at times t = and t = 5 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

5 Im(r) Coefficient Error 5 5 5 Re(r) (a) t =[, 5] s 5 5 Time (s) (b) Flowfield coefficient errors, k = 5 Figure 2 Stabilization of a circular formation in an estimated spatially varying flowfield using an information-consensus filter driven by noisy position measurements IV Conclusion This note describes the design of a decentralized control algorithm for autonomous vehicles that operate in an estimated spatially varying flowfield The authors provide a distributed motion coordination algorithm that estimates a spatially varying flowfield and uses the estimate in a closed-loop multi-vehicle control Each vehicle implements an informationconsensus 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 CMMI92846 and the Office of Naval Research under Grant No N4-9-- 58 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 Summers, 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, 9, pp 56 69 9 of

5 5 Im(r) Im(r) 5 Re(r) (a) t =[, ] s 5 Re(r) (b) Flowfield percent errors, t = s 5 5 Im(r) Im(r) 5 Re(r) (c) t =[, 5] s 5 Re(r) (d) Flowfield percent errors, t = 5 s Figure 3 Stabilization of a circular formation in an estimated spatially varying flowfield using a decentralized information-consensus filter in a randomly generated flowfield 2 Petrich, J, Woolsey, C A, and Stilwell, D J, Planar Flow Model Identification for Improved Navigation of Small AUVs, Ocean Engineering, Vol 36, No, Jan 9, pp 9 3 3 Lawrance, 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,, pp 79 733 4 Langelaan, 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,, pp 6 5 Olfati-Saber, R, Fax, J A, and Murray, R M, Consensus and Cooperation in Networked Multi- Agent Systems, Proceedings of the IEEE, Vol 95, No, 7, pp 25 233 6 Ren, W, Beard, R W, and Atkins, E M A, A Survey of Consensus Problems in Multi-Agent Coordination, American Control Conference, 5 Proceedings of the 5, IEEE, Portland, Oregon, June 5, pp 859 864 7 Lynch, 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, 8, pp 7 724 of

8 Casbeer, D W and Beard, R W, Distributed Information Filtering using Consensus Filters, American Control Conference, St Louis, Missouri, 9, pp 882 887 9 Olfati-Saber, R, Distributed Tracking for Mobile Sensor Networks with Information-Driven Mobility, American Control Conference, 7, pp 466 462 Olfati-Saber, R and Shamma, J S, Consensus Filters for Sensor Networks and Distributed Sensor Fusion, IEEE Conference on Decision and Control, Seville, Spain, Dec 5, pp 6698 673 Olfati-Saber, R, Ultrafast Consensus in Small-World Networks, American Control Conference, Portland, Oregon, June 5, pp 237 2378 2 Olfati-Saber, R, Kalman-Consensus Filter: Optimality, Stability, and Performance, IEEE Conference on Decision and Control held jointly with Chinese Control Conference, Dec 9, pp 736 742 3 Peterson, C K and Paley, D A, Multivehicle Coordination in an Estimated Time-Varying Flowfield, Journal of Guidance, Control, and Dynamics, Vol 34, No, Jan, pp 77 9 4 Paley, 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, 9, pp 77 779 5 Sepulchre, 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, 7, pp 8 824 6 Simon, D, Optimal State Estimation: Kalman, H [infinity] and nonlinear approaches, John Wiley & Sons, Inc, New Jersey, 6 of