decentralized estimation and control for multisensor systems

DecentralizedEstimation and Controlfor Multisensor Systems

2DecentralizedEstimation and Controlfor MultisensorSystems

Arthur G.O..Mutambara

CRC PressBoca Raton Boston London New York Washington, D.C.

1998 by CRC Press LLC

Trademark Notice: Product or corporate names may be trademarks or registered trademarks,and are used only for identification and explanation, without intent to infringe.

No claim to original U.S. Government worksInternational Standard Book Number 0-8493-1865-3Library of Congress Card Number 97-51553Printed in the United States of America 1 2 3 4 5 6 7 8 9 0Printed on acid-free paper

Preface

This book is concerned with the problem of developing scalable, decen-tralized estimation and control algorithms for both linear and non linearmultisensor systems. Such algorithms have extensive applications in mod-ular robotics and complex or large scale systems. Most existing algorithmsemploy some form of hierarchical or centralized structure for data gatheringand processing. In contrast, in a fully decentralized system, all informa-tion is processed locally. A decentralized data fusion system consists ofa network of sensor nodes, each with its own processing facility, whichtogether do not require any central processing or central communicationfacility. Only node-to-node communication and local system knowledge ispermitted.

Algorithms for decentralized data fusion systems based on the linear In-formation filter have previously been developed. These algorithms obtaindecentrally exactly the same results as those obtained in a conventionalcentralized data fusion system. However, these algorithms are limited inrequiring linear system and observation models, a fully connected sensornetwork topology, and a complete global system model to be maintainedby each individual node in the network. These limitations mean that ex-isting decentralized data fusion algorithms have limited scalability and arewasteful of communication and computation resources.

This book aims to remove current limitations in decentralized data fusionalgorithms and further to extend the decentralized estimation principle toproblems involving local control and actuation. The linear Information fil-ter is first generalized to the problem of estimation for nonlinear systemsby deriving the extended Information filter. A decentralized form of thealgorithm is then developed. The problem of fully connected topologies issolved by using generalized model distribution where the nodal system in-volves only locally relevant states. Computational requirements are reducedby using smaller local model sizes. Internodal communication is model de-fined such that only nodes that need to communicate are connected. Whennodes communicate they exchange only relevant information. In this way,

97-51553CIP

Library of Congress Cataloging-in-Publication Data

Mutambara, Arthur G.O.Decentralized estimation and control for multisensorsystems /

[Arthur G.O. Mutambara].p. cm.

Includes bibliographical references and index.ISBN 0-8493-1865-3 (alk. paper)1. Multisensor data fusion. 2. Automatic control. 3. Robots-

-Control systems. I. Title.TJ211.35.M88 1998629.8 -dc21

This book contains information obtained from authentic and highly regarded sources. Reprintedmaterial is quoted with permission, and sources are indicated. A wide variety of references are listed.Reasonable efforts have been made to publish reliable data and information, but the author and thepublisher cannot assume responsibility for the validity of all materials or for the consequences oftheir use.

Neither this book nor any part may be reproduced or transmitted in any form or by any means,electronic or mechanical, including photocopying, microfilming, and recording, or by any informationstorage or retrieval system, without prior permission in writing from the publisher.

The consent of CRC Press LLC does not extend to copying for general distribution, for promotion,for creating new works, or for resale. Specific permission must be obtained in writing from CRCPress LLC for such copying.

Direct all inquiries to CRC Press LLC, 2000 Corporate Blvd., N.W., Boca Raton, Florida 33431.

communication is minimized both in terms of the number of communicationlinks and size of message. The scalable network does not require propaga-tion of information between unconnected nodes. Estimation algorithms forsystems with different models at each node are developed.

The decentralized estimation algorithms are then applied to the problemof decentralized control. The control algorithms are explicitly described interms of information. Optimal control is obtained locally using reducedorder models with minimized communication requirements, in a scalablenetwork of control nodes. A modular wheeled mobile robot is used todemonstrate the theory developed. This is a vehicle system with nonlinearkinematics and distributed means of acquiring information.

Although a specific modular robot is used to illustrate the usefulnessof the algorithms, their application can be extended to many robotic sys-tems and large scale systems. Specifically, the modular design philosophy,decentralized estimation and scalable control can be applied to the MarsSojourner Rover with dramatic improvement of the Rover's performance,competence, reliability and survivability. The principles of decentralizedmultisensor fusion can also be considered for humanoid robots such as theMIT Humanoid Robot (Cog). Furthermore, the proposed decentralizationparadigm is widely useful in complex and large scale systems such as airtraffic control, process control of large plants, the Mir Space Station andspace shuttles such as Columbia.

The Author

Dr. Arthur G.O. Mutambara is an Assistant Professor of Robotics andMechatronics in the Mechanical Engineering Department at the joint En-gineering College of Florida Agricultural and Mechanical University andFlorida State University in Tallahassee, Florida (U.S.A.). He has beena Visiting Research Fellow at the Massachusetts Institute of Technology(MIT) in the Astronautics and Aeronautics Department (1995), at theCalifornia Institute of Technology (CaITech) (1996) and at the NationalAeronautics and Space Administration (NASA), Jet Propulsion .. Labora-tory, in California (1994). In 1997 he was a Visiting Research Scientist atthe NASA Lewis Research Center in Cleveland, Ohio. He has served onboth the Robotics Review Panel and the Dynamic Systems and ControlsPanel for the U.S.A. National Science Foundation (NSF).

Professor Mutambara received the Doctor of Philosophy degree in Robot-ics from Merton College, Oxford University (U.K.) in March 1995, wherehe worked with the Robotics Research Group. He went to Oxford as aRhodes Scholar and also earned a Master of Science in Computation fromthe Oxford University Computing Laboratory in October 1992, where heworked with the Programming Research Group. Prior to this, he had re-ceived a Bachelor of Science with Honors in Electrical Engineering from theUniversity of Zimbabwe in 199L

Professor Mutambara's main research interests include multisensor fu-sion, decentralized estimation, decentralized control, mechatronics and mod-ular robotics. He teaches graduate and undergraduate courses in robotics,mechatronics, control systems, estimation theory, dynamic systems and vi-brations. He is a Membet of the Institute of Electrical and ElectronicEngineering (IEEE), the Institute of Electrical Engineering (lEE) and theBritish Computer Society (BCS).

Acknowledgments

The research material covered in this book is an extension of the work I didfor my Doctor of Philosophy degree at Oxford University where I workedwith the Robotics Research Group. It is with great pleasure that I acknowl-edge the consistent and thorough supervision provided by Professor HughDurrant-Whyte of the Robotics Research Group, who is .. now Professor ofMechatronics Engineering at the University of Sydney in Australia. Hisresourcefulness and amazing subject expertise were a constant source ofinspiration. Professor Mike Brady, Head of the Robotics Research Groupat Oxford, was always accessible and supportive. My fellow graduate stu-dents in the Robotics Research Group provided the requisite team spiritand enthusiasm.

After finishing my Doctorate at Oxford University in March 1995, I tookup a Visiting Research Fellowship at the Massachusetts Institute of Tech-nology (MIT) in the Astronautics and Aeronautics Department where Icarried out additional research with the Space Engineering Research Cen-ter (SERC). I would like to thank Professor Edward Crawley for invitingme to MIT and for his insightful comments. I would also like to thankProfessor Rodney Brooks of the Artificial Intelligence (AI) laboratory atMIT for facilitating visits to the AI laboratory and providing informationabout the MIT Humanoid Robot (Cog). Further work on the book wascarried out at the National Aeronautics and Space Administration (NASA)Lewis Research Center in Cleveland Ohio, where I was a Summer FacultyResearch Fellow in 1997. I would like to thank Dr. Jonathan Litt of NASALewis for affording me that opportunity.

Quite a number of experts reviewed and appraised the material coveredin this book. In particular, I would like to thank the following for theirdetailed remarks and suggestions: Professor Yaakov Bar-Shalom of theElectrical and Systems Engineering at University of Connecticut, ProfessorPeter Fleming. who is Chairman of the Department of Automatic ControlEngineering at the University of Sheffield (U.K.), Dr. Ron Daniel of theRobotics Research Group at the University of Oxford, Dr. Jeff Uhlmann

and Dr. Simon Julier, who are both at the Naval Research Laboratory(NRL) in Washington D.C. I would also like to thank all my colleaguesand students at the FAMU-FSU College of Engineering, in particular thosegraduate research students that I have supervised and hence unduly sub-jected to some of the ideas from the book: Jeff, Selekwa, Marwan, Rashan,Robert and Todd. Their questions and comments helped me make some ofthe material more readable.

I attended Oxford University as a Rhodes Scholar and visited roboticsresearch laboratories (both academia and industry) in the United States ofAmerica, Japan, Germany and the United Kingdom while presenting pa-pers at international conferences, courtesy of funds provided by the RhodesTrust. Consequently, financial acknowledgment goes to the generality of thestruggling people of Southern Africa who are the living victims of the im-perialist Cecil John Rhodes. Every Rhodes Scholar should feel a sense ofobligation and duty to the struggle of the victims of slavery, colonialismand imperialism throughout the world.

This book is dedicated to oppressed people throughout the world and theirstruggle for social justice and egalitarianism. Defeat is not on the agenda.

Contents

1 Introduction 11.1 Background........ 11.2 Motivation 3

1.2.1 Modular Robotics 41.2.2 The Mars Sojourner Rover 41.2.3 The MIT Humanoid Robot (Cog) 61.2.4 Large Scale Systems ... , . . 71.2.5 The Russian Mir Space Station 71.2.6 The Space Shuttle Columbia 9

1.3 Problem Statement . 121.4 Approach .... 0 13

1.4.1 Estimation . 131.4.2 Control . 131.4.3 Applications 13

1.5 Principal Contributions 141.6 Book Outline . . . . . . 15

