Kalman filtering: Theory and practice using MATLAB

589 Pages·2008·3.73 MB·English

by  Mohinder S. Grewal

KALMAN FILTERING
KALMAN FILTERING
w
Theory and Practice Using MATLAB
Third Edition
MOHINDER S. GREWAL
California State Universityat Fullerton
ANGUS P. ANDREWS
Rockwell Science Center (retired)
Copyright#2008byJohnWiley&Sons,Inc.Allrightsreserved
PublishedbyJohnWiley&Sons,Inc.,Hoboken,NewJersey
PublishedsimultaneouslyinCanada
Nopartofthispublicationmaybereproduced,storedinaretrievalsystem,ortransmittedinanyformorby
anymeans,electronic,mechanical,photocopying,recording,scanning,orotherwise,exceptaspermitted
under Section 107 or 108 of the 1976 United States Copyright Act, without either the prior written
permission of the Publisher, or authorization through payment of the appropriate per-copy fee to the
Copyright Clearance Center, Inc., 222 Rosewood Drive, Danvers, MA 01923, [phone], fax
[phone],oronthewebatwww.copyright.com.RequeststothePublisherforpermissionshould
beaddressedtothePermissionsDepartment,JohnWiley&Sons,Inc.,111RiverStreet,Hoboken,NJ
07030,[phone],fax[phone],oronlineathttp://www.wiley.com/go/permission.
LimitofLiability/DisclaimerofWarranty:Whilethepublisherandauthorhaveusedtheirbesteffortsin
preparingthisbook,theymakenorepresentationsorwarrantieswithrespecttotheaccuracyorcomplete-
nessofthecontentsofthisbookandspecificallydisclaimanyimpliedwarrantiesofmerchantabilityor
fitness for a particular purpose. No warranty may be created or extended by sales representatives or
writtensalesmaterials.Theadviceandstrategiescontainedhereinmaynotbesuitableforyoursituation.
Youshouldconsultwithaprofessionalwhereappropriate.Neitherthepublishernorauthorshallbeliable
foranylossofprofitoranyothercommercialdamages,includingbutnotlimitedtospecial,incidental,
consequential,orotherdamages.
Forgeneralinformationonourotherproductsandservicesorfortechnicalsupport,pleasecontactour
Customer Care Department within the United States at [phone], outside the United States at
[phone]orfax(317)572-4002.
Wileyalsopublishesitsbooksinavarietyofelectronicformats.Somecontentthatappearsinprintmay
notbeavailableinelectronicformat.FormoreinformationaboutWileyproducts,visitourwebsiteat
www.wiley.com.
LibraryofCongressCataloging-in-PublicationData:
Grewal,MohinderS.
Kalmanfiltering : theoryandpracticeusingMATLAB/MohinderS.Grewal,
AngusP.Andrews.—3rded.
p.cm.
Includesbibliographicalreferencesandindex.
ISBN978-0-470-17366-4(cloth)
1. Kalmanfiltering. 2. MATLAB. I. Andrews,AngusP. II. Title.
QA402.3.G6952008
629.8’312—dc22
200803733
PrintedintheUnitedStatesofAmerica
10 9 8 7 6 5 4 3 2 1
CONTENTS
Preface ix
Acknowledgments xiii
List of Abbreviations xv
1 General Information 1
1.1 On Kalman Filtering, 1
1.2 On Optimal Estimation Methods, 5
1.3 On the Notation Used In This Book, 23
1.4 Summary, 25
Problems, 26
2 Linear Dynamic Systems 31
2.1 Chapter Focus, 31
2.2 Dynamic System Models, 36
2.3 Continuous Linear Systems and Their Solutions, 40
2.4 Discrete Linear Systems and Their Solutions, 53
2.5 Observability of Linear Dynamic System Models, 55
2.6 Summary, 61
Problems, 64
3 Random Processes and Stochastic Systems 67
3.1 Chapter Focus, 67
3.2 Probabilityand Random Variables (RVs), 70
3.3 Statistical Properties of RVs, 78
v
vi CONTENTS
3.4 Statistical Properties of Random Processes (RPs), 80
3.5 Linear RP Models, 88
3.6 Shaping Filters and State Augmentation, 95
3.7 Mean and Covariance Propagation, 99
3.8 Relationships Between Model Parameters, 105
3.9 Orthogonality Principle, 114
3.10 Summary, 118
Problems, 121
4 Linear Optimal Filters and Predictors 131
4.1 Chapter Focus, 131
4.2 Kalman Filter, 133
4.3 Kalman–Bucy Filter, 144
4.4 Optimal Linear Predictors, 146
4.5 Correlated Noise Sources, 147
4.6 Relationships Between Kalman–Bucyand Wiener Filters, 148
4.7 Quadratic Loss Functions, 149
4.8 Matrix Riccati Differential Equation, 151
4.9 Matrix Riccati Equation In Discrete Time, 165
4.10 Model Equations for Transformed State Variables, 170
4.11 Application of Kalman Filters, 172
4.12 Summary, 177
Problems, 179
5 Optimal Smoothers 183
5.1 Chapter Focus, 183
5.2 Fixed-Interval Smoothing, 189
5.3 Fixed-Lag Smoothing, 200
5.4 Fixed-Point Smoothing, 213
5.5 Summary, 220
Problems, 221
6 Implementation Methods 225
6.1 Chapter Focus, 225
6.2 Computer Roundoff, 227
6.3 Effects of Roundoff Errors on Kalman Filters, 232
6.4 Factorization Methods for Square-Root Filtering, 238
6.5 Square-Root and UD Filters, 261
6.6 Other Implementation Methods, 275
6.7 Summary, 288
Problems, 289
7 Nonlinear Filtering 293
7.1 Chapter Focus, 293
7.2 Quasilinear Filtering, 296
CONTENTS vii
7.3 Sampling Methods for Nonlinear Filtering, 330
7.4 Summary, 345
Problems, 350
8 Practical Considerations 355
8.1 Chapter Focus, 355
8.2 Detecting and Correcting Anomalous Behavior, 356
8.3 Prefiltering and Data Rejection Methods, 379
8.4 Stabilityof Kalman Filters, 382
8.5 Suboptimal and Reduced-Order Filters, 383
8.6 Schmidt–Kalman Filtering, 393
8.7 Memory, Throughput, and Wordlength Requirements, 403
8.8 Waysto Reduce Computational Requirements, 409
8.9 Error Budgets and Sensitivity Analysis, 414
8.10 Optimizing Measurement Selection Policies, 419
8.11 Innovations Analysis, 424
8.12 Summary, 425
Problems, 426
9 Applicationsto Navigation 427
9.1 Chapter Focus, 427
9.2 Host Vehicle Dynamics, 431
9.3 Inertial Navigation Systems (INS), 435
9.4 Global Navigation Satellite Systems (GNSS), 465
9.5 Kalman Filters for GNSS, 470
9.6 Loosely Coupled GNSS/INS Integration, 488
9.7 Tightly Coupled GNSS/INS Integration, 491
9.8 Summary, 507
Problems, 508
Appendix A MATLAB Software 511
A.1 Notice, 511
A.2 General System Requirements, 511
A.3 CD Directory Structure, 512
A.4 MATLAB Software for Chapter 2, 512
A.5 MATLAB Software for Chapter 3, 512
A.6 MATLAB Software for Chapter 4, 512
A.7 MATLAB Software for Chapter 5, 513
A.8 MATLAB Software for Chapter 6, 513
A.9 MATLAB Software for Chapter 7, 514
A.10 MATLAB Software for Chapter 8, 515
A.11 MATLAB Software for Chapter 9, 515
A.12 Other Sources of Software, 516
viii CONTENTS
Appendix B A Matrix Refresher 519
B.1 Matrix Forms, 519
B.2 Matrix Operations, 523
B.3 Block Matrix Formulas, 527
B.4 Functions of Square Matrices, 531
B.5 Norms, 538
B.6 Cholesky Decomposition, 541
B.7 Orthogonal Decompositions of Matrices, 543
B.8 Quadratic Forms, 545
B.9 Derivatives of Matrices, 546
Bibliography 549
Index 565
PREFACE
This book is designed to provide familiarity with both the theoretical and practical
aspectsofKalmanfilteringbyincludingreal-worldproblemsinpracticeasillustrative
examples.ThematerialincludestheessentialtechnicalbackgroundforKalmanfilter-
ingandthemorepracticalaspectsofimplementation:howtorepresenttheproblemin
a mathematical model, analyze the performance of the estimator as a function of
system design parameters, implement the mechanization equations in numerically
stable algorithms, assess its computational requirements, test the validity of results,
and monitor the filter performance in operation. These are important attributes of
the subject that are often overlooked in theoretical treatments but are necessary for
application of the theory to real-world problems.
In thisthird edition, we have included important developments in the implemen-
tationandapplicationofKalmanfilteringoverthepastseveralyears,includingadap-
tations for nonlinear filtering, more robust smoothing methods, and developing
applications in navigation.
We have also incorporated many helpful corrections and suggestions from our
readers, reviewers, colleagues, and students over the past several years for the
overall improvement of the textbook.
AllsoftwarehasbeenprovidedinMATLAB1sothatuserscantakeadvantageof
its excellent graphing capabilities and a programming interface that is very close to
the mathematical equations used for defining Kalman filtering and its applications.
See Appendix A for more information on MATLAB software.
Theinclusionofthesoftwareispracticallyamatterofnecessity,becauseKalman
filtering would not be very useful without computers to implement it. It provides a
1MATLABisaregisteredtrademarkofTheMathworks,Inc.
ix
x PREFACE
betterlearningexperienceforthestudenttodiscoverhowtheKalmanfilterworksby
observing it in action.
TheimplementationofKalmanfilteringoncomputersalsoilluminatessomeofthe
practical considerations of finite-wordlength arithmetic and the need for alternative
algorithms to preserve the accuracy of the results. If the student wishes to apply
what she or he learns, then it is essential that she or he experience its workings
and failings—and learn to recognize the difference.
Thebookisorganizedasatextforanintroductorycourseinstochasticprocessesat
theseniorlevelandasafirst-yeargraduate-levelcourseinKalmanfilteringtheoryand
application.Itcanalsobeusedforself-instructionorforpurposesofreviewbypracti-
cing engineers and scientists who are not intimately familiar with the subject. The
organization of the material is illustrated by the following chapter-level dependency
graph, which shows how the subject of each chapter depends upon material in other
chapters. The arrows in the figure indicate the recommended order of study. Boxes
above another box and connected byarrows indicate that the material represented by
the upper boxes isbackground material forthe subjectinthe lower box.
Chapter1providesaninformalintroductiontothegeneralsubjectmatterbyway
ofitshistoryofdevelopmentandapplication.Chapters2and3andAppendixBcover
theessentialbackgroundmaterialonlinearsystems,probability,stochasticprocesses,
andmodeling.Thesechapterscouldbecoveredinasenior-levelcourseinelectrical,
computer, and systems engineering.
Chapter 4 covers linear optimal filters and predictors, with detailed examples of
applications. Chapter 5 is a new tutorial-level treatment of optimal smoothing