Hirsch M.J., Pardalos P.M., Murphey R. Dynamics of Information Systems: Theory and Applications

Подождите немного. Документ загружается.

316 D.B.M.M. Fontes and F.A.C.C. Fontes

is agents 1, 2,...,k and their target locations S. It starts by checking the colli-

sion condition, given by (16.4), for the allocation pair i → j and k →B

, where

is the optimal target for agent k when agents 1, 2,...,k are allocated to tar-

gets in S. If there is collision it returns 1; otherwise it calls itself with arguments

(i,j,k −1,S\{B

}).

Algorithm 3: Recursive function: ﬁnd if the allocation i →j collides with any

of the existing allocations.

Recursive Collision(i,j,k,S);1

if Col(i,j,k,S)=∞ then2

return Col(i,j,k,S) to caller;3

end4

(k, S);5

if collision condition is not satisﬁed then6

Col(i,j,k,S)=1;7

return Col(i,j,k,S) to caller;8

end9



=S \{B

};10

Call Collision(i,j,k−1,S



);11

Store information: Col(i,j,k,S)= 0;12

Return: Col(i,j,k,S);13

Algorithm 4 is also a recursive algorithm and it backtracks through the infor-

mation stored while solving subproblems, in order to retrieve the solution structure,

i.e., the actual agent-target allocation. This algorithm works backward from the ﬁnal

state (N, J ), corresponding to the optimal solution obtained, and ﬁnds the partition

by looking at the target stored for this state B

(N, J ), with which it can build the

structure of the solution found. Algorithm 4 also receives two arguments: the agents

and the set of target locations. It starts by checking whether the agent current lo-

cations set is empty. If so, the program returns to the point where the function was

called; Otherwise the backtrack information of the state is retrieved and the other

needed states evaluated.

At the end of the algorithm, f(I,J) gives the performance associated with the

best agent-target allocation.

16 Minimal Switching Time of Agent Formations with Collision Avoidance 317

Algorithm 4: Recursive function: retrieve agent-target allocation.

Recursive Allocation(i, S);1

if S =∅ then2

j =targetB

(i, S);3

Alloc(i) =j ;4



=S \{j };5

Call Allocation(i −1,S



);6

end7

Return: Alloc;8

Table 16.1 Agents random

initial location

Location

Agent 1 30 113

Agent 2 183 64

Agent 3 348 349

Agent 4 30 200

16.5 Computational Experiments

In this section, we report on the computational experiments performed. We consider

several standard structured formations for a team of agents: line, column, square,

diamond, and wedge. We also considered nonrigid formations, which are randomly

generated. The agents initial positions were randomly generated and are ﬁxed for

all experiments. In our experiments, we have decided to use d

as the Euclidian

distance although any other distance measure may have been used. All agents are

considered to travel at the same velocity v. Therefore, traveling times for each pos-

sible agent-target allocation are computed as t

/v. The separation constraints

impose, at any point in time, the distance between any two agent trajectories to be

at least 10 points; otherwise, it is considered that those two agents collide.

Let us ﬁrst show how a solution is recomputed in order to guarantee collision

avoidance, using a small example. Consider 4 agents A,B,C, and D, positioned as

given in Table 16.1 and four target positions 1, 2, 3, and 4 as in Table 16.2.

In Figs. 16.5 and 16.6, we give the graphical representation of the optimal agent-

target allocation found, when collisions are allowed and no collisions are allowed,

respectively. Since the separation constraints are active, the time required for all

agents to travel to the targets they have been assigned to is larger when they are

enforced. This value goes up from 4.4197 units of time when collisions are allowed

to 4.4702 when they are not.

318 D.B.M.M. Fontes and F.A.C.C. Fontes

Table 16.2 Target random

locations

Location

Target 1 95 258

Target 2 147 78

Target 3 248 362

Target 4 293 228

Fig. 16.5 Collisions allowed

The algorithm was implemented in Matlab and all examples run in between 0.034

seconds and 13.117 seconds. We note that the Matlab functions were interpreted

and not compiled. In the case of faster solutions being required, the code could

be compiled. As it can be seen, in the last two columns of Table 16.3, the total

percentage of the state space used is very small and decreases with problem size. The

full representation of the state space comprises n ×(2

−1) states. However, in our

implementation just a small percentage of the state space is computed. This value is