2 Estimation and Information Space 192.1 Introduction......... 192.2 The Kalman Filter . . . . . . . . 19

2.2.1 System Description. . . . 202.2.2 Kalman Filter Algorithm 21

2.3 The Information Filter. . . . . . 222.3.1 Information Space . . . . 222.3.2 Information Filter Derivation 262.3.3 Filter Characteristics. . . . . 282.3.4 An Example of Linear Estimation 282.3.5 Comparison of the Kalman and Information Filters. 31

2.4 The Extended Kalman Filter (EKF) 332.4.1 Nonlinear State Space 342.4.2 EKF Derivation 34

5 Scalable Decentralized Control5.1 Introduction .5.2 Optimal Stochastic Control . .

5.2.1 Stochastic Control Problem5.2.2 Optimal Stochastic Solution.5.2.3 Nonlinear Stochastic Control5.2.4 Centralized Control .....

5.3 Decentralized Multisensor Based Control.5.3.1 Fully Connected Decentralized Control.5.3.2 Distribution of Control Models ....5.3.3 Distributed and Decentralized Control5.3.4 System Characteristics ..

5.4 Simulation Example .5.4.1 Continuous Time Models ..5.4.2 Discrete Time Global Models5.4.3 Nodal Transformation Matrices.5.4.4 Local Discrete Time Models ...

2.4.3 Summary of the EKF Algorithm2.5 The Extended Information Filter (ElF)

2.5.1 Nonlinear Information Space ..2.5.2 ElF Derivation . . . . . . . . . .2.5.3 Summary of the ElF Algorithm .2.5.4 Filter Characteristics .

2.6 Examples of Estimation in Nonlinear Systems2.6.1 Nonlinear State Evolution and Linear Observations.2.6.2 Linear State Evolution with Nonlinear Observations2.6.3 Nonlinear State Evolution with Nonlinear

Observations .2.6.4 Comparison of the EKF and ElF

2.7 Summary .

3 Decentralized Estimation for Multisensor Systems3.1 Introduction.................3.2 Multisensor Systems .

3.2.1 Sensor Classification and Selection .3.2.2 Positions of Sensors in a Data Acquisition System3.2.3 The Advantages of Multisensor Systems3.2.4 Data Fusion Methods3.2.5 Fusion Architectures . . . . . .

3.3 Decentralized Systems .3.3.1 The Case for Decentralization.3.3.2 Survey of Decentralized Systems

3.4 Decentralized Estimators .3.4.1 Decentralizing the Observer .3.4.2 The Decentralized Information Filter (DIF)3.4.3 The Decentralized Kalman Filter (DKF) . .3.4.4 The Decentralized Extended Information Filter

(DEIF) .3.4.5 The Decentralized Extended Kalman Filter (DEKF)

3.5 The Limitations of Fully ConnectedDecentralization

3.6 Summary . . . . . . . . . . . . . .

4 Scalable Decentralized Estimation4.1 Introduction...................

4.1.1 Model Distribution .4.1.2 Nodal Transformation Determination

4.2 An Extended Example . . . . . . . . . .4.2.1 Unsealed Individual States ...4.2.2 Proportionally Dependent States4.2.3 Linear Combination of States ..

383939404343444446

485153

555556565960616264646668686972

7476

7779

8181828283838688

4.3

4.4

4.5

4.6

4.7

4.2.4 Generalizing the Concept .....4.2.5 Choice of Transformation Matrices4.2.6 Distribution of Models . . . . . . .The Moore-Penrose Generalized Inverse: T+4.3.1 Properties and Theorems of T+ . . . .4.3.2 Computation of T+ .Generalized Internodal Transformation . . . .4.4.1 State Space Internodal Transformation: V ji (k)4.4.2 Information Space Internodal Transformation: T ji (k)Special Cases of T ji (k) . . . . . . . . . . . . . . . . . . . . .4.5.1 Scaled Orthonormal Ti(k) and Tj(k)4.5.2 DiagonaIIJ(zj(k)) .4.5.3 Nonsingular and Diagonal IJ(zj(k)) .4.5.4 Row Orthonormal Cj(k) and Nonsingular Rj(k)4.5.5 Row Orthonormal Ti(k) and Tj(k)4.5.6 Reconstruction of Global Variables.Distributed and Decentralized Filters ....4.6.1 The Distributed and Decentralized Kalman Filter

(DDKF) .4.6.2 The Distributed and Decentralized Information

Filter (DDIF) . . . . . .. . . . . . . . . . . .4.6.3 The Distributed and Decentralized Extended

Kalman Filter (DDEKF) .4.6.4 The Distributed and Decentralized Extended

Information Filter (DDEIF)Summary .

9394949697

100101101106108108109109110111111112

112

114

116

117119

121121121122123126127128129131132134135135137138139

5.5 Summary . . . . . . . . .. . . . . . . . . . . . . . . ., .

6 Multisensor Applications: A Wheeled Mobile Robot6.1 Introduction .6.2 Wheeled Mobile Robot (WMR) Modeling

6.2.1 Plane Motion Kinematics6.2.2 Decentralized Kinematics .

6.3 Decentralized WMR Control .6.3.1 General WMR System Models6.3.2 Specific WMR Implementation Models .6.3.3 Driven and Steered Unit (DSU) Control6.3.4 Application of Internodal Transformation

6.4 Hardware Design and Construction.6.4.1 WMR Modules .6.4.2 A Complete Modular Vehicle6.4.3 Transputer Architecture . . .

6.5 Software Development .6.5.1 Nodal Program (Communicating Control Process)6.5.2 Configuration Program (Decentralized Control)

6.6 On-Vehicle Software .6.6.1 Nodal Software .6.6.2 Decentralized Motor Control6.6.3 WMR Trajectory Generation

6.7 Summary . . . . . . . . . . . . . .

7 Results and Performance Analysis7.1 Introduction .7.2 System Performance Criteria

7.2.1 Estimation Criteria.7.2.2 Control Criteria

7.3 Simulation Results .7.3.1 Innovations .7.3.2 State Estimates.7.3.3 Information Estimates and Control.

7.4 WMR Experimental Results . . . . . . . . .7.4.1 Trajectory Tracking .7.4.2 Innovations and Estimated Control Errors.

7.5 Discussion of Results .7.5.1 Local DSU Innovations .7.5.2 Wheel Estimated Control Errors7.5.3 WMR Body Estimates.

7.6 Summary .

140

141141142143145149150152158159160161163165167168173174174176176181

183183183184185186186187188189190192204204206206207

8 Conclusions and Future Research8.1 Introduction............8.2 Summary of Contributions. . . .

8.2.1 Decentralized Estimation8.2.2 Decentralized Control8.2.3 Applications .

8.3 Research Appraisal . . . . . . . .8.3.1 Decentralized Estimation8.3.2 Decentralized Control

8.4 Future Research Directions8.4.1 Theory ...8.4.2 Applications ....

Bibliography

Index

209209209210210210211211213213214215

217

227

Chapter 1Introduction

1.1 BackgroundThis book is concerned with the problem of developing scalable decen-

tralized estimation and control algorithms for both linear and nonlinearmultisensor systems.

A sensor is any device which receives a signal or stimulus and gener-ates measurements that are functions of that stimulus. Sensors are usedto monitor the operation of a system and to provide information throughwhich the system may be controlled. In this way, a sensor allows a systemto learn and continuously update its own model of the world. However, asingle sensor is not always capable of obtaining all the required informationreliably at all times in varying environments. Furthermore, as the complex-ity of a system increases so does the number and variety of sensors requiredto provide a complete description of the system and allow for its effectivecontrol. Multiple sensors provide a better and more precise understand-ing of the system and its operation. Multisensor systems have found wideapplications in areas such as robotics, aerospace, defense, manufacturing,process control and power generation.

A multisensor system may employ a range of different sensors, with dif-ferent characteristics, to obtain information about an environment. Thediverse and sometimes conflicting information obtained from multiple sen-sors gives rise to the problem of how the information may be combined in aconsistent and coherent manner. This is the data fusion problem. Multisen-sor fusion is the process by which information from a multitude of sensorsis combined to yield a coherent description of the system under observa-tion. Both quantitative and qualitative sensor fusion methods have beenadvanced in the literature. Quantitative methods are used exclusively inthis book. They are based on probabilistic and statistical methods of mod-eling and combining information. Quantitative techniques include methodsof statistical decision theory, Bayesian analysis and filtering techniques.

1

2 Decentralized Estimation and Control Introduction 3

K alman filtering and its algebraically equivalent technique, informationfiltering, are quantitative data fusion methods based on linear decision rules.The Information filter essentially tracks information about states and notthe states themselves. The properties of information variables enable thisfilter to be easily distributed and decentralized. The work described in thisbook is based on these methods.

A variety of information based data fusion algorithms have been em-ployed in recent work [22], [41], [71], [80]. In this work extensive descrip-tions of centralized, hierarchical and decentralized architectures and theiradvantages and limitations are discussed. Emphasis is placed on fully de-centralized sensing based on the linear Information filter. A fully decentral-ized system is defined as a data processing system in which all informationis processed locally and there is no central processing site. It consists ofa network of sensor nodes, each with its own processing facility, whichtogether do not require any central fusion or communication facility. Spe-cial Transputer based architectures have been built to demonstrate thatthe principle of decentralized sensing is indeed viable. Elsewhere, researchwork using conventional state space multisensor fusion methods has alsobeen extensive, as evidenced by the work of Abidi and Gonzalez [1], Aggar-wal [3], Bar-Shalom [14], Luo [68], McKendall and Mintz [77], and Richardand Marsh [111].

