KALMAN FILTERING Theory and Practice Using Matlabw
Total Page:16
File Type:pdf, Size:1020Kb
KALMAN FILTERING KALMAN FILTERING Theory and Practice Using MATLABw Third Edition MOHINDER S. GREWAL California State University at Fullerton ANGUS P. ANDREWS Rockwell Science Center (retired) Copyright # 2008 by John Wiley & Sons, Inc. All rights reserved Published by John Wiley & Sons, Inc., Hoboken, New Jersey Published simultaneously in Canada No part of this publication may be reproduced, stored in a retrieval system, or transmitted in any form or by any means, electronic, mechanical, photocopying, recording, scanning, or otherwise, except as permitted 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, (978) 750-8400, fax (978) 750-4470, or on the web at www.copyright.com. Requests to the Publisher for permission should be addressed to the Permissions Department, John Wiley & Sons, Inc., 111 River Street, Hoboken, NJ 07030, (201) 748-6011, fax (201) 748-6008, or online at http:// www.wiley.com/go/permission. Limit of Liability/Disclaimer of Warranty: While the publisher and author have used their best efforts in preparing this book, they make no representations or warranties with respect to the accuracy or complete- ness of the contents of this book and specifically disclaim any implied warranties of merchantability or fitness for a particular purpose. No warranty may be created or extended by sales representatives or written sales materials. The advice and strategies contained herein may not be suitable for your situation. You should consult with a professional where appropriate. Neither the publisher nor author shall be liable for any loss of profit or any other commercial damages, including but not limited to special, incidental, consequential, or other damages. For general information on our other products and services or for technical support, please contact our Customer Care Department within the United States at (800) 762-2974, outside the United States at (317) 572-3993 or fax (317) 572-4002. Wiley also publishes its books in a variety of electronic formats. Some content that appears in print may not be available in electronic format. For more information about Wiley products, visit our web site at www.wiley.com. Library of Congress Cataloging-in-Publication Data: Grewal, Mohinder S. Kalman filtering : theory and practice using MATLAB/Mohinder S. Grewal, Angus P. Andrews. — 3rd ed. p. cm. Includes bibliographical references and index. ISBN 978-0-470-17366-4 (cloth) 1. Kalman filtering. 2. MATLAB. I. Andrews, Angus P. II. Title. QA402.3.G695 2008 629.8’312—dc22 200803733 Printed in the United States of America 10987654321 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 Probability and 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–Bucy and 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 Stability of 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 Ways to 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 Applications to 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 aspects of Kalman filtering by including real-world problems in practice as illustrative examples. The material includes the essential technical background for Kalman filter- ing and the more practical aspects of implementation: how to represent the problem in 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 this third edition, we have included important developments in the implemen- tation and application of Kalman filtering over the past several years, including adap- 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. All software has been provided in MATLAB1 so that users can take advantage of 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. The inclusion of the software is practically a matter of necessity, because Kalman filtering would not be very useful without computers to implement it. It provides a 1MATLAB is a registered trademark of The Mathworks, Inc. ix x PREFACE better learning experience for the student to discover how the Kalman filter works by observing it in action. The implementation of Kalman filtering on computers also illuminates some of the 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. The book is organized as a text for an introductory course in stochastic processes at the senior level and as a first-year graduate-level course in Kalman filtering theory and application. It 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. 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 by arrows indicate that the material represented by the upper boxes is background material for the subject in the lower box. Chapter 1 provides an informal introduction to the general subject matter by way of its history of development and application.