By Armando Barreto, Malek Adjouadi, Francisco R. Ortega, Nonnarit O-larnnithipong
Contents
Acknowledgments, xi
Authors, xiii
Introduction, xv
PART I
Background
CHAPTER 1 ■ System Models and Random Variables 3
1.1 DETERMINISTIC AND RANDOM MODELS AND VARIABLES 3
1.2 HISTOGRAMS AND PROBABILITY FUNCTIONS 6
1.3 THE GAUSSIAN (NORMAL) DISTRIBUTION 12
1.4 MODIFICATION OF A SIGNAL WITH GAUSSIAN DISTRIBUTION THROUGH A FUNCTION REPRESENTED BY A STRAIGHT LINE 14
1.5 EFFECTS OF MULTIPLYING TWO GAUSSIAN DISTRIBUTIONS 21 CHAPTER 2 ■ Multiple Random Sequences Considered Jointly 25
2.1 JOINT DISTRIBUTIONS—BIVARIATE CASE 25
2.2 BIVARIATE GAUSSIAN DISTRIBUTION— COVARIANCE AND CORRELATION 32
2.3 COVARIANCE MATRIX 38
2.4 PROCESSING A MULTIDIMENSIONAL GAUSSIAN DISTRIBUTION THROUGH A LINEAR TRANSFORMATION 39
2.5 MULTIPLYING TWO MULTIVARIATE GAUSSIAN DISTRIBUTIONS 40
CHAPTER 3
■ Conditional Probability, Bayes’ Rule and Bayesian Estimation 45
3.1 CONDITIONAL PROBABILITY AND THE BAYES’ RULE 45
3.2 BAYES’ RULE FOR DISTRIBUTIONS 50
PART II Where Does Kalman Filtering Apply and What Does It Intend to Do?
CHAPTER 4 ■ A Simple Scenario Where Kalman Filtering May Be Applied 55
4.1 A SIMPLE MODELING SCENARIO: DC MOTOR CONNECTED TO A CAR BATTERY 55
4.2 POSSIBILITY TO ESTIMATE THE STATE VARIABLE BY PREDICTION FROM THE MODEL57
4.2.1 Internal Model Uncertainty 58
4.2.2 External Uncertainty Impacting the System 58
4.3 POSSIBILITY TO ESTIMATE THE STATE VARIABLE BY MEASUREMENT OF EXPERIMENTAL VARIABLES 59
4.3.1 Uncertainty in the Values Read of the Measured Variable 59
CHAPTER 5 ■ General Scenario Addressed by Kalman Filtering and Specifc Cases 61
5.1 ANALYTICAL REPRESENTATION OF A GENERIC KALMAN FILTERING SITUATION 62
5.2 UNIVARIATE ELECTRICAL CIRCUIT EXAMPLE IN THE GENERIC FRAMEWORK 67
5.3 AN INTUITIVE, MULTIVARIATE SCENARIO WITH ACTUAL DYNAMICS: THE FALLING WAD OF PAPER 70
CHAPTER 6 ■ Arriving at the Kalman Filter Algorithm 75
6.1 GOALS AND ENVIRONMENT FOR EACH ITERATION OF THE KALMAN FILTERING ALGORITHM 75
6.2 THE PREDICTION PHASE 76
6.3 MEASUREMENTS PROVIDE A SECOND SOURCE OF KNOWLEDGE FOR STATE ESTIMATION 78
6.4 ENRICHING THE ESTIMATE THROUGH BAYESIAN ESTIMATION IN THE “CORRECTION PHASE” 79 CHAPTER 7 ■ Refecting on the Meaning and Evolution of the Entities in the Kalman Filter Algorithm 87
7.1 SO, WHAT IS THE KALMAN FILTER EXPECTED TO ACHIEVE? 87
7.2 EACH ITERATION OF THE KALMAN FILTER SPANS “TWO TIMES” AND “TWO SPACES”88
7.3 YET, IN PRACTICE ALL THE COMPUTATIONS ARE PERFORMED IN A SINGLE, “CURRENT” ITERATION—CLARIFICATION 90
7.4 MODEL OR MEASUREMENT? KG DECIDES WHO WE SHOULD TRUST 91 PART III Examples in MATLAB® CHAPTER 8 ■ MATLAB® Function to Implement and Exemplify the Kalman Filter 103 8.1 DATA AND COMPUTATIONS NEEDED FOR THE IMPLEMENTATION OF ONE ITERATION OF THE KALMAN FILTER 103 8.2 A BLOCK DIAGRAM AND A MATLAB® FUNCTION FOR IMPLEMENTATION OF ONE KALMAN FILTER ITERATION 106 8.3 RECURSIVE EXECUTION OF THE KALMAN FILTER ALGORITHM 108 8.4 THE KALMAN FILTER ESTIMATOR AS A “FILTER” 110 CHAPTER 9 ■ Univariate Example of Kalman Filter in MATLAB® 113 9.1 IDENTIFICATION OF THE KALMAN FILTER VARIABLES AND PARAMETERS 113
9.2 STRUCTURE OF OUR MATLAB® SIMULATIONS 114
9.3 CREATION OF SIMULATED SIGNALS: CORRESPONDENCE OF PARAMETERS AND SIGNAL CHARACTERISTICS 116
9.4 THE TIMING LOOP 119
9.5 EXECUTING THE SIMULATION AND INTERPRETATION OF THE RESULTS 121
9.6 ISOLATING THE PERFORMANCE OF THE MODEL (BY NULLIFYING THE KALMAN GAIN) 125
CHAPTER 10 ■ Multivariate Example of Kalman Filter in MATLAB® 131 10.1 OVERVIEW OF THE SCENARIO AND SETUP OF THE KALMAN FILTER 10.2 STRUCTURE OF THE MATLAB® SIMULATION FOR THIS CASE 131 134
10.3 TESTING THE SIMULATION 10.4 FURTHER ANALYSIS OF THE SIMULATION RESULTS 140 144
10.5 ISOLATING THE EFFECT OF THE MODEL 148
PART IV
Kalman Filtering Application to IMUs
CHAPTER 11
■ Kalman Filtering Applied to 2-Axis Attitude Estimation from Real IMU Signals 153
11.6 STRUCTURE OF THE MATLAB® IMPLEMENTATION OF THIS KALMAN FILTER APPLICATION
166
11.7 TESTING THE IMPLEMENTATION OF KALMAN FILTER FROM PRE-RECORDED IMU SIGNALS 170
CHAPTER 12
■ Real-Time Kalman Filtering Application to Attitude Estimation from IMU Signals 179
12.1 PLATFORM AND ORGANIZATION OF THE REAL-TIME KALMAN FILTER IMPLEMENTATION FOR ATTITUDE ESTIMATION 12.2 SCOPE OF THE IMPLEMENTATION AND ASSUMPTIONS 12.3 INITIALIZATION AND ASSIGNMENT OF PARAMETERS FOR THE EXECUTION 12.4 BUILDING (COMPILING AND LINKING) THE EXECUTABLE PROGRAM RTATT2IMU.EXE— REQUIRED FILES 12.5 COMMENTS ON THE CUSTOM MATRIX AND VECTOR MANIPULATION FUNCTIONS 12.6 INPUTS AND OUTPUTS OF THE REAL-TIME IMPLEMENTATION 12.7 TRYING THE REAL-TIME IMPLEMENTATION OF THE KALMAN FILTER FOR ATTITUDE ESTIMATION 12.8 VISUALIZING THE RESULTS OF THE REAL-TIME PROGRAM APPENDIX A LISTINGS OF THE FILES FOR REAL-TIME IMPLEMENTATION OF THE KALMAN FILTER FOR ATTITUDE ESTIMATION WITH ROTATIONS IN 2 AXES, 197 REFERENCES, 217 INDEX, 221
Introduction
The Kalman Filter, envisioned by Dr. Rudolf E. Kalman (1930–2016) provides an efective mechanism to estimate the state of a dynamic system when a model is available to sequentially predict the state and sequential measurements are also available. Tis is a common kind of situation in the study of many practical dynamical systems, in diverse felds.
Te Kalman Filter has had an important impact on advances within the felds related to the navigation of ships, aircraf and spacecraf. In those initial areas of use, the Kalman Filter was almost exclusively used for highly specifc applications, by a very small group of highly specialized users. However, in the XXI century its potential application to small robots and miniature unmanned vehicles has broadened the appeal of this powerful estimation approach to a much wider audience. Furthermore, the recent emergence of cheap, miniaturized Inertial Measurement Units (IMUs) or Magnetic, Angular Rate and Gravity (MARG) sensing modules in contemporary applications has magnifed the importance of techniques such as Kalman Filtering, capable of combining information from multiple sensors. Of course, all of these contemporary applications work on digitized data, and, as such, are addressed by the Discrete Kalman Filter, which is the subject of this book.
Tis book is our efort to provide that wider audience with a presentation of the Kalman Filter that is not a mere “cookbook” list of steps (which may result in a sub-optimal use of this important tool), while not requiring the reader to wade through several formal proofs to accomplish a strict derivation of the algorithm. Tose proofs were given by Kalman in his 1960 paper and others who have studied the issue from a formal perspective since then.
Instead, it is our hope to provide the reader with an explanation of the Kalman Filter approach that requires a minimum of background concepts, presented simply and succinctly in Part I of this book. We expect that this concise background review will, nonetheless, be enough to help the reader see why it is that the Kalman Filter is capable of obtaining improved estimates of the variables studied. Most importantly, we would like to help the reader acquire an intuitive understanding of the meaning of all the parameters involved in the Kalman Filter algorithm and their interactions. Te development of that intuitive understanding of the elements at play in the Kalman Filter, and how they come together, according to the background concepts, is the objective of Part II of this book. Te chapters in Part II lead the reader to the formulation of the Kalman Filter algorithm as the logical conclusion of the considerations made throughout that part of this book. Part III of this book focuses on of-line implementation of the Kalman Filter to address estimation challenges of increasing complexity. Tis third part starts by leveraging the understanding of the Kalman Filter algorithm to develop a MATLAB implementation for a single Kalman Filter iteration. Te rest of the of-line examples in this book are developed from that basic one-iteration function, which can be tailored to create of-line Kalman Filter solutions to diverse estimation problems. Part IV of this book includes two chapters that focus on the application of Kalman Filtering to the estimation of orientation of a miniature IMU module, utilizing the gyroscope and accelerometer signals provided by the module. Te frst of the two chapters of this part develops an of-line solution that still uses the basic one-iteration MATLAB function created at the beginning of Part III. Te gyroscope and accelerometer signals are read from a pre-recorded fle which was written while the IMU module was subjected to a specifc series of orientation changes, so that the results can be compared with the known series of orientations experienced by the module. Te second chapter in Part IV of this book reproduces the implementation of the algorithm in a C program which calculates the Kalman Filter results in real time and stores them to a text fle, for later review.