Purchased from American Institute of Aeronautics and Astronautics Fundamentals of Kalman Filtering: A Practical Approach Second Edition Purchased from American Institute of Aeronautics and Astronautics This page intentionally left blank Purchased from American Institute of Aeronautics and Astronautics Fundamentals of Kalxnan Filtering: A Practical Approach Second Edition Paul Zarchan MIT Lincoln Laboratory Lexington, Massachusetts Howard Musoff Charles Stark Draper Laboratory, Inc. Cambridge, Massachusetts Volume 208 PROGRESS IN ASTRONAUTICS AND AERONAUTICS Paul Zarchan, Editor-in-Chief MIT Lincoln Laboratory Lexington, Massachusetts Published by the American Institute of Aeronautics and Astronautics, Inc. 1801 Alexander Bell Drive, Reston, Virginia 20191-4344 Purchased from American Institute of Aeronautics and Astronautics Copyright © 2005 by the American Institute of Aeronautics and Astronautics, Inc. Printed in the United States of America. All rights reserved. Reproduction or translation of any part of this work beyond that permitted by Sections 107 and 108 of the U.S. Copyright Law without the permission of the copyright owner is unlawful. The code following this statement indicates the copyright owner's consent that copies of articles in this volume may be made for personal or internal use, on condition that the copier pay the per-copy fee ($2.00) plus the per-page fee ($0.50) through the Copyright Clearance Center, Inc., 222 Rosewood Drive, Danvers, Massachusetts 01923. This consent does not extend to other kinds of copying, for which permission requests should be addressed to the publisher. Users should employ the following code when reporting copying from the volume to the Copyright Clearance Center: 1-56347-694-0/00 $2.50 + .50 Data and information appearing in this book are for informational purposes only. AIAA is not responsible for any injury or damage resulting from use or reliance, nor does AIAA warrant that use or reliance will be free from privately owned rights. ISBN 1-56347-694-0 Purchased from American Institute of Aeronautics and Astronautics To Maxine, Adina, An, and Ronit P.Z. Wally, Sandy, Jay, Stephanie, Charlie, Scott, Cindy, Danielle, and Adam H.M. Purchased from American Institute of Aeronautics and Astronautics This page intentionally left blank Purchased from American Institute of Aeronautics and Astronautics Progress in Astronautics and Aeronautics Editor-in-Chief Paul Zarchan MIT Lincoln Laboratory Editorial Board David A. Bearden Richard C. Lind The Aerospace Corporation University of Florida John D. Binder Richard M. Lloyd viaSolutions Raytheon Electronics Company Steven A. Brandt Frank K. Lu U.S. Air Force Academy University of Texas at Arlington Fred R. DeJarnette Ahmed K. Noor North Carolina State University NASA Langley Research Center Philip D. Hattis Albert C. Piccirillo Charles Stark Draper Laboratory Institute for Defense Analyses Abdollah Khodadoust Ben T. Zinn The Boeing Company Georgia Institute of Technology Peter H. Zipfel Air Force Research Laboratory Purchased from American Institute of Aeronautics and Astronautics This page intentionally left blank Purchased from American Institute of Aeronautics and Astronautics Table of Contents Preface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. xv Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. xvii Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. xxv Chapter 1. Numerical Basics . . . . . . . . . . . . . . . . . . . . . . . . . .. 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 1 Simple Vector Operations........................ 1 Simple Matrix Operations. . . . . . . . . . . . . . . . . . . . . . .. 3 Numerical Integration of Differential Equations. . . . . . . . . .. 13 Noise and Random Variables. . . . . . . . . . . . . . . . . . . . .. 19 Gaussian Noise Example . . . . . . . . . . . . . . . . . . . . . . .. 23 Calculating Standard Deviation . . . . . . . . . . . . . . . . . . .. 26 White N o i s e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 28 Simulating White Noise......................... 30 State-Space Notation. . . . . . . . . . . . . . . . . . . . . . . . . .. 33 Fundamental Matrix . . . . . . . . . . . . . . . . . . . . . . . . . .. 34 Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 38 References................................. 38 Chapter 2. Method of Least Squares . . . . . . . . . . . . . . . . . . . .. 41 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 41 Overview. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 41 Zeroth-Order or One-State Filter. . . . . . . . . . . . . . . . . . .. 42 First-Order or Two-State Filter . . . . . . . . . . . . . . . . . . . .. 46 Second-Order or Three-State Least-Squares Filter . . . . . . . .. 50 Third-Order System . . . . . . . . . . . . . . . . . . . . . . . . . .. 56 Experiments with Zeroth-Order or One-State Filter . . . . . . .. 59 Experiments with First-Order or Two-State Filter.......... 64 Experiments with Second-Order or Three-State Filter . . . . . .. 71 Comparison of Filters . . . . . . . . . . . . . . . . . . . . . . . . .. 78 Accelerometer Testing Example . . . . . . . . . . . . . . . . . . .. 80 Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 89 References. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 90 Chapter 3. Recursive Least-Squares Filtering . . . . . . . . . . . . . .. 91 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 91 Making Zeroth-Order Least-Squares Filter Recursive . . . . . .. 91 Properties of Zeroth-Order or One-State Filter . . . . . . . . . .. 93 Properties of First-Order or Two-State Filter. . . . . . . . . . . .. 103 Properties of Second-Order or Three-State Filter . . . . . . . . .. 112 Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 124 References. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 128 ix