25% for the smallest problems and comes down to just over 8% for the largest ones.

The efﬁciency of the proposed method can also be seen from the ratio between the

number of subproblems used and the number of potential feasible solutions for the

problem (n!). This value comes down from 62.50% to less than 0.001%.

The beneﬁts of our particular implementation can be seen both from (1) the fact

that computation of an optimal solution is very fast; and (2) from the fact that only

a small part of the states is actually computed, either when this number is compared

16 Minimal Switching Time of Agent Formations with Collision Avoidance 319

Fig. 16.6 No collisions allowed

Table 16.3 Running time

and efﬁciency of the

computational

implementation

Number

of agents

Running

time (s)

Used states, percentage of

State space Feasible sol.

40.034 25.00 62.50

50.064 20.00 25.83

60.109 16.67 8.75

70.212 14.28 2.52

80.519 12.50 0.63

91.181 11.11 0.14

10 2.706 10.00 0.03

11 5.947 9.09 0.01

12 13.117 8.33 8E−04

to the total number of states in our representation, or when it is compared to the

number of possible feasible solutions.

16.6 Conclusion

We have developed an optimization algorithm to decide how to reorganize a for-

mation of vehicles into another formation of different shape with collision avoid-

ance, which is a relevant problem in cooperative control applications. The method

320 D.B.M.M. Fontes and F.A.C.C. Fontes

proposed here should be seen as a component of a framework for multiagent coor-

dination/cooperation, which must necessarily include other components such as a

trajectory control component.

The algorithm proposed is based on a dynamic programming approach that is

very efﬁcient for small dimensional problems. As explained before, the original

problem is solved by combining, in an efﬁcient way, the solution to some subprob-

lems. The method efﬁciency improves with the number of times the subproblems

are reused, which obviously increases with the number of feasible solutions. This

can be seen from the very small percentage of states use when compared with the

number of states represented or the number of potential feasible solutions.

Moreover, the proposed methodology is very ﬂexible, in the sense that it easily

allows for the inclusion of additional problem features, e.g., imposing geometric

constraints on each agent or on the formation as a whole, deciding on the agents

velocity, among others.

References

1. Aho, A.V., Hopcroft, J.E., Ullman, J.D.: Data Structures and Algorithms. Addison-Wesley,

Reading (1983)

2. Bellman, R.: Dynamic Programming. Princeton University Press, Princeton (1957)

3. Bender, J.: An overview of systems studies of automated highway systems. IEEE Trans. Veh.

Technol. 40(1 Part 2), 82–99 (1991)

4. Buzogany, L.E., Pachter, M., d’Azzo, J.J.: Automated control of aircraft in formation ﬂight. In:

AIAA Guidance, Navigation, and Control Conference and Exhibit, pp. 1349–1370, Monterey,

California (1993)

5. Desai, J.P., Ostrowski, P., Kumar, V.: Modeling and control of formations of nonholonomic

mobile robots. IEEE Trans. Robot. Autom. 17(6), 905–908 (2001)

6. Fontes, D.B.M.M., Fontes, F.A.C.C.: Optimal reorganization of agent formations. WSEAS

Trans. Syst. Control 3(9), 789–798 (2008)

7. Fontes, F.A.C.C., Fontes, D.B.M.M., Caldeira, A.C.D.: Model predictive control of vehicle

formations. In: Pardalos, P., Hirsch, M.J., Commander, C.W., Murphey, R. (eds.) Optimization

and Cooperative Control Strategies. Lecture Notes in Control and Information Sciences, vol.

381. Springer, Berlin (2009). ISBN:978-3-540-88062-2

8. Hu, J., Sastry, S.: Optimal collision avoidance and formation switching on Riemannian mani-

folds. In: IEEE Conference on Decision and Control, vol. 2, pp. 1071–1076. IEEE Press, New

York (1998)

9. Jin, Z., Shima, T., Schumacher, C.J.: Optimal scheduling for refueling multiple autonomous

aerial vehicles. IEEE Trans. Robot. 22(4), 682–693 (2006)

10. Murray, R.M.: Recent research in cooperative control multivehicle systems. J. Dyn. Syst.

Meas. Control 129, 571–583 (2007)

11. Rasmussen, S.J., Shima, T.: Branch and bound tree search for assigning cooperating UVAs to

