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
Table of Contents Preface Introduction Acknowledgments xv xvii 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 Noise 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
TABLE OF CONTENTS Chapter 4. Polynomial Kalman Filters 129 Introduction 129 General Equations 129 Derivation of Scalar Riccati Equations 131 Polynomial Kalman Filter (Zero Process Noise) 134 Comparing Zeroth-Order Recursive Least-Squares and Kalman Filters 136 Comparing First-Order Recursive Least-Squares and Kalman Filters 139 Comparing Second-Order Recursive Least-Squares and Kalman Filters 142 Comparing Different-Order Filters 148 Initial Covariance Matrix 151 Riccati Equations with Process Noise 155 Example of Kalman Filter Tracking a Falling Object 159 Revisiting Accelerometer Testing Example 171 Summary 179 References 182 Chapter 5. Kalman Filters in a Nonpolynomial World 183 Introduction 183 Polynomial Kalman Filter and Sinusoidal Measurement 183 Sinusoidal Kalman Filter and Sinusoidal Measurement 194 Suspension System Example 203 Kalman Filter for Suspension System 207 Summary 218 References 218 Chapter 6. Continuous Polynomial Kalman Filter 219 Introduction 219 Theoretical Equations 219 Zeroth-Order or One-State Continuous Polynomial Kalman Filter 221 First-Order or Two-State Continuous Polynomial Kalman Filter 227 Second-Order or Three-State Continuous Polynomial Kalman Filter 232 Transfer Function for Zeroth-Order Filter 238 Transfer Function for First-Order Filter 243 Transfer Function for Second-Order Filter 248 Filter Comparison 251 Summary 255 References 255 Chapter 7. Extended Kalman Filtering 257 Introduction 257 Theoretical Equations 257
TABLE OF CONTENTS xi Drag Acting on Falling Object 259 First Attempt at Extended Kalman Filter 261 Second Attempt at Extended Kalman Filter 274 Third Attempt at Extended Kalman Filter 284 Summary 291 References 291 Chapter 8. Drag and Falling Object 293 Introduction 293 Problem Setup 293 Changing Filter States 309 i. Why Process Noise Is Required 311 Linear Polynomial Kalman Filter 320 Summary 329 References 329 Chapter 9. Cannon-Launched Projectile Tracking Problem 331 Introduction 331 Problem Statement 331 Extended Cartesian Kalman Filter 334 Polar Coordinate System 349 Extended Polar Kalman Filter 354 Using Linear Decoupled Polynomial Kalman Filters 367 Using Linear Coupled Polynomial Kalman Filters 376 Robustness Comparison of Extended and Linear Coupled Kalman Filters 385 Summary 393 Reference 394 Chapter 10. Tracking a Sine Wave 395 Introduction 395 Extended Kalman Filter 395 Two-State Extended Kalman Filter with a Priori Information... 408 Alternate Extended Kalman Filter for Sinusoidal Signal 417 Another Extended Kalman Filter for Sinusoidal Model 431 Summary 441 References 441 Chapter 11. Satellite Navigation 443 Introduction 443 Problem with Perfect Range Measurements 443 Estimation Without Filtering 447 Linear Filtering of Range 453 Using Extended Kalman Filtering 455 Using Extended Kalman Filtering with One Satellite 465 Using Extended Kalman Filtering with Constant Velocity Receiver 474
XII TABLE OF CONTENTS Single Satellite with Constant Velocity Receiver 479 Using Extended Kalman Filtering with Variable Velocity Receiver 493 Variable Velocity Receiver and Single Satellite 505 Summary 513 References 513 Chapter 12. Biases 515 Introduction 515 Influence of Bias 515 Estimating Satellite Bias with Known Receiver Location 519 Estimating Receiver Bias with Unknown Receiver Location and Two Satellites 525 Estimating Receiver Bias with Unknown Receiver Location and Three Satellites 533 Summary 544 Reference 547 Chapter 13. Linearized Kalman Filtering 549 Introduction 549 Theoretical Equations 549 Falling Object Revisited 552 Developing a Linearized Kalman Filter 556 Cannon-Launched Projectile Revisited 569 Linearized Cartesian Kalman Filter 570 Summary 583 References 585 Chapter 14. Miscellaneous Topics 587 Introduction 587 Sinusoidal Kalman Filter and Signal-to-Noise Ratio 587 When Only a Few Measurements Are Available 595 Detecting Filter Divergence in the Real World 606 Observability Example 618 Aiding 629 Summary 642 References 646 Chapter 15. Fading-Memory Filter 647 Introduction 647 Fading-Memory-Filter Structure and Properties 647 Radar Tracking Problem, 662 Summary 673 References 675 Chapter 16. Assorted Techniques for Improving Kalman-Filter Performance 677
TABLE OF CONTENTS XIII Introduction 677 Increasing Data Rate 677 Adding a Second Measurement 682 Batch Processing 690 Adaptive Filtering Multiple Filters 701 Adaptive Filtering Single Filter with Variable Process Noise.. 710 Summary 720 Appendix A Fundamentals of Kalman-Filtering Software 723 Software Details 723 MATLAB 724 True BASIC 730 Reference 738 Appendix B Key Formula and Concept Summary 739 Overview of Kalman-Filter Operation Principles 739 Kalman-Filter Gains and the Riccati Equations 739 Kalman-Filter Gain Logic 740 Matrix Inverse 740 Numerical Integration 741 Postprocessing Formulas 741 Simulating Pseudo White Noise 742 Fundamental Matrix 742 Method of Least-Squares Summary 742 Fading-Memory Filter Summary 745 Index 747 Supporting Materials 765