Most of the current sensor fusion algorithms consider systems describedby linear dynamics and observation models. Most practical problems havenonlinear dynamics and sensor information nonlinearly dependent on thestates that describe the environment. Although, linearization methods suchas the extended Kalman filter are popular, there is currently no algorithmthat solves the non linear data fusion problem in Information filter. form.Given the advantages of using information variables in distributed and de-centralized fusion, this is an extremely important case to address. Anothermajor drawback of the algorithms presented to date is that although theytell us how to fuse information, they do not say how to use this fusedinformation to control the system. The applications of decentralized multi-sensor and multiactuator control are potentially huge. Research on systemsthat have been described as 'decentralized' control has been prolific. Thedefinition of a decentralized system has been varied, in some cases sim-ply referring to schemes involving more than one controller. Work in thisfield has included that of Chong and Mori [38], Hashemipour [50], Sandell[113], Siljak [115] and Speyer [117]. The issue, however, is that most ofthese systems are not fully decentralized and they do not exploit the useof information variables. In these systems, some central processing site isalways retained, leading to an essentially hierarchical structure consistingof interacting levels.

The work by Speyer is the exception. However, he does not exploit theuse of information variables. Moreover, in Speyer's algorithm and fully de-centralized estimation algorithms in [41], [71], [106], the sensing networktopology is fully connected, that is, each local sensing node communicateswith all the other nodes. This poses serious problems of communication re-dundancy, duplication of computation and limited system sealability. Fur-thermore, loss of anyone communication link violates the fully connectedassumption. In fully connected networks local models of state, informa-tion and control are the same as those of the equivalent centralized system.Consequently, the decentralized control algorithm derived from such a net-work is essentially the centralized controller repeated at each node. Thisis of limited practical benefit, particularly for a large system with a largenumber of nodes.

There have been efforts to derive non-fully connected decentralized es-timation topologies [48], [54], using a special internodal filter, the channelfilter. This is an additional filter which integrates information common totwo communicating nodes. It is used to propagate information between twounconnected nodes. Interesting though this approach is, it still employs thesame size of variables locally as in the centralized case and the additionalfiltering process at each node increases the computational load. Moreover,this work only addresses estimation in linear systems and not nonlinearestimation or control systems.

1.2 MotivationThe motivation for the material presented in this book derives from two

aspects of the work discussed above. The first point of motivation is thebenefits of multisensor systems, in particular decentralized methods of datafusion and control. The second point derives from the limitations of existingdecentralized methods. This book seeks to develop fully decentralized datafusion and control algorithms which do not exhibit the drawbacks of exist-ing methods. In particular, it aims to address both the problem of usingreduced local models at sensor nodes and that of reducing communicationand connection requirements.

The estimation and control algorithms developed have potential applica-tions in multisensor systems and large scale systems, which. are also oftencharacterized by multiple actuators, controllers, targets and trackers. Thealgorithms can also be applied in such fields as space structures and flexiblestructures.

4 Decentralized Estimation and ControlIntroduction 5

1.2.1 Modular RoboticsOf the many multisensor systems that motivate the theory developed

in this book, modular robotics is the most specific application of interest.A modular vehicle has the same function as any conventional robot ex-cept that it is constructed from a small number of standard unit~. Eachmodule has its own hardware and software, driven and steered units, sen-sors, communication links, power unit, kinematics, path planning, obstacleavoidance sensor fusion and control systems. There is no central processoron the vehicle. Vehicle kinematics and dynamics are invariably nonlinearand sensor observations are also not linearly dependent on the sensed en-vironmental states. These kinematics, models and observation spaces must.be distributed to the vehicle modules.

The vehicle employs multiple sensors to measure its body. position a~dorientation, wheel positions and velocities, obstacle locations and chang~smthe terrain. Sensor information from the modules is fused in a decentrahzedway and used to generate local control for each module. The adv~nta~esof this modular technology include reduction of system costs, applicationflexibility, system reliability, scalability and survivability. However, for themodularity to be functional and effective, fully decentralized and scalablemultisensor fusion and control are mandatory.

1.2.2 The Mars Sojourner RoverOne robotic vehicle that has recently fired many researchers' imagination

is the NASA Mars Pathfinder Mission's Sojourner Rover which is currentlycarrying out exploration on Mars. The Prospector spacecraft containingthe Rover landed on Mars on July 4th 1997. The Mars Pathfinder Roverteam plans a vehicle traverse from the Rover Control Workstation at NASA(Jet Propulsion Laboratory) in Pasadena, California. Due to the speed oflight time delay from Earth to Mars (11 minutes), and the constramt .ofa single uplink opportunity per day, the Rover is required t.o per~orm. Itsdaily operations autonomously. These activities include terram navigation,rock inspection, terrain mapping and response to contingencies [43J.

During traverses the Rover uses its look ahead sensors (5 laser stripe pro-jectors and two CCD cameras) to detect and avoid rocks, dangerous slopesand drop off hazards, changing its path as needed before turnmg back to-wards its goal. Bumpers, articulation sensors and accelerometers allow theRover to recognize other unsafe conditions. The hazard detection systemcan also be adopted to center the. Rover on a target rock in preparationfor deployment of its spectrometer. Other on-board experiments charac-terize soil mechanics, dust adherence, soil abrasiveness and vehicle traverseperformance. A picture of the Mars Rover is shown in Figure 1.1.

FIGURE 1.1The Mars Sojourner Rover: A Multisensor System. (PhotoCourtesy of NASA)

The capability of the Rover to operate in unmodeled environment, choos-ing actions in response to sensor inputs to accomplish requested objectives,is unique among robotic space missions to date. Being such a complexand dynamic robotic vehicle characterized by a myriad of functions anddifferent types of sensors while operating in an unmodeled and clutteredenvironment, the Sojourner Rover is an excellent example of a multisensorand multiactuator system. Establishing efficient and effective multisensorfusion and control for such a system provides motivation Ifor the materialpresented in this book. How can the vehicle combine and integrate infor-mation obtained from its multiple sensors? How can it optimally and effi-ciently use this information. to control its motion and accomplish its tasks,that is, achieve intelligent connection of perception to action? Currentlythe principal sensor fusion algorithms being used on the Rover are based onstate space methods, in particular, the extended Kalman filter; and thesedata fusion algorithms and their corresponding architectures are ostensiblycentralized [73]. There is also very little modularity in the hardware andsoftware design of the Rover.

Design modularity, decentralized estimation and control provide certainadvantages that would be relevant to the Rover. For example, if eachwheel or unit is monitored and controlled by an independent mechanism,then decentralized sensor processing and local control can permit the Roverto continue its mission even if one or more wheels/units are incapacitated.


FIGURE 1.2The MIT Humanoid Robot (Cog): A Multisensor System.(Photo Courtesy of Donna CoveneyjMIT)

In addition, information from the various sensors will be efficiently utilized,thus, optimally taking advantage of the redundancy inherent in the Rover'smultiple sensors. It is submitted here that if the estimation, control anddesign paradigm proposed in this book is adopted for the Mars SojournerRover, its competence, reliability and survivability could be improved.

1.2.3 The MIT Humanoid Robot (Cog)The principle behind creating the MIT Humanoid Robot (Cog) derives

from the hypothesis that humanoid intelligence requires humanoid interac-tions with the world. The form of the human body is critical to the rep-resentations that are developed and used for both human internal thoughtand language. If a robot with human-like intelligence is to be built, thenit must have a human-like body in order to be able to develop similar rep-resentations. A second reason for building a humanoid form is that animportant aspect of being human is interaction with other humans. Fora human-level intelligent robot to gain experience in interacting with hu-mans it needs a large number of interactions. If the robot has humanoidform then it will be both easy and natural for humans to interact with itin a human-like way. In this way a large source of dynamic interactions isobtained which will not be possible with disembodied human intelligence.Hence, in order to understand human cognition and utilize it in machines,it is necessary to built a humanoid robot [31], [32].

The entire mission of building a humanoid robot would be inconceivablewithout the use of multiple sensors. MIT's Cog is a set of multiple sensorsand multiple actuators which approximate the sensory and motor dynamics?f a human body. The sensory functions include sight (video cameras), hear-mg, touch, proprioception (joint position and torque), a vestibular systemand a vocalization system. Cog's "brain" is a large scale MIMD (multipleinput and multiple data) computer architecture which consists of a set ofMotorolla 68332 processors executing parallel computations. Its head andvisual system is designed such that it approximates the complexities of thehuman visual system and the output is displayed on the rack of twentymonitors. Cog's eye, the camera system, has four degrees of freedom con-sisting of two active "eyes". To mimic human eye movements, each eye canrotate about a vertical axis and a horizontal axis [72], [127].

With such a myriad of multiple sensors in the humanoid robot it isessential that the issue of multisensor. fusion is appropriately addressed sothat the information from the sensors is efficiently and optimally used.

1.2.4 Large Scale SystemsThe problems of monitoring, supervising and controlling large scale sys-

tems also provide a compelling case for the material presented in this book.A large scale system is defined as a group of subsystems that are inter-connected in such a way that decentralized operation is mandatory. Suchs!ste~s h~ve a large number of sensors and actuators, and a large dimen-sI,onalIty (l.e., a large number of states), A large scale system is so physicallydispersed such that a centralized sensor fusion center or controller wouldbe prohibitively expensive. Furthermore, sometimes the system is knownto be weakly coupled so that the degradation in performance resulting fromforced decentralization should be modest.

Systems that can be classified as large scale include the following: anurban traffi~ control system, control of a large paper making plant, an airtraffic control system, control of a large processing plant and a militarycommand control system. Two examples of large scale systems which arealso complex are the Space Shuttle Columbia and the Russian Mir Station.Their main features and functions are described in the next subsections inorder to capture the complexity and extensiveness of such systems thusamply illustrating the case for both decentralized multisensor fusion anddecentralized control.

1.2.5 The Russian Mir Space StationThe Russian Mir Space Station which was launched into space in Febru-