multiple tasks. In: Institute of Electrical and Electronic Engineers, American Control Confer-

ence 2006, Minneapolis, Minnesota, USA (2006)

12. Rasmussen, S.J., Shima, T., Mitchell, J.W., Sparks, A., Chandler, P.R.: State-space search for

improved autonomous UAVs assignment algorithm. In: IEEE Conference on Decision and

Control, Paradise Island, Bahamas (2004)

13. Schumacher, C.J., Chandler, P.R., Rasmussen, S.J.: Task allocation for wide area search mu-

nitions via iterative network ﬂow. In: American Institute of Aeronautics and Astronautics,

Guidance, Navigation, and Control Conference 2002, Reston, Virginia, USA (2002)

16 Minimal Switching Time of Agent Formations with Collision Avoidance 321

14. Schumacher, C.J., Chandler, P.R., Rasmussen, S.J.: Task allocation for wide area search muni-

tions with variable path length. In: Institute of Electrical and Electronic Engineers, American

Control Conference 2003, New York, USA (2003)

15. Smith, T.R., Hanssmann, H., Leonard, N.E.: Orientation control of multiple underwater vehi-

cles with symmetry-breaking potentials. In: IEEE Conference on Decision and Control, vol. 5,

pp. 4598–4603 (2001)

16. Swaroop, D., Hedrick, J.K.: Constant spacing strategies for platooning in automated highway

systems. J. Dyn. Syst. Meas. Control 121, 462 (1999)

17. Wolfe, J.D., Chichka, D.F., Speyer, J.L.: Decentralized controllers for unmanned aerial vehi-

cle formation ﬂight. In: AIAA Guidance, Navigation, and Control Conference and Exhibit,

pp. 96–3833, San Diego, California (1996)

18. Yamagishi, M.: Social rules for reactive formation switching. Technical Report UWEETR-

2004-0025, Department of Electrical Engineering, University of Washington, Seattle, Wash-

ington, USA (2004)

19. Yamaguchi, H.: A cooperative hunting behavior by mobile-robot troops. Int. J. Robot. Res.

18(9), 931 (1999)

20. Yamaguchi, H., Arai, T., Beni, G.: A distributed control scheme for multiple robotic vehicles

to make group formations. Robot. Auton. Syst. 36(4), 125–147 (2001)

Chapter 17

A Moving Horizon Estimator Performance

Bound

Nicholas R. Gans and Jess W. Curtis

Summary Moving Horizon implementations of the Kalman Filter are widely used

to overcome weaknesses of the Kalman Filter, or in problems when the Kalman

Filter is not suitable. While these moving horizon approaches often perform well, it

is of interest to encapsulate the loss in performance that comes when terms in the

Kalman Filter are ignored. This paper introduces two methods to calculate a worst

case performance bound on a Moving Horizon Kalman Filter.

17.1 Introduction

Since its introduction, the Kalman Filter (KF) [2] has enjoyed widespread and suc-

cessful use in estimating the state of a dynamic system assailed by noise. Given

an accurate model and appropriate noise statistics, the KF provides state estimates

that are mean-square-optimal. Fields that make wide use of the KF include avia-

tion, localization, navigation, and economics. For its strengths, the performance of

the KF will suffer if the provided system model is inaccurate, the noise statistics

are inaccurate, or the noise signals can not be modeled as white, Gaussian random

processes with known noise statistics. Furthermore, the recursive nature of the KF

requires that measurement data arrive in order and at known period [6]. This makes

the KF inappropriate for use in distributed and networked inappropriate for use in

distributed and networked estimation schemes that may have transmission delays.

For these reasons, Moving Horizon Kalman Filter (MHKF) approaches (also re-

ferred to as receding horizon and limited memory approaches) have been inves-

tigated for over forty years [3]. These various approaches are often designed to

address speciﬁc problems with the standard Kalman Filter. Problems addressed

N.R. Gans (



)

National Research Council and Air Force Research Laboratory, Eglin Air Force Base, FL, USA

e-mail: ngans@uﬂ.edu

J.W. Curtis

Air Force Research Laboratory, Eglin Air Force Base, FL, USA

e-mail: jess.curtis@eglin.af.mil

M.J. Hirsch et al. (eds.), Dynamics of Information Systems,

