Introduction to the Kalman Filter and Tuning Its Statistics for Near Optimal Estimates and Cramer Rao Bound
Total Page:16
File Type:pdf, Size:1020Kb
Introduction to the Kalman Filter and Tuning its Statistics for Near Optimal Estimates and Cramer Rao Bound by Shyam Mohan M, Naren Naik, R.M.O. Gemson, M.R. Ananthasayanam Technical Report : TR/EE2015/401 arXiv:1503.04313v1 [stat.ME] 14 Mar 2015 DEPARTMENT OF ELECTRICAL ENGINEERING INDIAN INSTITUTE OF TECHNOLOGY KANPUR FEBRUARY 2015 Introduction to the Kalman Filter and Tuning its Statistics for Near Optimal Estimates and Cramer Rao Bound by Shyam Mohan M1, Naren Naik2, R.M.O. Gemson3, M.R. Ananthasayanam4 1Formerly Post Graduate Student, IIT, Kanpur, India 2Professor, Department of Electrical Engineering, IIT, Kanpur, India 3Formerly Additional General Manager, HAL, Bangalore, India 4Formerly Professor, Department of Aerospace Engineering, IISc, Banglore Technical Report : TR/EE2015/401 DEPARTMENT OF ELECTRICAL ENGINEERING INDIAN INSTITUTE OF TECHNOLOGY KANPUR FEBRUARY 2015 ABSTRACT This report provides a brief historical evolution of the concepts in the Kalman filtering theory since ancient times to the present. A brief description of the filter equations its aesthetics, beauty, truth, fascinating perspectives and competence are described. For a Kalman filter design to provide optimal estimates tuning of its statistics namely initial state and covariance, unknown parameters, and state and measurement noise covariances is important. The earlier tuning approaches are reviewed. The present approach is a reference recursive recipe based on multiple filter passes through the data without any optimization to reach a ‘statistical equilibrium’ solution. It utilizes the a priori, a pos- teriori, and smoothed states, their corresponding predicted measurements and the actual measurements help to balance the measurement equation and similarly the state equation to help form a generalized likelihood cost function. The filter covariance at the end of each pass is heuristically scaled up by the number of data points is further trimmed to statistically match the exact estimates and Cramer Rao Bounds (CRBs) available with no process noise provided the initial covariance for subsequent passes. During simulation studies with process noise the matching of the input and estimated noise sequence over time and in real data the generalized cost functions helped to obtain confidence in the results. Simulation studies of a constant signal, a ramp, a spring, mass, damper system with a weak non linear spring constant, longitudinal and lateral motion of an airplane was followed by similar but more involved real airplane data was carried out in MATLAB R . In all cases the present approach was shown to provide internally consistent and best possible estimates and their CRBs. i ACKNOWLEDGEMENTS It is a pleasure to thank many people with whom the authors interacted over a period of time in the topic of Kalman filtering and its Applications. Decades earlier this topic was started as a course in the Department of Aerospace Engineering and a Workshop was conducted along with Prof. S. M. Deshpande who started it and then moved over full time to Computational Fluid Dynamics. Subsequently MRA taught the course and spent many years carrying out research and development in this area during his tenure at the IISc, Bangalore. The problem of filter tuning has always been intriguing for MRA since most people in the area tweaked rather than tuned most of the time with the result that there is no one procedure that could be used routinely in applying the Kalman filter in its innumerable applications. The PhD thesis of RMOG has been the only effort for a proper tuning of the filter parameters but this was not too well known. In the recent past for a couple of years the interaction among the present authors helped to reach the present method which we believe is quite good for such routine applications. The report has been written in such a way to be useful as a teaching material. Our grateful thanks are due to Profs. R. M. Vasu (Dept. of Instrumentation and Applied Physics), D. Roy (Dept. of Civil Engineering), and M. R. Muralidharan (Supercomputer Education and Research Centre) for help in a number of observable and unobservable ways without which this work would just not have been possible at all and also for providing computational facilities at the IISc, Bangalore. It should be possible to further improve the present approach based on applications in a wide variety of problems in science and technology. - AUTHORS. Shyam Mohan M ([email protected]) Naren Naik ([email protected]) R.M.O. Gemson ([email protected]) M.R. Ananthasayanam ([email protected]) ii Contents 1 Historical Development of Concepts in Kalman Filter1 1.1 Randomness from Ancient Times to the Present . .1 1.2 Conceptual Development of the Kalman Filter . .2 1.3 Estimation Theory in Ancient Indian Astronomy . .3 1.4 Estimation Theory during the Medieval Period . .5 1.5 Estimation Theory (ET) During the Twentieth Century . .6 1.6 Present day Applications of Estimation Theory . .7 1.7 Three Main Types of Problems in Estimation Theory . .7 1.8 Summary of different aspects of Estimation Theory . 10 2 Introduction to the Kalman Filter 13 2.1 Important Features of the Kalman Filter . 13 2.2 Importance of Proper Data Processing . 14 2.3 Problem Formulation and Extended Kalman Filter Equations . 15 2.4 Aesthetics, Beauty, and Truth of the Kalman Filter . 20 2.5 Definition and Different ways of Looking at the Kalman Filter . 21 2.5.1 Inverse Problem . 21 2.5.2 Qualitative Modeling . 22 2.5.3 Quantitative Estimation . 23 2.5.4 Handling Deterministic State and Measurement Errors . 23 2.5.5 Estimation of Unmodelable Inputs by Noise Modeling . 23 2.5.6 Unobservables from Observables . 24 2.5.7 Expansion of the Scenario . 24 2.5.8 Deterministic or Probabilistic Approach? . 24 2.5.9 Handling Numerical Errors by Noise Addition . 25 2.5.10 Stochastic Corrective Process . 26 iii 2.5.11 Data Fusion and Statistical Estimation by Probabilistic Mixing . 26 2.5.12 Optimization in the Time Domain . 27 2.5.13 Frequency Domain Analysis . 27 2.5.14 Smoothing of the Filter Estimates . 28 2.5.15 Improved States and Measurements . 28 2.5.16 Estimation of Process and Measurement Noise . 29 2.5.17 Correlation of the Innovation Sequence . 29 2.5.18 Reversing an ‘Irreversible’ Process . 29 2.5.19 Some Human Activities from ET Perspective . 30 2.6 Tuning of the Kalman Filter Statistics . 31 2.7 Important Qualitative Features of the Filter Statistics . 32 2.8 Constant Gain Approach . 33 2.9 Kalman Filter and some of its Variants . 34 3 Kalman Filter Tuning : Earlier and the Present Approach 37 3.1 Introduction to Kalman filter Tuning . 37 3.2 Some Simple Choices for Initial X0 for States and Parameters . 38 3.3 Importance of Initial P0 for States and Parameters . 38 3.4 Choice of Initial R ............................. 39 3.5 Choice of Initial Q ............................. 39 3.6 Simultaneous Estimation of R and Q .................... 40 3.6.1 Tuning Filter Statistics with only R ................ 40 3.6.2 Tuning Filter Statistics with both R and Q ............. 41 3.7 Earlier Approaches for Tuning the Kalman Filter Statistics . 41 3.7.1 Mathematical Approaches and Desirable Features . 41 3.7.2 Review of Earlier Adaptive Kalman Filtering Approaches . 42 3.7.3 Earlier versus the Present Approach for Filter Tuning . 47 3.8 Present Approach for Tuning Filter Statistics . 49 3.8.1 Choice of X0 ............................ 51 3.8.2 Choice of P0 ............................ 52 3.8.3 Estimation of R and Q using EM Method . 53 3.8.4 The Present DSDT Method for Estimating Q ........... 55 3.8.5 Choice of R ............................. 57 3.8.6 Choice of Q ............................. 58 iv 3.9 Adaptive Tuning Algorithm and the Reference Recursive Recipe . 58 4 Simulated Case Studies 63 4.1 Simulated Constant Signal System . 64 4.1.1 Details of Sensitivity Study . 64 4.1.2 Remarks on the Results . 68 4.1.3 Constant System Tables (Q =0).................. 70 4.1.4 Constant System Tables (Q > 0) . 73 4.1.5 Constant System Figures (Q = 0) . 78 4.1.6 Constant System Figures (Q > 0) . 82 4.2 Simulated Ramp System . 88 4.2.1 Remarks on the Results . 88 4.2.2 Ramp System Tables (Q =0) ................... 90 4.2.3 Ramp System Tables (Q > 0) ................... 93 4.2.4 Ramp System Figures (Q =0)................... 98 4.2.5 Ramp System Figures (Q > 0) . 102 4.3 Simulated Spring, Mass, and Damper System . 108 4.3.1 Remarks on the Results . 108 4.3.2 SMD System Tables (Q =0).................... 110 4.3.3 SMD System Tables (Q > 0) ................... 113 4.3.4 SMD System Figures (Q =0) ................... 118 4.3.5 SMD System Figures (Q > 0)................... 124 4.4 Simulated Longitudinal Motion of Aircraft . 133 4.4.1 Remarks on the Results . 133 4.4.2 Longitudinal Motion of Aircraft System Tables (Q = 0) . 135 4.4.3 Longitudinal Motion of Aircraft System Tables (Q > 0) . 139 4.4.4 Longitudinal Motion of Aircraft System Figures (Q = 0) . 143 4.4.5 Longitudinal Motion of Aircraft System Figures (Q > 0) . 155 4.5 Simulated Lateral Motion of Aircraft System . 167 4.5.1 Remarks on the Results . 168 4.5.2 Lateral Motion of Aircraft System Tables (Q = 0) . 169 4.5.3 Lateral Motion of Aircraft System Tables (Q > 0) . 175 4.5.4 Lateral Motion of Aircraft System Figures (Q = 0) . 180 4.5.5 Lateral Motion of Aircraft System Figures (Q > 0) . 200 v 5 Real Flight Test Data Analysis 215 5.1 Real Flight Test Case-1 . 216 5.1.1 Remarks on Case-1 . 219 5.1.2 Case-1 Tables .