ary 1986 has been in orbit for eleven years, and staffed continuously forthe past six years, It consists of modules launched separately and brought

The Spektr Module carries four solar arrays and scientific equipment,and its main focus is earth observation, specifically natural resources andatmosphere. Kristall module carries scientific equipment, retractable solararrays, and a docking node equipped with a special androgynous dockingmechanism designed to receive a spacecraft weighing up to 100 tons. TheDocking module allows a space shuttle to dock with the Mir station withoutinterfering with the solar arrays. The purpose of the Kristall module is todevelop biological and materials production technologies in the space envi-ronment. Priroda module's primary function is to add earth remote-sensingcapability to Mir and contains the hardware and supplies for several jointU.S.-Russian science experiments. Its earth remote-sensing capabilities in-clude, monitoring the ecology of large industrial areas, measuring concen-tration of gaseous components in the atmosphere, determining temperaturefields on the ocean surface, and monitoring the process of energy and massexchange between ocean and atmosphere which affect the weather.

Clearly, the Mir station is a large, modular and dispersed system whichemploys a huge number of sensors, actuators and controllers to carry outthe functions of its various modules. It is inconceivable and impracticalto consider centralized multisensor fusion or centralized control for such asystem.


FIGURE 1.3The Mir Station: A Complex and Large Scale System. (RussianSpace Agency Photo Courtesy of NASA)

together in space, and it weighs more than one hundred tons. The de-sign philosophy behind the Mir station is that of an assembly of separatepressurized modules with both core and specialized functions. As of Novem-ber 1997 the modular station consists of the Mir core, Kvant 1, Kvant 2,Kristall, 'Spektr, Priroda and Docking modules [100]. Mir measures. morethan 107 feet long and is about 90 feet wide across its modules. A pictureof the station in space is shown in Figure 1.3. . .

The 20.4 ton Core module is the central portion and the first buildingblock of the Mir station which supports the modular design. It providesbasic services (living quarters, life support, power) and sc~entific resear.chcapabilities. Kvant 1 is a small, 11-ton module which contams astrophysicsinstruments, life support and altitude control equipment. The purpos~ ofthe Kvant-1 module is to provide data and observations for research intothe physics of active galaxies, quasars, and neutron stars. The Kvan~-2module which weighs 19.6 tons carries an EVA airlock, solar arrays, lifesupport equipment, drinking water, oxygen provisions, motio~ control S!S-tems, power distribution and washing facilities. Its purpose IS ~~ providebiological research data, earth observation data and EVA capability [100].

1.2.6 The Space Shuttle ColumbiaThe space shuttle Columbia, also referred to as arbiter Vehicle-102, is

the oldest orbiter in the shuttle fleet and was the first U.S.A. space shuttleto fly into earth orbit in 1981. Over the years it has been updated andmodified several times. It has carried out 23 flights and 3,286 orbits, andhas spent a total of 196 days in space [98], [99]. Since 1981 four otherships have joined the fleet; Challenger in 1982 (but destroyed four yearslater), Discovery in 1983, Atlantis in 1985 and Endeavor which was builtas a replacement for Challenger in 1991. The last shuttle mission of 1997,the Space Shuttle Columbia STS-87, was launched into space on the 19thof November from the Kennedy Space Center in Florida, U.S.A. Figure 1.4shows a picture of Columbia blasting off the launch pad into space. In orderto illustrate the complexity of a space shuttle and show the diversity andmultiplicity of its sensors, some of the experiments and instrumentation onthe Columbia STS-87 mission are briefly described here.

The objective of the mission is to carry out several scientific experimentsin space. The United States Microgravity Payload (USMP) is a spacelabconsisting of microgravity research experiments, while the Solar PhysicsSpacecraft (SPS) is to perform remote-sensing of the hot outer layers of thesun's atmosphere. The Space Acceleration Measurement System (SAMS)is a microprocessor-driven data acquisition system designed to measure andrecord the microgravity acceleration environment of the USMP carrier.


FIGURE 1.4The Space Shuttle Columbia: A Complex arid Large ScaleSystem. (Photo Courtesy of NASA)

The Orbital Acceleration Research Experiment (OARE) is a highly sensi-tive instrument designed to acquire and record data of low-level aerody-namic acceleration along the orbiter's principal axes in the free-molecularflow regime at orbital altitudes [99].

The objective of the Shuttle Ozone Limb Sounding Experiment (SOLSE)is to determine the altitude distribution of ozone in an attempt to under-stand its behavior so that quantitative changes in the composition of theatmosphere can be predicted, whereas the Loop Heat Pipe (LHP) test ad-vances thermal energy management technology and validates technol.ogyreadiness for up coming commercial spacecraft applications. The SodiumSulfur Battery Experiment (NaSBE) characterizes the performance of four40 amp-hour sodium-sulfur battery cells. In order to gain an understa~ding of the fundamental characteristics of transitional and turbulent gas J~tdiffusion flames under microgravity conditions, the Turbulent Gas Jet DIf-fusion (G-744) experiment is provided. The Autonomous EVA RoboticCamera (AERC) is a small, unobtrusive, free-flying camera platform ~oruse outside a spacecraft. On board the free-flyer are rate sensors to providedata for an automatic altitude hold capability.

The Shuttle Infrared Leeside Temperature Sensing (SILTS) experimentis used to obtain high-resolution infrared imagery of the upper (leeward)surface of the orbiter fuselage and left wing during atmospheric entry. Thisinformation is hoped to increase understanding of leeside aeroheating phe-

nomena and can be used to design a less conservative thermal protectionsystem. The primary components of the SILTS system include an infraredcamera, infrared-transparent windows, a data and control electronics mod-ule, and a pressurized nitrogen module. Accurate aerodynamic researchrequires precise knowledge of vehicle altitude and state. This information,commonly referred to as air data, includes vehicle angle of attack, angle ofsideslip, free-stream dynamic pressure, Mach number and total pressure.Hence the Shuttle Entry Air Data System (SEADS) was developed to takethe measurements required for precise determination of air data across theorbiter's atmospheric flight-speed range.

The Shuttle Upper Atmosphere Mass Spectrometer (SUMS) experimentis for obtaining measurements of free-stream density during atmosphericentry in the hypersonic, rarefied flow regime. These measurements, com-bined with acceleration measurements from the companion high-resolutionaccelerometer package experiment, allow calculation of orbiter aerodynamiccoefficients in the flow regime previously inaccessible using experimen-tal and analytic techniques. The High Resolution Accelerometer Package(HRAP) experiment uses an orthogonal, triaxial set of sensitive linear ac-celerometers to take accurate measurements of low-level (down to micro-gs)aerodynamic accelerations along the orbiter's principal axes during initialre-entry into the atmosphere, that is, in the rarefied flow regime.

The Orbiter operational instrumentation (01) is used to collect, route andprocess information from transducers and sensors throughout the orbiterand its payloads. This system also interfaces with the solid rocket boost-ers, external tank and ground support equipment. The instrumentationsystem consists of transducers, signal conditioners, two pulse code mod-ulation master units, encoding equipment, two operational recorders, onepayload recorder, master timing equipment and on-board checkout equip-ment. The 01 system senses, acquires, conditions, digitizes, formats anddistributes data for display, telemetry, recording and checkout. The digitalsignal conditioners convert digital and analog data signals from the varioussensors into usable forms. These measured parameters include frequency,voltage, current, pressure, temperature (variable resistance and thermocou-pie), displacement (potentiometer) [98].

The Network Signal Processor (NSP) is the nucleus of the communicationsystems an~ it is responsible for processing and routing commands, teleme-try and VOIce between the orbiter and the ground. The Closed-CircuitTelevision System (CCTV) is used primarily to support on-orbit activitiesthat require visual feedback to the crew. The CCTV system also providesthe capability to document on-orbit activities and vehicle configurationsfor permanent record or for real-time transmission to the ground. Typicaluses of the CCTV monitoring system include payload bay door operationsremote manipulator system operations, experiment operations, rendezvousand station keeping operations, and various on-board crew activities [99].

The CCTV system consists of the video control unit, television cameras,VTR and two on-board television monitors.

From the above descriptions of shuttle experiments and instrumentationit is evident that there is need for decentralized and synergistic integrationof information from sensors in addition to decentralized supervision andcontrol of the different shuttle units.

13

1.4 Approach1.4.1 Estimation

Introduction

The approach adopted is to extend the algebraic equivalent of the Kalmanfilter, the Information filter to problems involving both system and obser-vation nonlinearities. The data fusion problem in nonlinear multisensorsystems is then considered and a decentralized linearized estimation algo-rithm proposed. Considering problems of full connectedness leads to theuse of model distribution methods, where local models involve only rele-vant global states. In such a system, communication is achieved by modeldefined internodal communication. Estimation algorithms for nodes usingreduced order models are thus derived.

1.4.3 ApplicationsThe proposed theory is tested using software written in par-allel ANSI

C running on Transputer based parallel hardware. Some demonstrativesimulations are run using Matlab. Validation is carried out by comparingthe results of the distributed and decentralized systems with correspondingconventional centralized controllers. Application of the theory to control a

The key issue is identified as complementarity between data fusion andcontrol. This is because two distinct but complementary theories of datafusion and control are required to solve the problem stated above. It thenbecomes pertinent to understand the relationship between estimation andsensor based control. The central organizing principle in this book is theseparation of estimation from control. The two are solved as separate butcomplementary subproblems. For linear systems this is justified by theseparation and certainty equivalence principles. In the nonlinear case, thenotion of assumed certainty equivalence is employed. In both cases an op-timal estimator, separately designed, is cascaded with the correspondingoptimal deterministic feedback control gain. Optimal stochastic control fora linear, quadratic and Gaussian (LQG) problem is considered. The opti-mal deterministic control gain is generated from backward Riccati recursionusing the optimality principle and stochastic dynamic programming. Ex-pressing the control law in terms of information estimates, an informationform of the standard LQG controller is derived. A system with several ac-tuators is then configured into a fully connected topology of decentralizedcommunicating control nodes. Control vectors, models and informationvectors are then distributed to resolve issues of full connectedness.

