MATLAB在卡尔曼滤波器中应用的理论与实践Kalman

MATLAB在卡尔曼滤波器中应用的理论与实践KalmanKALMAN FILTERINGTheory and Practice Using MATLABThird editionMOHINDER S GREWALCalifornia State University at FullertonANGUS P. ANDREWSRockwell Science Center (retired)WILEYA JOHN WILEY & SONS, INC. PUBLICATIONCopyright 2008 by John Wiley sons, Inc. All rights reservedPublished by John Wiley sons, InC, Hoboken, New JerseyPublished simultaneously in CanadaNo part of this publication may be reproduced, stored in a retrieval system, or transmitted in any form or byany means, electronic, mechanical, photocopying, recording, scanning, or otherwise, except as permittedunder Section 107 or 108 of the 1976 United States Copyright Act, without either the prior writtenpermission of the Publisher, or authorization through payment of the appropriate per-copy fee to theCopyright Clearance Center, Inc, 222 Rosewood Drive, Danvers, MA 01923,(978)750-8400, fax(978)750-4470,oronthewebatwww.copyright.com.RequeststothePublisherforpermissionshouldbe addressed to the Permissions Department, John Wiley Sons, Inc, lll River Street, Hoboken, NJ07030,(201)748-6011,fax(201)748-6008,oronlineathttp://www.wiley.com/go/permissionimit of liability Disclaimer of Warranty: While the publisher and author have used their best efforts inpreparing this book, they make no representations or warranties with respect to the accuracy or completeness of the contents of this book and specifically disclaim any implied warranties of merchantability orfitness for a particular purpose. No warranty may be created or extended by sales representatives orwritten sales materials. The advice and strategies contained herein may not be suitable for your situationYou should consult with a professional where appropriate. Neither the publisher nor author shall be liablefor any loss of profit or any other commercial damages, including but not limited to special, incidentalconsequential, or other damagesFor general information on our other products and services or for technical support, please contact ourCustomer Care Department within the United States at(800)762-2974, outside the United States at(317)572-3993 or fax(317)572-4002Wiley also publishes its books in a variety of electronic formats. Some content that appears in print maynot be available in electronic format. For more information about wiley products, visit our web site atwww.wiley.comLibrary of Congress Cataloging- in-Publication DataGrewal. Mohinder sKalman filtering: theory and practice using MATLAB/Mohinder S. GrewalAngus p. andrews. 3rd edIncludes bibliographical references and indexISBN978-0-470-17366-4( cloth)1. Kalman filtering. 2. MATLAB. I. Andrews, Angus P. II. TitleQA402.3.G69520086298312—dc22200803733Printed in the United States of america10987654321CONTENTSPrefaceAcknowledgmentsXIIIList of abbreviationsXV1 General Information1.1 On Kalman Filtering1.2 On Optimal Estimation Methods, 51. 3 On the notation Used In This book 231. 4 Summary, 25Problems. 262 Linear Dvnamic Systems2. 1 Chapter focus, 312.2 Dynamic System Models, 362. 3 Continuous Linear Systems and Their Solutions, 402.4 Discrete Linear Systems and Their Solutions, 532.5 Observability of Linear Dynamic System Models, 552.6 Summary, 61Problems. 643 Random Processes and Stochastic Systems3.1 Chapter Focus, 673.2 Probability and random Variables (rvs), 703.3 Statistical Properties of RVS, 78CONTEN3.4 Statistical Properties of Random Processes(RPs),803.5 Linear rp models. 883.6 Shaping Filters and State Augmentation, 953.7 Mean and Covariance propagation, 993.8 Relationships between Model Parameters, 1053.9 Orthogonality principle 1143.10 Summary, 118Problems. 1214 Linear Optimal Filters and Predictors1314.1 Chapter Focus, 1314.2 Kalman Filter. 1334.3 Kalman-Bucy filter, 1444.4 Optimal Linear Predictors, 1464.5 Correlated noise Sources 1474.6 Relationships between Kalman-Bucy and wiener Filters, 1484.7 Quadratic Loss Functions, 1494.8 Matrix Riccati Differential Equation. 1514.9 Matrix Riccati Equation In Discrete Time, 1654.10 Model equations for Transformed State Variables, 1704.11 Application of Kalman Filters, 1724.12 Summary, 177Problems. 1795 Optimal Smoothers5.1 Chapter Focus, 1835.2 Fixed-Interval Smoothing, 1895.3 Fixed-Lag Smoothing, 2005.4 Fixed-Point Smoothing, 2135.5 Summary, 220Problems. 226 Implementation Methods2256. 1 Chapter Focus, 2256.2 Computer Roundoff, 2276.3 Effects of roundoff errors on Kalman filters 2326.4 Factorization Methods for Square-Root Filtering, 2386. 5 Square-Root and UD Filters, 2616.6 Other Implementation Methods, 2756.7 Summary, 288Problems. 2897 Nonlinear Filtering2937.1 Chapter Focus, 2937.2 Quasilinear Filtering, 296CONTENTS7.3 Sampling Methods for Nonlinear Filtering, 3307.4 Summary, 345Problems. 3508 Practical Considerations3558.1 Chapter Focus. 3558.2 Detecting and Correcting Anomalous behavior, 3568.3 Prefiltering and Data Rejection Methods, 3798.4 Stability of Kalman Filters, 3828. 5 Suboptimal and reduced- Order Filters, 3838.6 Schmidt-Kalman Filtering, 3938.7 Memory, Throughput, and wordlength Requirements, 4038.8 Ways to Reduce Computational requirements 4098.9 Error Budgets and Sensitivity Analysis, 4148.10 Optimizing Measurement Selection Policies, 4198.11 Innovations analysis, 4248.12 Summary, 425Problems. 4269 Applications to Navigation4279.1 Chapter focus, 4279.2 Host vehicle dynamics, 4319.3 Inertial Navigation Systems(INS), 4359. 4 Global Navigation Satellite Systems(GNSS), 4659.5 Kalman Filters for GNSS. 4709.6 Loosely Coupled GNSS/INS Integration, 4889.7 Tightly Coupled GNSS /INS Integration, 4919. 8 Summary, 507Problems. 508Appendix A MATLAB Software511A 1 Notice. 511A 2 General System Requirements, 511A 3 CD Directory Structure, 512A 4 MATLAB Software for Chapter 2, 512A. 5 MATLAB Software for Chapter 3, 512A6 MATLAB Software for Chapter 4, 512A. 7 MATLAB Software for Chapter 5, 513A 8 MATLAB Software for Chapter 6, 513A 9 MATLAB Software for Chapter 7, 514A10 MATLAB Software for Chapter 8, 515A 11 MATLAB Software for Chapter 9, 515A 12 Other Sources of software 516CONTENAppendix b A Matrix Refresher519B. 1 Matrix Forms. 519B 2 Matrix Operations, 523B 3 Block matrix Formulas. 527B 4 Functions of Square Matrices, 531B 5 Norms. 538B6 Cholesky decomposition, 541B7 Orthogonal Decompositions of Matrices, 543B 8 Quadratic Forms, 545B 9 Derivatives of matrices. 546Bibliography549Index565PREFACEThis book is designed to provide familiarity with both the theoretical and practicalaspects of Kalman filtering by including real-world problems in practice as illustrativeexamples. The material includes the essential technical background for Kalman filter-ing and the more practical aspects of implementation: how to represent the problem ina mathematical model, analyze the performance of the estimator as a function ofsystem design parameters, implement the mechanization equations in numericallystable algorithms, assess its computational requirements, test the validity of resultsitor the filteThetant attributes ofthe subject that are often overlooked in theoretical treatments but are necessary forapplication of the theory to real-world problemsIn this third edition, we have included important developments in the implemen-tation and application of Kalman filtering over the past several years, including adaptations for nonlinear filtering, more robust smoothing methods, and develelopingapplications in navigationWe have also incorporated many helpful corrections and suggefrom ourreaders, reviewers, colleagues, and students over the past several years for theoverall improvement of the textbookAll software has been provided in MatLab so that users can take advantage ofits excellent graphing capabilities and a programming interface that is very close tothe mathematical equations used for defining Kalman filtering and its applicationsSee Appendix a for more information on MATLAB softwareThe inclusion of the software is practically a matter of necessity because Kalmanfiltering would not be very useful without computers to implement it. It provides aMATLAB is a registered trademark of The Mathworks, IncEFACEbetter learning experience for the student to discover how the Kalman filter works byobserving it in actionThe implementation of Kalman filtering on computers also illuminates some of thepractical considerations of finite-wordlength arithmetic and the need for alternativealgorithms to preserve the accuracy of the results. If the student wishes to applywhat she or he learns, then it is essential that she or he experience its workingsand failings--and learn to recognize the differenceThe book is organized as a text for an introductory course in stochastic processes atthe senior level and as a first-year graduate-level course in Kalman filtering theory andapplicationIt can also be used for self-instruction or for purposes of review by practi-cing engineers and scientists who are not intimately familiar with the subject. Theorganization of the material is illustrated by the following chapter-level dependencygraph, which shows how the subject of each chapter depends upon material in otherchapters. The arrows in the figure indicate the recommended order of study. Boxesabove another box and connected by arrows indicate that the material represented bythe upper boxes is background material for the subject in the lower boxAPPENDIX B: A MATRIX REFRESHERGENERAL INFORMATION2. LINEAR DYNAMIC SYSTEMSRANDOM PROCESSES AND STOCHASTIC SYSTEMS4. OPTIMAL LINEAR FILTERS AND PREDICTORS5. OPTIMAL SMOOTHERS6. IMPLEMENTATIONMETHODS7. NONLINEAR8. PRACTICAL9. APPLICATIONSFILTERINGCONSIDERATIONSTO NAVIGATIONAPPENDIX A: MATLAB SOFTWAREChapter l provides an informal introduction to the general subject matter by wayof its history of development and application. Chapters 2 and 3 and Appendix b coverthe essential background material on linear systems, probability, stochastic processesand modeling. These chapters could be covered in a senior-level course in electricalcomputer, and systems engineeringChapter 4 covers linear optimal filters and predictors, with detailed examples ofapplications. Chapter 5 is a new tutorial-level treatment of optimal smoothing