Springer Optimization and Its Applications 40, DOI 10.1007/978-1-4419-5689-7_17,

323

324 N.R. Gans and J.W. Curtis

through the use of a moving horizon include modeling error and bias [3, 8], un-

known (possible nonGaussian) noise statistics [1], constrained state or noise distri-

butions [7], and unknown initial state statistics [4]. Some authors note that the ﬁlter

length is a tunable parameter [5], or that the system must be observable within the

window length [1] but there appears to be little investigation into the effect of ﬁlter

length on the accuracy of the estimate.

The investigation in this chapter is primarily interested in using the MHKF for

overcoming data-rate and order dependency and hidden measurement-state correla-

tion that may render standard Kalman ﬁltering results inconsistent. Consider a cen-

tral facility is estimating a dynamic system state via measurements from a collection

of distributed sensors. If the communication between the sensors and the central fa-

cility is subject to random delays, and the central estimator is running a KF, the

central estimator might receive a measurement from time t −N at time t. It thus is

currently carrying an estimate ˆx

and error covariance matrix P

that do not allow

a fusion with the delayed measurement. The problem of out-of-sequence measure-

ments has been a well-studied topic in optimal estimation. Several approaches (both

optimal and suboptimal) have been proposed to deal with the problem of ﬁltering

measurements that arrive for fusion out of their proper sequence [6].

One approach for solving this problem is to save a window of estimates ˆx

and

their associated uncertainty matrices P

. Then, in the event that a late measurement

is received, the ﬁlter can be re-run from time t − N to the present. The difﬁculty

of this approach is that it requires a great deal more storage than a standard KF,

and the computational requirements become likewise more severe (since instead of

being required to perform a single time-update and measurement update, the central

estimator might be required to do N times this computation at any given time).

One of the key questions in terms of analyzing this moving horizon strategy is

how the performance degrades as a function of horizon length. In particular, is it

possible to deﬁne a bound as a function of the horizon length such that if we desire

some given level of estimation performance then a horizon length can be found to

guarantee that minimal level of accuracy?

This paper attempts to address this question. Two error bounds are formulated

that encapsulate worst case performance of a MHKF. Guarantee of worst case per-

formance is essential in many robust estimation problems.

17.2 Linear State Estimation

Consider the linear, discrete-time system

k+1

= Ax

+ω

= Hx

+ν

(17.1)

where k ∈[0, ∞] is the discrete time index, x

∈

is the system state at time k,

∈

is the measurement vector at time k, and ω ∈

and ν ∈

are white,

zero-mean, Gaussian, noise process with covariance matrices Q ∈

n×n

and R ∈

17 A Moving Horizon Estimator Performance Bound 325



m×m

, respectively. The matrix H ∈

m×n

maps a state vector into a measurement

vector.

The well-known Kalman Filter equations [2] provide an optimal estimate in the

sense that is provides the minimum mean square error estimate for linear estimators.

The one-step KF equations are given as

ˆx

k+1

= A ˆx

−H ˆx

) (17.2)

= AP



R +HP



−1

(17.3)

k+1

= A



−1



−1

+Q (17.4)

where K

is the Kalman gain and P

is the covariance of the estimate error e

ˆx

−x

. It is known that if {H, A} is observable and {A, Q} controllable, then K

and P

will converge to a steady state values K and P such that

K =AP H



R +HPH



−1

P = A



−1



−1

17.2.1 Kalman Filter as an IIR Filter

Assume that the conditions for observability and controllability have been met, and

that we have a collected measurements for time i ={0 ...k} (denote the batch of

measurement by Y), which will be used to estimate the current state at time k.

Further assume that all of the measurement come from a single sensor with error

covariance matrix R, and that we have an initial estimate of the state, ˆx

k−N

, whose

associated covariance matrix P

k−N

is such that the ﬁlter is in steady state. That is,

for all i

ˆx

i+1

=A ˆx

+K(y

i+1

−HAˆx

)

where K is the constant, steady-state Kalman gain. An artiﬁcial measurement is

created to capture this initial condition as



=ˆx

Assume then that the measurement list contains this artiﬁcial measurement whose

effect is to initialize the ﬁlter with our a priori information.

The time history of the state can be estimated as

ˆx

= AKy

+K(y

−HAKy

)

= Ky

+(A −KHA)Ky

(17.5)