1.4.2 Control

Decentralized Estimation and Control

1.3 Problem StatementThe problem addressed in this book is that of formulating algorit~ms

which obtain globally optimal state estimates and control locally, subjectto the following constraints:

No node acts as a central processing site for fusion or control, andthe size of the system and number of nodes are arbitrary.

Only nodes with a common state space, observed by either or bothnodes, communicate. Any such communicating nodes exchange onlyrelevant information and there is no propagation of information be-tween any two unconnected nodes.

Only locally relevant computation takes place, thus reducing localcomputational requirements.

The observation space and system dynamics are nonlinear.

Optimal here means the estimate or control signal at each node is. equ~valent to that obtained by a corresponding centralized system. Optimalityconcepts are traditionally asserted in the context of centralized ~y~te~s,where the optimization criterion for an estimator is usually the ~mImIzation of the covariance while, for control, it is minimization of a performancecriterion.

In terms of applications the specific motivation is the design of a ~e-centralized sensor fusion and control system for a modular wheeled mobilerobot. This is a robot vehicle system with nonlinear kinematics and withdistributed sources of sensor information.

12


modular Wheeled Modular Robot (WMR) is demonstrated. This is doneby distributing the vehicle kinematics, constructing a vehicle model andthen developing generic software which is the same at each vehicle module.Test runs are carried out for a number of WMR trajectories. The principalgoal is to demonstrate the effectiveness of decentralized WMR estimationand control.

1.5 Principal ContributionsThis book makes a number of theoretical and practical contributions in

the area of decentralized estimation and control for multisensor systemsand large scale systems:

The linear Information filter is generalized and extended to the prob-lem of estimation for nonlinear systems by deriving the extended In-formation filter (ElF). A decentralized form of the algorithm, thedecentralized extended Information filter (DEIF), is also developed,thus, generalizing methods traditionally applied for decentralized es-timation in linear systems to the much larger class of applicationsinvolving nonlinear systems.

Solutions to the generalized model distribution problem in decentral-ized .data fusion and control systems are presented. This allows formodel defined, non-fully connected estimation and control networksbased on internodal information transformation. In these topologiesthere is local internodal communication and no propagation of infor-mation between unconnected nodes. The main advantages of thesenetworks are reduced computation and minimized communication.

Estimation algorithms for systems with different models at each nodeare derived. For linear systems, the distributed and decentralizedInformation filter (DDIF) is developed and for nonlinear systems thedistributed and decentralized extended Information filter (DDEIF)is developed.

Fully decentralized estimation algorithms are applied to the prob-lem of decentralized control for both linear and nonlinear systems.The control algorithms are explicitly expressed in terms of informa-tion. Globally optimal control is obtained locally using reduced ordermodels with minimized communication requirements.

A decentralized kinematic model and modular software for any wheel-ed mobile robot (WMR) with simple wheels is contributed. Generic

software based on Transputer technology is developed which can beloaded onto a vehicle of any kinematic configuration.

The internodal transformation theory provides a formal WMR designmethodology by specifying which vehicle modules have to communi-cate and what information they have to exchange. In this way scalableand efficient WMR configurations can be derived.

The value of the extended Information filter is further enhanced by itsflexibility to work with recently developed techniques for improving theaccuracy and generality of Kalman and extended Kalman filters. Specifi-cally, the Unscented Transform provides a mechanism for applying nonlin-ear transformations to the mean and covariance estimates that is provablymore accurate than standard linearization [60], [62], [105]. The ElF canalso be extended to exploit the generality of Covariance Intersection (Cl)to remove the independence assumptions required by all Kalman-type up-date equations [61], [123], [124]. All results relating to the ElF can be easilyextended to exploit the benefits of the Unscented Transform and Cl.

1.6 Book OutlineThe current chapter, (Chapter 1), provides the background and moti-

vation for the work covered. In Chapter 2 the essential estimation tech-niques used in this book are introduced. These techniques are based on theKalman filter, a widely used data fusion algorithm. The Information filter,an algebraic equivalent of the Kalman filter, is derived. The advantageof this filter in multisensor problems is discussed. For nonlinear systemsthe conventional extended Kalman filter is derived. For use in multisen-sor problems, involving nonlinearities, the extended Information filter isdeveloped by integrating principles from the extended Kalman and linearInformation filters. Examples of estimation in linear and nonlinear systemsare used to validate the Information filter and ElF algorithms with respectto those of the Kalman filter and EKF.

Chapter 3 extends the estimation algorithms of Chapter 2 to fully con-nected, multisensor decentralized estimation problems. An overview of mul-tisensor systems, fusion architectures and data fusion methods is given. Adefinition of a decentralized system is given and the literature in this areais discussed. Decentralized estimation schemes consisting of communicat-ing sensor nodes are then developed by partitioning and decentralizing thestate and information space filters of Chapter 2. In this way four decentral-ized estimation algorithms are derived and compared. The decentralized


extended Information filter (DEIF) is a new result which serves to ad-dress the practical constraint of system nonlinearities. However, all four ofthe decentralized estimation algorithms developed require fully connectednetworks of communicating sensor nodes in order to produce the same esti-mates as their corresponding centralized systems. The problems associatedwith fully connected decentralized systems are discussed.

In Chapter 4 the problems arising from the constraint of fully con-nectedness are resolved by removing it. This is accomplished by using dis-tributed reduced order models at local nodes, where each local state consistsonly of locally relevant states. Information is exchanged by model defined,internodal communication. Generalized internodal transformation theoryis developed for both state space and information space estimators. Thenetwork topology resulting from this work is model defined and non-fullyconnected. Any two unconnected nodes do not have any relevant informa-tion for each other, hence there is no need to propagate information betweenthem. Scalable decentralized estimation algorithms for non-fully connectedtopologies are then derived for both linear and nonlinear systems. The mostuseful algorithm is the distributed and decentralized extended informationfilter (DDEIF). It provides scalable, model distributed, decentralized (lin-earized) estimation for nonlinear systems in terms of information variables.

In Chapter 5 the decentralized estimation algorithms from Chapter 3and 4 are extended to the problem of decentralized control. First, for asingle sensor-actuator system the standard stochastic LQG control prob-lem is solved using information variables. The same approach is used forthe (linearized) nonlinear stochastic control problem. Equipped with theinformation forms of the LQG controller and its nonlinear version, decen-tralized multisensor and multiactuator control systems are then considered.A decentralized algorithm for a fully connected topology of communicatingcontrol nodes is derived from the estimation algorithms of Chapter 3. Theattributes of such a system are discussed. By removing the constraint offully connectedness as discussed in Chapter 4, the problem of scalable de-centralized control is developed. Of most practical value is the distributedand decentralized control algorithm, expressed explicitly in terms of in-formation, which applies to systems with nonlinearities. The advantagesof model defined, non-fully connected control systems are then presented.Simulation examples are also presented.

In Chapter 6 the hardware and software implementation of the the-ory is described. A general decentralized and modular kinematic modelis developed for a WMR with simple wheels. This is combined with thedecentralized control system from Chapter 5 to provide a modular decen-tralized WMR control system. The actual WMR system models used inthe implementation are presented. The modular vehicle used in this work isbriefly introduced and the units of the WMR described. Examples of com-plete assembled vehicle systems are presented to illustrate design flexibility

and scalability. The Transputer based software developed is then outlinedand explained using pseudocode. Software modularity is achieved by usinga unique configuration file and a generic nodal program.

In Chapter 7 the experimental results are presented and analyzed. Thekey objective is to show that given a good centralized estimation or controlalgorithm, an equally good decentralized equivalent can be provided. Thisis done by using results from both simulations and the WMR application.The same performance criteria are used for centralized and decentralizedsystems. For estimation, the innovations sequences are analyzed, while theestimated control errors (from reference trajectories) are used to evaluatecontrol performance. The results are discussed and conclusions drawn.

In Chapter 8 the work described in the book is summarized and futureresearch directions explored. First, the contributions made are summarizedand their importance put into the context of existing decentralized estima-tion and control methods. The limitations of the techniques developed areidentified and possible solutions advanced. Research fields and applicationsto which the work can be extended are proposed.

Chapter 2Estimation and Information Space

2.1 IntroductionIn this chapter the principles and concepts of estimation used in this

book are introduced. General recursive estimation and, in particular, theK alman filter is discussed. A Bayesian approach to probabilistic informa-tion fusion is outlined. The notion and measures of information are defined.This leads to. the derivation of the algebraic equivalent of the Kalman fil-ter, the (linear) Information filter. The characteristics of this filter and theadvantages of information space estimation are discussed. State estima-tion for systems with nonlinearities is considered and the extended Kalmanfilter treated. Linear information space is then extended to nonlinear infor-mation space by deriving the extended Information filter. This establishesall the necessary mathematical tools required for exhaustive informationspace estimation. The advantages of the extended Information filter overthe extended Kalman filter are presented and demonstrated. This filterconstitutes an original contribution to estimation theory and forms the ba-sis of the decentralized estimation and control methods developed in thisbook.

2.2 The Kalman FilterAll data fusion problems involve an estimation process. An estimator

is a decision rule which takes as an argument a sequence of observationsand computes a value for the parameter or state of interest. The Kalmanfilter is a recursive linear estimator which successively calculates a mini-mum variance estimate for a state, that evolves over time, on the basis ofperiodic observations that are linearly related to this state. The Kalman

19

20 Decentralized Estimation and Control Estimation and Information Space 21

filter estimator minimizes the mean squared estimation error and is optimalwith respect to a variety of important criteria under specific assumptionsabout process and observation noise. The development of linear estimatorscan be extended to the problem of estimation for nonlinear systems. TheKalman filter has found extensive applications in such fields as aerospacenavigation, robotics and process control.

2.2.1 System DescriptionA very specific notation is adopted to describe systems throughout this

book [12]. The state of nature is described by an n-dimensional vectorX=[Xl,X2, ..., xn]T. Measurements or observations are made of the state ofx. These are described by an m-dimensional observation vector z.

A linear discrete time system is described as follows:

x(k) = F(k)x(k - 1) + B(k)u(k - 1) + w(k - 1), (2.1)where x(k) is the state of interest at time k, F(k) is the state transition ma-trix from time (k-1) to k, while u(k) and B(k) are the input control vectorand matrix, respectively. The vector, w(k) rv N(O, Q(k)) is the associatedprocess noise modeled as an uncorrelated, zero mean, white sequence withprocess noise covariance,

INITIALIZE

II1

PREDICTION ~....-

'I,

OBSERVATION

I

ESTIMATION

x(i I j) = E [x(i) Iz(l), z(j)] .

E[v(i)vT(j)] = 8ijR(i).It is assumed that the process and observation noises are uncorrelated, Le.,

E[v(i)wT (j)] = 0.The notation due to Barshalom [12] is used to denote the estimate of the

state x(j) at time i given information up to and including time j by

2.2.2 Kalman Filter Algorithm

FIGURE 2.1Kalman Filter Stages

A great deal has been written about the Kalman filter and estimationtheory in general [12], [13], [74]. An outline of the Kalman filter algorithmis presented here without derivation. Figure 2.1 summarizes its main func-tional stages. For a system described by Equation 2.1 and being observedaccording to Equation 2.2, the Kalman filter provides a recursive estimate

-, x(k I k) for the state x(k) at time k given all information up to time k interms of the predicted state x(k Ik - 1) and the new observation z(k) [41].The one-step-ahead prediction, x(k I k - 1), is the estimate of the state ata time k given only information up to time (k - 1). The Kalman filteralgorithm may be summarized in two stages:

(2.2)z(k) = H(k)x(k) + v(k),

This is the conditional mean, the minimum mean square error estimate.This estimate has a corresponding variance given by

P(i I j) = E [(x(i) - x(i I j)) (x(i) - x(i I j))T I z(l)," .z(j)]. (2.3)

The system is observed according to the linear discrete equation

where z(k) is the vector of observations made at time k. H(k) is the obser-vation matrix or model and v(k) rv N(O, R(k)) is the associated observationnoise modeled as an uncorrelated white sequence with measurement noisecovariance,

22 Decentralized Estimation and ControlEstimation and Information Space 23

(2.11)

(2.12)

(2.14)

(2.13)

(2.10)

p(x, z) = p(xlz)p(z)=p(zlx)p(x)

{:} p(xlz) = p(zlx)p(x)p(z)

k b. {Z = z(l), z(2), ..., z(k)}.

p(xIZ k ) = p(z(k)lx)p(xIZk - 1 )p(z(k)IZk - 1 ) .

The corresponding likelihood function is given by

where p(z) is the marginal distribution.In order to reduce uncertainty several measurements may be taken over

time before constructing the posterior. The set of all observations up totime k is defined as

In this recursive form there is no need to store all the observations. Onlythe current observation z(k) at step k is considered. This recursive defini-tion has reduced memory requirements and hence it is the most commonlyimplemented form of Bayes theorem.

This is a measure of how "likely" a parameter value x is, given that allthe observations in Zk are made. Thus the likelihood function serves as ameasure of evidence from data. The posterior distribution of x given theset of observations Zk is now computed as .

This leads to the formulation of the likelihood principle which states that,all that is known about the unknown state is what is obtained throughexperimentation. Thus the likelihood function contains all the informationneeded to construct an estimate for x. However, the likelihood functiondoes not give the complete picture, if before measurement, informationabout the state x is made available exogenously. Such a priori informationabout the state is encapsulated in the prior distribution function p(x) andis regarded as subjective because it is not based on any observed data. Howsuch prior information and the likelihood information interact to provide aposteriori (combined prior and observed) information, is solved by Bayestheorem which gives the posterior conditional distribution of x given z,

It can also be computed recursively after each observation z(k) as follows:

(2.8)

(2.9)W (k) = P (k I k - 1)HT (k )S-1 (k ),

S(k) = H(k)P(k I k - l)HT(k) + R(k).

The Information filter is essentially a Kalman filter expressed in termsof measures of information about the parameters (states) of interest ratherthan direct state estimates and their associated covariances [47]. This filterhas also been called the inverse covariance form of the Kalman filter [13],[74]. In this section, the contextual meaning of information is explainedand the Information filter is derived.

2.3 The Information Filter

x(k I k) = [1 - W(k)H(k)] x(k I k - 1) + W(k)z(k) (2.6)P(k I k) = P(kl k - 1) - W(k)S(k)WT(k), (2.7)

x(k I k - 1) = F(k)x(k - 1 I k - 1) + B(k)u(k) (2.4)P(k I k - 1) = F(k)P(k -1 I k -l)FT(k) + Q(k). (2.5)

2.3.1 Information SpaceBayesian Theory

The probabilistic information contained in z about x is described by theprobability distribution function, p(zlx), known as the likelihood function.Such information is considered objective because it is based on observations.The likelihood function contains all the relevant information from the ob-servation z required in order to make inferences about the true state x.

The matrix 1 represents the identity matrix. From Equation 2.6, theKalman filter state estimate can be interpreted as a linear weighted sum ofthe state prediction and observation. The weights in this averaging processare {I - W(k)H(k)} associated with the prediction and W(k) associatedwith the observation. The values of the weights depend on the balance ofconfidence in prediction and observation as specified by the process andobservation noise covariances.

where W(k) and S(k) known as the gain and innovation covariance matri-ces, respectively, are given by

Estimation

Prediction


By considering Sk(X) as a random variable, its mean is obtained from

For a non-random state x the expression of the Fisher information matrixbecomes

(2.20)_(2.21)

:J(k) = -E [\7x\7;ln p(x(k)IZk)]

_ [ T { [x(k)-x(k I k)] Tp-1(k I k) [x(k)-x(k I k)] }]- E \7 x \7z 2 . + In A

_ E [ T ([X(k)-X(k I k)]Tp-l(k I k) [x(k)-x(k I k)])]- \7x\7x 2

= E [P-l(k I k) {[x(k) - x(k I k)] [x(k) - x(k I k)]T} P-l(k I k)]= P-l(k I k)P(k I k)P-l(k I k)= P-l(k Ik)= (CRLB)-l.

where A = Vdet(27fP(k I k)). Substituting this distribution Into Equation2.17 leads to

Thus, assuming Gaussian noise and minimum mean squared error estima-tion, the Fisher information matrix is equal to the inverse of the covariancematrix.

This information matrix is central to the filtering techniques employedin this book. Although the filter constructed from this information spaceis algebraically equivalent to the Kalman filter; it has been shown to haveadvantages over the Kalman filter in multisensor data fusion applications.These include reduced computation, algorithmic simplicity and easy initial-ization. In particular, these attributes make the information filter easier todecouple, decentralize and distribute. These are important filter character-istics in multisensor data fusion systems.

sense is that there must be an increasing amount of information (in thesense of Fisher) about the parameter in the measurements, i.e., the Fisherinformation has to tend to infinity as k -+ 00. The CRLB then convergesto zero as k -+ 00 and thus the variance can also converge to zero. Fur-thermore, if an estimator's variance is equal to the CRLB, then such anestimator is called efficient.

Consider the expression for the Fisher information matrix in Equations2.16 or 2.17. In the particular case where the likelihood function, Ak(x),is Gaussian, it can be shown that the Fisher information matrix, :J(k), isequal to the inverse of the covariance matrix P(k I k), that is, the CRLB isthe covariance matrix. This is done by considering the probability distribu-tion function of a Gaussian random vector x(k) whose mean and associatedcovariance matrix are x(k I k) and P(k I k), respectively. In particular,p(x(k)jZk) .AI (x(k), x(k I k), P(k I k))

~ 1 {[X(k) - x(k I k)]T P-1(k I k) [x(k) - x(k Ik)]}-A exp 2 '

(2.19)

(2.18)

(2.17)

(2.16)

(2.15)

:J(k) = -E [\7z \7;ln p(Zk Ix)].

E[{x(k) - x(k I k)}{x(k) - x(k I k)}TIZ k] ~ :J- 1 (k).

The notion of Fisher information is useful in estimation and control.It is consistent with information in the sense of the Cramer-Rao lowerbound (CRLB) [13]. According to the CRLB, the mean squared errorcorresponding to the estimator of a parameter cannot be smaller than acertain quantity related to the likelihood function. Thus the CRLB boundsthe mean squared error vector of any unbiased estimator x( k I k) for a statevector x(k) modeled as random.

f \7xp(Zk,x) (k )dE[Sk(X)] = p(Zk,x) P Z ,x z= \7xf p(Zk, x)dz = O.

The Fisher information matrix :J(k) is then defined as the covariance ofthe score function,

Expressing this result as the negative expectation of the Hessian of thelog-likelihood gives

In this way the covariance matrix of an unbiased estimator is bounded frombelow. It follows from Equation 2.19 that the CRLB is the inverse of theFisher information matrix, :J(k). This is a very important relationship. Anecessary condition for an estimator to be consistent in the mean square

Measures of InformationThe term information is employed in the Fisher sense, that is, a measure

of the amount of information about a random state x present in the set ofobservations z, up to time k. The score function, Sk(X), is defined as thegradient of the log-likelihood function,

b. k _ \7xp(Zk,x)Sk(X) = \7xln p(Z , x) - p(Zk, x) .


P-1(k I k)x(k I k) = P-1(k I k - l)x(k I k - 1) + HT(k)R-1(k)z(k),

The information state vector is a product of the inverse of the covariancematrix (information matrix) and the state estimate,

(2.37)

(2.35)

(2.36)

(2.32)

(2.33)

i(k) ~ HT(k)R-1(k)z(k),I(k) ~ HT(k)R-1(k)H(k).

y(k I k) = y(k I k - 1) + i(k)

y(k I k - 1) = L(k I k - l)y(k - 1 I k - 1)Y(k I k - 1) = [F(k)y-1(k - 11 k - l)FT(k) + Q(k)] -1 .

Estimation

Prediction

L(k 1k - 1) = Y(k I k - 1)F(k)y-1(k - 1 I k ~ 1). (2.34)

The information propagation coefficient L(k I k - 1), which is independentof the observations made, is given by the expression

P(k I k) = [1 - W(k)H(k)] P(k I k - 1)[1 - W(k)H(k)]T+W(k)R(k)WT(k). (2.28)

y(k I k) = y(k I k - 1) + HT(k)R-1(k)z(k). (2.27)

P(k I k) = [P(k I k)P-1(k I k -1)] P(k I k - 1) [P(k I k)P-1(k I k - l)]T+ [P(k I k)HT(k)R-1(k)] R(k) [P(k I k)HT(k)R-1(k)]T. (2.29)

With these information quantities well defined, the linear Kalman filtercan now be written in terms of the information state vector and the infor-mation matrix.

P-1(k I k) = P-1(k I k - 1) + HT(k)R-1(k)H(k) (2.30)

Y(k I k) = Y(k I k - 1) .'f- HT(k)R-1(k)H(k). (2.31)The information state contribution i(k) from an observation z(k), and itsassociated information matrix I(k) are defined, respectively, as follows:

or

Pre- and post-multiplying by P-1(k I k) then simplifying gives the infor-mation matrix update equation as

Substituting in Equations 2.24 and 2.26 gives

A similar expression can be found for the information matrix associatedwith this estimate. From Equations 2.7, 2.8 and 2.24 it follows that

or

(2.26) ,

(2.23)

(2.22)Y(i I j) ~ P-1(i I j).

y(i I j) ~ P-1(i I j)x(i I j)= Y(i I j)x(i I j)

Substituting Equation 2.24 into Equation 2.25 gives

W(k) = P(k Ik)HT(k)R-1(k).

Substituting the expression of the innovation covariance S(k), given inEquation 2.9, into the expression of the filter gain matrix W(k), fromEquation 2.8 gives

W(k) = P(k I k -l)HT(k)[H(k)P(k I k - l)HT(k) + R(k)]-l{::}W(k)[H(k)P(k I k - l)HT(k) + R(k)] = P(k I k - l)HT(k)

{::} W(k)R(k) = [1 - W(k)H(k)]P(k I k - l)HT(k)

{::}W(k) = [1- W(k)H(k)]P(k I k - 1)HT(k)R-1(k). (2.25)

Substituting Equations 2.24 and 2.26 into Equation 2.6 and pre-multiplyingthrough by P-1(k I k) gives the update equation for the information statevector as

The variables, Y(i I j) and y(i I j), form the basis of the information spaceideas which are central to the material presented in this book.

The Information filter is derived from the Kalman filter algorithm bypost-multiplying the term {I - W(k)H(k)} from Equation 2.6, by the term{P(k I k - 1)P-1 (k I k - I)} (Le., post-multiplication by the identity ma-trix 1),

1 _ W(k)H(k) = [P(k I k - 1) - W(k)H(k)P(k I k - 1)]p-1 (k I k - 1)= [P(k I k - 1) - W(k)S(k)S-l(k)H(k)P(k I k - 1)] x

P-1(k I k -1)= [P(k I k - 1) - W(k)S(k)WT(k)] P-1(k I k -1)=P(k I k)P-1(k I k -1). (2.24)

2.3.2 Information Filter DerivationThe two key information-analytic variables are the information matrix

and information state vector. The information matrix has already beenderived above (Section 2.3.1) as the inverse of the covariance matrix,


where 1 is an identity matrix and A is given by

F(k) == eA A T ~ 1 + fJ.TA

(2.39)

Q(k) =

[ 1 000]H(k) = 0100 .In order to complete the construction of models, the measurement error

covariance matrix R(k) and the process noise Q(k) are then obtained asfollows:

R(k) = [a~eas_nOise 2 0 ] ,o ameae.sioise

[

0 0 1 0]0001A= 0000 .

0000

Since only linear measurements of the two target positions are taken, theobservation matrix is given by

[

1 0 fJ.T 0 ]_ 0 1 0 fJ.T- 0 0 1 0 '

o 0 0 1

x(k) = F(k)x(k - 1) + w(k - 1), (2.40)where F(k) is the state transition matrix. This matrix is obtained by theSeries method as follows:

x(k) = [;~~~l] = [~~~] .Xl (k) VIX2(k) V2

The objective is to estimate the entire state vector x(k) in Equation 2.39after obtaining observations of the two target positions, Xl (k) and x2(k).

The discrete time state equation with sampling interval fJ.T is given by

is considered. Consider two targets moving with two different but constantvelocities, VI and V2. The state vector describing their true positions andvelocities can be represented as follows:

By comparing the implementation requirements and performance of theKalman and Information filters, a number of attractive features of the latterare identified:

The information estimation Equations 2.37 and 2.38 are computation-ally simpler than the state estimation Equations 2.6 and 2.7. Thiscan be exploited in partitioning these equations for decentralized mul-tisensor estimation.

2.3.4 An Example of Linear EstimationTo compare the Kalman and the Information filter and illustrate the

issues discussed above, the following example of a linear estimation problem

Although the information prediction Equations 2.35 and 2.36 are morecomplex than Equations 2.4 and 2.5, prediction depends on a propa-gation coefficient which is independent of the observations. It is thusagain easy to decouple and decentralize.

There are no gain or innovation covariance matrices and the max-imum dimension of a matrix to be inverted is the state dimension.In multisensor systems the state dimension is generally smaller thanthe observation dimension, hence it is preferable to employ the In-formation filter and invert smaller information matrices than use theKalman filter and invert larger innovation covariance matrices.

Initializing the Information filter is much easier than for the Kalmanfilter. This is because information estimates (matrix and state) areeasily initialized to zero information. However, in order to imple-ment the Information filter, a start-up procedure is required wherethe information matrix is set with small non-zero diagonal elementsto make it invertible.

These characteristics are useful in the development of decentralized datafusion and control systems. Consequently, this book employs informationspace estimation as the principal filtering technique.

2.3.3 Filter Characteristics

Y(k I k) = Y(k I k - 1) + I(k). (2.38)This is the information form of the Kalman filter [46], (87], [48]. Despite

its potential applications, it is not widely used and it is thinly coveredin literature. Barshalom [13] and Maybeck [74] briefly discuss the idea ofinformation estimation, but do not explicitly derive the algorithm in termsof information as done above, nor do they use it as a principal filteringmethod.


70

c::

~ 60Q)


5r---,---.-------r----r-------.---.---.,..------r---~-~ 1.5 X 10.14

5045403520 25 30Time in [8]

15105_1.50~-~--...L..--.,........L---L---Jl...-----l---.l-------L--...L..---l

c::o1aE'~. 0.5(iiE

~ol:l 0c::ctIE~

~ -0.5I.0

.E -1

5045403520 25 30Time in [8]

15105

4

-4

-1

c::o1 0c::E

-3

-2

FIGURE 2.4Innovations for the Kalman and Information Filters

mean with variance S(k). Practically, it means the noise level in the filteris of the same order as the true system noise. There is no visible correla-tion of the innovations sequences. This implies that there are no significanthigher-order unmodeled dynamics nor excessive observation noise to pro-cess noise ratio. The innovations also satisfy the 95% confidence rule. Thisimplies that the filters are consistent and well-matched. Since the curvesin Figures 2.2, 2.3 and 2.4 look indistinguishable for the two filters, it isprudent to plot parameter differences between the filters to confirm thealgebraic equivalence. Figure 2.5 shows the difference between the state es-timates for the filters. The difference is very small (lies within 10-13 %) andhence attributable to numerical and computational errors such as trunca-tion and rounding off errors. Thus the Kalman and Information filters aredemonstrably equivalent. This confirms the algebraic equivalence which ismathematically proven and established in the derivation of the Informationfilter from the Kalman filter.

FIGURE 2.5The Difference between Kahnan and Information Filters' StateEstimates

2.4 The Extended Kalman Filter (EKF)In almost all real data fusion problems the state or environment of interest

does not evolve linearly. Consequently simple linear models will not beadequate to describe the system. Furthermore, the .sensor observationsmay not depend linearly on the states that describe the environment. Apopular approach to solve nonlinear estimation problems has been to usethe extended Kalman filter (EKF) [12], [24]. This is a linear estimatorfor a nonlinear system obtained by linearization of the nonlinear state andobservations equations. For any nonlinear system the EKF is the bestlinear, unbiased, estimator with respect to minimum mean squared errorcriteria.

The EKF is conceptually simple and its derivation follows from argu-ments of linearization and the Kalman filter algorithm. The difficulty arisesin implementation. It can be made to work well, but may perform badly oreven become unstable with diverging estimates. This is most often due tolack of careful modeling of sensors and environment. Failure to understandthe limitations of the algorithm exacerbates the problem.


with the corresponding covariances being given by

It is also assumed that the process and observation noises are uncorrelated, (2.47)

(2.46)

(2.44)

x(i I j) ~ x(i) - x(i I j),

P(i I j) ~ E [x(i I j)xT(i 1j) I Zj].

x(k) = f(x(k - 1 I k - 1), u(k - 1), (k - 1))+V'fx(k) [x(k - 1) - x(k - 1 Ik - 1)]+0 ([x(k - 1) - x(k - 1 Ik - 1)]2) + w(k)

This follows from the assumption that the estimate x(k - 1 Ik - 1) is ap-proximately equal to the conditional mean (Equation 2.43) and that theprocess noise w(k) has zero mean. The state estimate error at a time igiven all observations up to time j is defined as

and the state covariance is defined as the outer product of this error withitself conditioned on the observations made

x(k I k -1) = E [x(k) IZk-l]~ E [f(x(k - 11 k - 1) + A + w(k) IZk-l, u(k - 1), (k - 1))]

(where A = V'fx(k) [x(k - 1) - x(k - 11 k - 1)])= f(x(k - 1\ k - 1), u(k -1), (k - 1)). (2.45)

x(k \ k - 1)= x(k) -x(k \ k-1)= f(x(k - 1 I k - 1), u(k), k) + V'fx(k) [x(k - 1) - x(k - 1 \ k - 1)]

+0 ([x(k - 1) - x(k - 1 Ik - 1)]2) + w(k)-f(x(k - 1 Ik - 1), u(k), k)~ V'fx(k) [x(k - 1) - x(k - 1 Ik - 1)] + w(k)=V'fx(k)x(k - 1 I k - 1) + w(k). (2.48)

In particular, the prediction error x(k I k - 1) can be found by subtract-ing the true state x(k) given in Equation 2.44 from the prediction given inEquation 2.45 -

where V'fx(k) is the Jacobian of fevaluated at x(k -1) = x(k - 11 k -1).Truncating Equation 2.44 at first order, and taking expectations con-

ditioned on the first (k - 1) observations gives an equation for the stateprediction.

Equation 2.41 as a Taylor series about the estimate x(k - 1 I k - 1), thefollowing expression is obtained.

(2.43)

(2.42)

(2.41)

Vi,j.

z(k) = h (x(k), k) + v(k),

E[w(i)vT(j)] = 0,

x(k - 1 I k - 1) ~ E[x(k - 1) I Zk-l].

x(k) = f (x(k - 1), u(k - 1), (k - 1)) + w(k),

Le.,

2.4.2 EKF DerivationThe derivation of the EKF follows from that of the linear Kalman filter,

by linearizing state and observation models using Taylor's series expansion[13], [74].

E[w(k)] = E[v(k)] = 0, Vk,

where z(k) is the observation made at time k, x(k) is the state at time k,v(k) is some additive observation noise, and hf-, k) is a nonlinear observa-tion model mapping current state to observations.

It is assumed that both noise vectors v(k) and w(k) are linearly additiveGaussian, temporally uncorrelated with zero mean, which means

where x(k - 1) is the state vector and u(k - 1) is a known input vector,both at time (k -1). The vectors x(k) and w(k) represent the state vectorand some additive process noise vector, respectively, both at time-step k.The nonlinear function f(,, k) is the nonlinear state transition functionmapping the previous state and current control input to the current state. Itis assumed that observations of the state of this system are made accordingto a nonlinear observation equation of the form

State PredictionIt is assumed that there exists an estimate at time (k - 1) which is

approximately equal to the conditional mean,

2.4.1 Nonlinear State SpaceThe system of interest is described by a nonlinear discrete-time state

transition equation in the form

The objective is to find a prediction x(k I k - 1) for the state at the nexttime k based only on the information available up to time (k-1). Expanding

The prediction is unbiased when the previous estimate is unbiased and thecondition that the noise sequences are zero mean and white hold.


Taking expectations conditioned on the observations made up to time(k - 1) of the outer product of the prediction error gives an expressionfor the prediction covariance in terms of the covariance of the previousestimate.

The innovation covariance can now be found from the mean squarederror in the predicted observation. The error in the predicted observationcan be approximated by subtracting this prediction from the Taylors seriesexpansion of the observation in Equation 2.50 as

where the last two lines follow from the fact that the state prediction errorand the observation noise both have zero mean. After taking an observationz(k), the innovation can be found by subtracting the predicted observationas

The last two lines follow from the fact that the estimate and true state attime (k - 1) are statistically dependent only on the noise terms v(j) andw(j), j ::; (k - 1). Hence, by assumption, they are uncorrelated with thecurrent process noise w(k).

(2.55)x(k I k) = x(k I k - 1) + W(k) [z(k) - h(x(k Ik -1))] .

z(k Ik - 1) f:::" z(k) - z(k I k - 1)h (x(kl k - 1)) + V'hx(k) [x(k I k - 1) - x(k)]+0 ([x(k I k - 1) - X(k)]2) + v(k)-h (x(k I k - 1))~ V'hx(k) [x(k I k - 1) - x(k)] + v(k). (2.53)

S(k) = E [z(k I k -1)zT(k I k -1)]= E [AAT ](where A = (V'hx(k) [x(k I k - 1) - x(k)] + v(k)))= V'hx(k)P(k I k - 1)V'hI(k) + R(k). (2.54)

The gain matrix W (k) is chosen so as to minimize conditional mean squaredestimation error. This error is equal to the trace of the estimate covarianceP(k I k) which itself is simply the expected value of the state error x(k I k)squared.

Update EquationsEquipped with the prediction and innovation equations, a linearized es-

timator can be proposed. It gives a state estimate vector x(k I k) for thestate vector x( k) which is described by the nonlinear state transition ofEquation 2.41 and which is being observed according to the nonlinear ob-servation Equation 2.42. It is assumed that a prediction x(k I k - 1) for thestate at time k has been made on the basis of the first (k - 1) observationsZk-l according to Equation 2.45. The current observation is z(k). Theestimator is assumed to be in the form of a linear unbiased average of theprediction and innovation so that,

This follows from the fact that the state prediction is dependent only onthe noise terms v(j) and w(j), j ::; (k - 1). Consequently, by assumption,it is statistically uncorrelated with the current observation noise v(k).

A clear distinction is made between the 'estimated' observation errorz(k I k - 1) and the measured observation error, the innovation, v(k). Squa-ring the expression for the estimated observation error and taking expecta-tion conditions on the first (k - 1) measurements gives an equation for theinnovation covariance.

(2.52)

(2.50)

v(k) = z(k) - h (x(k I k - 1)) .

z(k) = h (x(k)) + v(k)= h (x(k I k - 1)) + V'hx(k) [x(k I k - 1) - x(k)] +

o ([x(k Ik - 1) - x(k)]2) + v(k)where V'hx(k) is the Jacobian of h evaluated at x(k) = x(k I k - 1). Again,ignoring second and higher order terms and taking expectations conditionedon the first (k - 1) observations gives an equation for the predicted obser-vation.

z(k I k - 1)f:::" E [z(k) IZk-l]~ E [h (x(k Ik - 1)) + V'hx(k) [x(k Ik - 1) - x(k)] + v(k) I Zk-l]

h (x(k I k - 1)) , (2.51)

P(k I k - 1) ~ E [x{k I k - l)xT(k Ik - 1) I Zk-l]~ E [(V'fx(k)x(k -11 k - 1) + w(k)) A I Zk-l](where A = (V'fx(k)x(k - 1 I k - 1) + w(k))T)=V'fx(k)E [x(k - 1 I k - l)xT(k - 1 I k - 1) IZk-l] V'f; (k)+E [w(k)wT(k)]=V'fx(k)P(k - 1 I k - 1)V'f;(k) +Q(k). (2.49)

Observation Prediction and InnovationThe next objective is to obtain a predicted observation and itscorre-

sponding innovation to be used in updating the predicted state. This isachieved by expanding Equation 2.42, describing the observations made, asa Taylor series about the state prediction x(k I k - 1).


2.4.3 Summary of the EKF Algorithm

From Equation 2.55 and the approximate observation error given inEquation 2.53, the state error becomes

(2.65)(2.66)

(2.63)(2.64)

(2.62)

W(k) = P(k I k - l)V'hxT(k)S-1(k)S(k) = V'hx(k)P(k I k - l)V'hxT(k) + R(k).

x(k I k) = x(k I k - 1) + W(k) [z(k) - h(x(k I k - 1))]P(k I k) = P(k I k - 1) - W(k)S(k)WT(k).

P(k I k - 1) = V'fx(k)P(k - 1 I k - l)V'fxT(k) + Q(k - 1).

2.5 The Extended Information Filter (ElF)2.5.1 Nonlinear Information Space

In this section the linear Information filter is extended to a linearized es-timation algorithm for nonlinear systems. The general approach is to applythe principles of the EKF and those of the linear Information filter in orderto construct a new estimation method for nonlinear systems. This gener-ates a filter that predicts and estimates information about nonlinear stateparameters given nonlinear observations and nonlinear system dynamics.

The gain and innovation covariance matrices are given by

Estimation

The Jacobians V'fx(k) and V'hx(k) are typically not constant, beingfunctions of both the state and time-step. It is clearly evident that theEKF is very similar to the Kalman filter algorithm, with the substitutionsF ~ V'fx(k) and H ~ V'hx(k) being made in the equations for the varianceand gain propagation.

It is prudent to note a number of problematic issues specific to the EKF.Unlike the linear filter, the covariances and gain matrix mustbe computedon-line as estimates and predictions are made available, and will not, ingeneral, tend to constant values. This significantly increases the amount ofcomputation which must be performed on-line by the algorithm. Also, ifthe nominal (predicted) trajectory is too far away from the true trajectory,then the true covariance will be much larger than the estimated covarianceand the filter will become poorly matched. This might lead to severe filterinstabilities. Last, the EKF employs a linearized model which must becomputed from an approximate knowledge of the state. Unlike the linearalgorithm, this means that the filter must be accurately initialized at thestart of operation to ensure that the linearized models obtained are valid.All these issues must be taken into account in order to achieve acceptableperformance for the EKF.

(2.61)x(k I