Improved Filtering And Estimation Methods For Aerospace Problems

This thesis is submitted to the Jadavpur University in partial fulfillment of the requirements for the degree of Doctor of Philosophy

By SHOVAN BHAUMIK

Department of Electrical Engineering Jadavpur University Kolkata, India 2008

Names, Designations and Affiliations of Supervisors 1. Dr. SMITA SADHU Reader Department of Electrical Engineering Jadavpur University Kolkata-700 032 India

2. Dr. TAPAN KUMAR GHOSHAL Professor Department of Electrical Engineering Jadavpur University Kolkata-700 032 India

Certificate from Supervisors This is to certify that the candidate Shri SHOVAN BHAUMIK was registered on …………………., and fulfilled the residence and other requirements for submitting the thesis for the Ph.D. (Engg) degree of this University as per rules. The thesis is a genuine piece of research carried out by the candidate under our supervision and neither his thesis nor any part of part of the thesis has been submitted for any degree / diploma or any other academic award anywhere before. In our opinion this is a fit piece of work for submission for the Ph.D. degree.

1. -------------------------------------------Signature of the supervisor and date with Office Seal

2.--------------------------------------Signature of the supervisor and date with Office Seal

ABSTRACT This dissertation deals with nonlinear filtering and estimation methods for systems with parametric uncertainties. The work focuses primarily on risk-sensitive estimators, which are known to be robust in the presence of uncertainties in the system parameters. Risk sensitive filters (RSF), which are based on risk-sensitive control law, minimize the expected value of an exponential of a convex function of the estimation error. A designer-chosen parameter, called the risk-sensitive parameter, provides a tool for design trade-off between the filtering performance for the nominal model and the robustness to model uncertainty. Though the work was motivated by the need to evolve improved methods of filtering for aerospace systems, the scope of the thesis has been broadened to include non-aerospace problems as well. Thus the dissertation deals with aerospace problems and other nonlinear problems with uncertain system model, un-modeled biases, unknown inputs and non-nominal noise covariance. Though existing literature on risk-sensitive filters claims that these filters are robust, the implementation of these filters for nonlinear problems had been practically impossible as these filters admit closed form expressions only for a very limited class of models including finite statespace Markov chains and linear Gaussian models. Until very recently, the only method of implementation of risk-sensitive filters for nonlinear problems was by using the extended Risk sensitive filter (ERSF), which uses an EKF-based linearization method. However, all the shortcomings of the EKF are inherited by the ERSF. In this dissertation, some novel filters have been proposed, which significantly extend the range of applications of risk-sensitive techniques to nonlinear, non-Gaussian systems. The proposed filters would enable one to study the properties of risk-sensitive filters and to explore the possibility of application of RSFs for a wide variety of problems including aerospace problems like target tracking and navigation. Some simple test problems have been selected for characterizing the existing filters and testing the applicability of developed filters. Though it cannot be claimed that RSFs have been found to be superior to other existing (risk-neutral) nonlinear filters, this dissertation makes an effort to summarize the results of comparison of the various filters for each chosen application area.

i

The salient contributions of this research work are as follows: •

Development of Risk sensitive unscented Kalman filter (RSUKF), which is a novel method for non-linear risk-sensitive estimation based on the unscented Kalman filter. (Paper accepted for publication Applications, UK, 2008.)

•

in

IET

Control

Theory

and

Development of Central difference risk-sensitive filter (CDRSF), which is a novel numerically efficient algorithm for risk-sensitive filters of nonlinear plants, using central difference approximation. (Paper published in IEEE Signal Processing Letters, 2007.)

•

Development of Risk sensitive particle filter (RSPF), which is a novel particle implementation of Risk-sensitive filters (RSF) for nonlinear, non-Gaussian statespace models based on a probabilistic re-interpretation of the RSF recursions. (Paper accepted for publication in Elsevier Journal of Signal Processing and available online since 1st October, 2008. One IEEE INDICON conference papers published in 2006 have been cited and critically appreciated by international researchers.)

•

Development of Adaptive grid risk sensitive filter (AGRSF), which is a novel adaptive grid based method employing the point-mass approximation for computation of risk-sensitive state estimates in non-linear non-Gaussian problems. (Work presented and published in the proceedings of the IEEE Nonlinear Statistical Signal Processing Workshop, Cambridge, UK, 2006 and IEEE INDICON, 2005.)

•

Formulation of an approach for preliminary design of guidance system for homing missiles by using the Cramer Rao Bound (CRB) to provide a quantitative understanding of the influence of model parameters and instrumentation/signal processing capabilities of the tracking filter performance, without going into the specifics of filter design and elaborate Monte Carlo simulation for performance analysis. (Work presented and published in the proceedings of the IEEE International Symposium on Signal Processing and Information Technology (ISSPIT), Rome, Italy, 2004, paper published in the Journal of the Institution of Engineers, India, 2006.)

•

Comparative performance analysis of existing and proposed filters for the Bearing only tracking problem with different measurement models.

•

Application of the filters to a self-alignment problem for Inertial Navigation System.

ii

ACKNOWLEDGEMENTS My supervisors Dr. Smita Sadhu and Professor Tapan Kumar Ghoshal deserve my deepest gratitude. This thesis is, to a large extent, a result of their skillful meticulous guidance and encouraging support to my work. I would also like to acknowledge them for their valuable comments suggestions during work and review of manuscript at different stages that have lifted the thesis to a level I never would have reached on my own. I would like to thank my co-worker M Srinivasan for research cooperation, comparing simulation results and fruitful discussions. The work has been partly sponsored by Aeronautical Research and Development Board (India) and Council of Scientific & Industrial Research (CSIR). I thankfully acknowledge their support. I have great pleasure in expressing my thanks to, Prof. Shyamal Kumar Goswami, Prof. Kalyan Kumar Dutta and Prof Samar Bhattacharya, Jadavpur University, for their encouragement. I also wish to thank the successive Heads of Electrical Engineering and the Coordinators of Center for Knowledge Based System for providing research infrastructure. Lastly I would like to express my heart-felt gratitude to my parents, family members and friends for their love, affection, support and encouragement.

Shovan Bhaumik Jadavpur University, October 2008

iii

LIST OF PUBLICATION International Journal (Published/Communicated) 1. Shovan Bhaumik, Smita Sadhu, T.K.Ghoshal “Risk Sensitive Formulation of Unscented Kalman Filter”, accepted for publication to IET Control Theory & Applications, 2008. 2. Smita Sadhu, M. Srinivasan, S. Bhaumik, T. K. Ghoshal, Central Difference Formulation of Risk-Sensitive Filter, IEEE Signal Processing Letters, Vol 14, issue 6, pp 421-424, June 2007. 3. Smita Sadhu, Shovan Bhaumik, Arnaud Doucet, T.K.Ghoshal “Risk Sensitive Filtering by Particle Methods” accepted for publication in Elsevier Journal of Signal Processing, 2008. 4. S.Bhaumik, S.Sadhu, M.Srinivasan, T.K.Ghoshal, “Adaptive grid method for nonlinear risk sensitive estimation problems”, submitted to International journal of modelling and simulation.

National Journal (Published) 5. S.Bhaumik, S.Sadhu and T.K. Ghoshal “Parametric Performance Analysis of Tracking System Using Posterior Cramer Rao Lower Bounds” Journal of Institution of Engineers (India), electrical engineering division, issue 3, vol 47, December 2006.

Papers Presented/Published in International Conferences 6. S.Bhaumik, M. Srinivasan, S. Sadhu, T.K.ghoshal “A Risk Sensitive Estimator for Nonlinear Problems using the Adaptive Grid Method” in Proceedings of Nonlinear Statistical Signal Processing Workshop- University of Cambridge, UK 2006. 7. S. Sadhu, S. Bhaumik, and Prof. T. K. Ghoshal “Homing Guidance Configuration using Cramer Rao Bounds”- in Proceedings of 4th IEEE international symposium on signal processing and information technology, Rome, Italy, pp 437-440, Dec 2004. 8. M.Srinivasan, S.Bhuamik, S.Sadhu, T.K.Ghoshal, “Exo-Atmospheric IR Tracking of Ballistic Objects”, in Proceedings of International Conference of IEEE INDICON 2005, IIT Madras, Chennai, India, Dec 12-13, 2005

iv

9. S.Bhaumik, S. Sadhu, T.K.Ghoshal “Risk Sensitive Estimators for Inaccurately Modelled Systems”, in Proceedings of International Conference of IEEE INDICON’2005, IIT Madras, Chennai, India, Dec 12-13, 2005. 10. S.Bhuamik, M.Srinivasan, S.Sadhu, T.K.Ghoshal, “Adaptive Grid Solution of Risk Sensitive Estimator Problems”, in Proceedings of International Conference of IEEE INDICON 2005, IIT Madras, Chennai, India, Dec 12-13, 2005. 11. S.Bhaumik, S.Sadhu, and T.K.Ghoshal, “Alternative Formulation for Risk Sensitive Particle Filter (Posterior)”, in Proceedings of International Conference of IEEE INDICON’2006, New Delhi, India, Sep 15-17, 2006.

Papers Presented/Published in National Conferences 12. S. Bhaumik, S. Sadhu “Auxiliary Particle filter for Bearing Only Tracking” in proceedings of 28th national systems Conference, Vellore institute of technology, pp 263-267, Dec 2004. 13. S.Bhaumik, S. Sadhu and T.K.ghoshal “Formulation of LMI Based Risk Sensitive Estimator”, in proceedings of International Conference on “Emerging Trends in Electrical Engineering” Kolkata, January 12-14, 2007.

v

CONTENTS

Abstract Acknowledgement List of Publication Content List of Figures List of Table Chapter 1 Introduction 1.1Motivation 1.2Thesis Objective 1.3 Background on aerospace Problem Domain 1.3.1 Target Tracking 1.3.2 Navigation and Its Use 1.4Background in Solution Domain 1.4.1 Background on Filters and State Estimators 1.5Problem Statement and Scope 1.6The Approach and Methodology 1.7Contribution 1.7.1 Publication Generated from this Work 1.7.2 Credits to Co-Workers 1.8Organization and Content of the Thesis Chapter 2 Literature Survey 2.1Introduction to Literature Survey 2.2Problem Domain Literature Survey 2.2.1 Tracking of Maneuvering Target 2.2.1.1 Bearing Only Tracking 2.2.1.2 Ballistic Object Tracking 2.2.2 Navigation 2.2.2.1 Estimation Problems in Inertial Navigation 2.2.2.2 Literature on Error Models 2.2.2.3 Alignment Problem

2.2.2.4

2.2.2.3.1 Self Alignment: Coarse 2.2.2.3.2 Self Alignment: Fine 2.2.2.3.3 Transfer and Aided Alignment 2.2.2.3.4 In Flight Alignment 2.2.2.3.5 GPS Based Alignment GPS INS Estimation and Data Fusion 2.2.2.4.1 GPS INS Estimation Using Kalman Filter

vi

i iii iv vi xi xiv

1 3 3 3 5 6 7 8 9 10 11 11 12

13 13 13 14 16 19 21 22 23 24 25 27 28 29 29 30

2.2.2.4.2 GPS INS Estimation Using Post Kalman Filter 2.3Literature Survey Solution Domain 2.3.1 Linear Regression Kalman Filter (LRKF) Techniques 2.3.1.1 Unscented Kalman Filter (UKF) 2.3.1.2 Central Difference Filter (CDF) and Divided Difference Filter (DDF)

33 34 35 35 36

2.3.2 Pseudo Measurement Filter 2.3.3 Approximate Grid Based Filter 2.3.3.1 Adaptive Grid Filter 2.3.4 Particle Filter 2.3.5 Robust Filtering 2.3.6 Robust H2 and H ∞ Filter 2.3.6.1 Riccati Equation Based Approach 2.3.6.2 LMI Based Approach 2.3.7 Risk Sensitive (RS) Filtering 2.3.7.1 Nonlinear Risk sensitive Filter 2.3.8 Cramer Rao Lower Bound (CRLB) 2.4Discussion and Conclusion

37 38 38 39 41 41 41 43 44 46 47 48

Chapter 3 Test Problems 3.1 Introduction 3.2First Order Linear System (L1) 3.3Second Order Linear (L2) 3.4First Order Nonlinear Problem (N1-1) 3.5Another First Order Nonlinear System (N1-2) 3.62nd Order Bearing Only Tracking (BOT) Problem (non-linear) 3.6.1 BOT Problem Formulation 3.6.2 Measurement Model I 3.6.3 Measurement Model II 3.6.4 Measurement Model III 3.6.5 Filter Initialisation

49 49 49 50 51 52 53 54 56 57 58

Chapter 4 Risk Sensitive Estimator 4.1 Introduction 4.2 Formulation of Risk Sensitive Estimation Problem 4.3 Recursive Solution of Risk Sensitive Estimation 4.3.1 Posterior Solution 4.3.2 Prior Solution 4.4 Risk Sensitive Estimation For Linear Gaussian Problem 4.4.1 Posterior Risk Sensitive Kalman Filter 4.4.2 Prior Risk Sensitive Kalman Filter 4.5 LMI Based Formulation of RSKF 4.5.1 LMI Based Kalman Filter Formulation

59 60 61 61 62 63 63 64 65 65

vii

4.5.2 LMI Based Risk Sensitive Kalman Filter Formulation 4.6 Robustness Properties of RSKF 4.6.1 Robustness Properties 4.6.2 Pragmatics Related to RS Parameter 4.7 Numerical Experiment and Illustration for RSKF 4.7.1 Sensitivity to State Model Error 4.7.2 Sensitivity to Unmodelled Bias 4.7.3 Sensitivity to Noise Modeling Error 4.8 Discussion and Conclusion Chapter 5 Risk Sensitive LRKF 5.1 Introduction 5.2 Risk sensitive Filter 5.3 CDRSF for Prior Estimation 5.3.1 Relations for Prior Estimation 5.3.2 Implementation of CDRSF for Prior Estimation 5.4 Formulation of Risk Sensitive Unscented Kalman Filter 5.4.1 The Prior Form of RSUKF 5.4.2 The Posterior Form of RSUKF 5.4.3 Risk-Sensitive Unscented Kalman Filter Algorithm for Prior Estimation 5.4.3.1 General RSUKF Algorithm for Nonlinear Systems 5.4.3.2 Special Cases 5.4.3.3 Scaled Risk Sensitive Unscented Kalman Filter (Scaled RSUKF) 5.5 Case Study 5.6 Scope for Future Work 5.7 Discussion and Conclusion

66 67 68 69 69 69 70 73 74

75 76 77 77 78 79 80 81 82 82 83 84 86 89 91

Chapter 6 Adaptive Grid Risk Sensitive Filter 6.1 Introduction 6.2 Grid Based Solution of Risk Sensitive Estimation Problem 6.2.1 Risk Sensitive Estimation Problem 6.2.2 Recursive Solution of Risk Sensitive Estimation 6.2.3 Grid Formulation 6.2.3.1 Representing Probability Densities as Point Mass on Grid Points 6.2.3.2 The Grid based RSE Algorithm 6.2.3.3 Weakness of Simple Grid Based Risk Sensitive Filter 6.3 Grid Adaptation Techniques 6.3.1 Obtaining Approximate Boundaries of the Region of Interest 6.3.2 Selecting Grid Intervals 6.3.3 Alignment of Axes

viii

92 94 94 95 95 95 96 97 98 99 100 102

6.4 6.5 6.6

6.7

6.3.4 Determination of Approximate Posterior Distribution Search Algorithm Implementation of Risk Sensitive Adaptive Grid Filtering Algorithm 6.5.1 Notes on AGRSF Algorithm Example and Case Study 6.6.1 Case Study I First Order Linear System 6.6.2 Case Study II Second Order Linear System 6.6.3 Case Study III First Order Nonlinear System 6.6.4 Case Study IV Second Order Nonlinear System (BOT Problem) 6.6.4.1 Problem Description 6.6.4.2 Understanding the Intermediate Steps in Simulation 6.6.4.3 Simulation Results Discussion and Conclusion

103 105 106 108 110 110 111 114 116 116 116 120 123

Chapter 7 Risk Sensitive Particle Filter 7.1 Introduction 126 7.2 Formulation of Risk Sensitive Estimation Problem 127 7.3 Prior Risk Sensitive Particle Filter Formulation 128 7.3.1 Solution of Risk Sensitive Estimation Problem for Prior Estimation 128 7.3.2 Probabilistic Interpretation 129 7.3.3 Particle Implementation 129 7.3.4 Optimal Proposal 130 7.4 Posterior Risk Sensitive Particle Filter Formulation 132 7.4.1 Solution of Risk Sensitive Estimation Problem for Posterior 132 Estimation

7.5 7.6

7.7 7.8 7.9

7.4.2 Probabilistic Interpretation 7.4.3 Particle Implementation 7.4.4 Optimal Proposal Notes Case Study 7.6.1 1D Nonlinear Case 7.6.2 2D Bearing Only Target Tracking Convergence Study Scope for Further Work Discussion and Conclusion

132 133 134 135 136 136 138 139 141 142

Chapter 8 Cramer Rao Lower Bound and Preliminary Design for Target Tracking Systems 8.1 Introduction 8.2 CRLB Computation 8.3 Parametric Studies of BOT Problem Using CRLB 8.3.1 Nominal Performance Comparison 8.3.2 Influence of Sampling Time on CRB

ix

143 145 146 147 147

8.3.3 Influence of Measurement Error Covariance 8.3.4 Influence of Process Excitation Covariance 8.3.5 Influence of P22,0 8.3.6 Adding Another Measurement 8.3.7 Influence of Eclipsing Effect 8.4 Preliminary Tracking System Design Using CRLB 8.5 Discussion and Conclusion

149 151 151 153 154 155 156

Chapter 9 Risk Sensitive Filter For INS Self Alignment 9.1 Introduction 9.2 Problem Formulation 9.2.1 Velocity Error Equation 9.2.2 Attitude Error Equation 9.3 Estimation of Misalignment Angle Considering Biases in State Variable 9.4 Estimation of Misalignment Angles Considering Biases as Known Input 9.5 Effect of Unmodelled Bias 9.6 Risk Sensitive Kalman Filter With Unmodelled Bias 9.7 Misalignment Angle Estimation Using Gyro Output as Measurement 9.8 State Space Model For Large Misalignment Angle 9.8.1 Alignment Considering Biases as Known Input 9.8.2 Alignment in Presence of Unmodelled Bias 9.9 Discussion and Conclusion

158 159 160 161 162 166 169 172 174 177 178 180 181

Chapter 10 Discussions and Conclusions 10.1Discussions 10.2Conclusions 10.3Suggestion for Further Research

183 183 185

Bibliography

187

Appendices

204

x

LIST OF FIGURES Figure 3.1 Figure 3.2 Figure 3.3 Figure 3.4 Figure 3.5 Figure 3.6 Figure 3.7 Figure 3.8 Figure 3.9 Figure 3.10

Truth value for state stabilizes around the equilibrium point -1 Measurement for state when stabilizes around –1 equilibrium point Truth values for state stabilizes around the equilibrium point +1 Measurement for state stabilizes around +1 equilibrium point Tracking platform kinematics. Truth value of position for a particular run Truth value of velocity for a particular run Measurement versus position versus time Signal to noise ratio (with and without eclipsing) Measurement error covariance with eclipsing

51 51 52 52 54 56 56 57 57 58

Figure 4.1 Figure 4.2 Figure 4.3 Figure 4.4 Figure 4.5 Figure 4.6 Figure 4.7

RMSE of first state variable for KF and RSKF RMSE of 2nd state variable for KF and RSKF Terminal RMSE of 1st state variable for KF and RSKF for different δ The truth and estimated value of the state with unknown bias Truth and estimated value of 1st state variable with unknown bias for 2D case RMSE for 1st state with unknown bias 2D case RMSE for RSKF and KF for Q mismatch

70 71 71 72 72 73 73

Figure 5.1 Figure 5.2 Figure 5.3 Figure 5.4 Figure 5.5 Figure 5.6

Identical nature of RSUKF and RSKF for linear Gaussian system Performance of ERSF and RSUKF for a representative run RMSE plot of ERSF, CDRSF and RSUKF for 1000 runs RMSE of position obtained from UKF & RSUKF RMSE of velocity obtained from UKF & RSUKF Position estimation obtained from ERSF and RSUKF for large initial position error Velocity estimation obtained from ERSF and RSUKF for large initial position error

87 87 88 89 90 90

Figure 5.7

Figure 6.1 Figure 6.2 Figure 6.3 Figure 6.4 Figure 6.5 Figure 6.6 Figure 6.7 Figure 6.8 Figure 6.9 Figure 6.10 Figure 6.11

Idea of Grid and supports Selections of grid intervals Alternative method for selection of grid intervals Alignment of axes to reduce wastage of grid Determination of mean and co variance from EKF Comparison of AGRSF and KF for linear Systems Convergence of AGRSF with number of grid points Plot of initial support points (16 X 16) and their respective area Posterior probability density function at intermediate step Truth and estimated value of first state variable Truth and estimated value of first state variable

xi

90

97 101 102 103 105 110 111 112 113 113 113

Figure 6.12 Figure 6.13 Figure 6.14 Figure 6.15 Figure 6.16 Figure 6.17 Figure 6.18 Figure 6.19 Figure 6.20 Figure 6.21 Figure 6.22 Figure 6.23 Figure 6.24 Figure 6.25 Figure 6.26

Truth & Estimated value for ERSF, RSPF and AGRSF. RMSE of ERSF and AGRSF for 1000 MC run. Plot of probability density of estimated states with time Plot of probability density of estimated states with time for grid starvation case BOT point mass distribution after time update Likelihood distribution 50 X 50 supports Posterior distribution with 50 X 50 supports Posterior distribution with 50 X 50 supports and 3 span factor Posterior distribution with 20 X 20 support points Truth and estimated value of position Truth and estimated value of velocity Absolute error of position Absolute error of velocity Convergence of position error with number of grid points Convergence of velocity error with number of grid points

115 115 115 116 117 118 118 119 119 120 121 121 122 122 122

Figure 7.1 Figure 7.2 Figure 7.3 Figure 7.4 Figure 7.5 Figure 7.6 Figure 7.7 Figure 7.8

Truth & estimated value of state obtained from ERSF, RSPF and AGRSF RMSE of ERSF, RSPF and AGRSF for 1000 MC run RMSE of position for different risk sensitive parameter RMSE of velocity for different risk sensitive parameter Variance of error across MC runs vs no of particles for 1D nonlinear system RMSE of position for different particle RMSE of velocity for different particle Variance of error for position vs no. of particles in 2D BOT problem

137 137 138 139 140 140 141 142

Figure 8.1

Comparison RMS error performance of EKF and PF with LBRE of position at nominal condition of R, P22,0 , Q and T

148

Figure 8.2

Comparison of RMS error performance EKF and PF with LBRE of velocity at nominal condition of , P22,0 , Q and T

148

Figure 8.3 Figure 8.4 Figure 8.5 Figure 8.6 Figure 8.7 Figure 8.8 Figure 8.9 Figure 8.10 Figure 8.11 Figure 8.12 Figure 8.13 Figure 8.14

Influence of the sampling time on LBRE of position Influence of the sampling time on LBRE of velocity Influence of measurement error covariance on LBRE of position Influence of measurement error covariance on LBRE of velocity Influence of process excitation covariance on LBRE of position Influence of process excitation covariance on LBRE of velocity Influence of P22,0 on LBRE of position Influence of P22,0 on LBRE of velocity Effect of introducing sight line rate measurement (position) Effect of introducing sight line rate measurement (velocity) Effect of Changing SNR on LBRE of Position Effect of Changing SNR on LBRE of velocity

149 150 150 151 152 152 153 153 154 154 155 155

Figure 9.1

RMSE for east misalignment estimation considering biases as state variable

165

xii

Figure 9.2 Figure 9.3 Figure 9.4 Figure 9.5 Figure 9.6 Figure 9.7 Figure 9.8 Figure 9.9 Figure 9.10 Figure 9.11 Figure 9.12 Figure 9.13 Figure 9.14 Figure 9.15 Figure 9.16 Figure 9.17

RMSE for north misalignment estimation considering biases as state variable RMSE for azimuth misalignment estimation considering biases as state variable Estimation error of east misalignment angle for a typical run Estimation error of north misalignment angle for a typical run Estimation error of azimuth misalignment angle for a typical run RMSE of east misalignment angle for 100 MC run RMSE of north misalignment angle for 100 MC run RMSE of azimuth misalignment angle for 100 MC run RMSE of azimuth misalignment angle for different unmodelled gyro biases RMSE of east misalignment angle for different unmodelled accel bias RMSE of north misalignment angle for different unmodelled accel bias RMSE of azimuth misalignment angle for different unmodelled accel bias RMSE of east misalignment angle under unmodelled bias RMSE of north misalignment angle under unmodelled biases RMSE of azimuth misalignment angle under unmodelled biases RMSE for azimuth misalignment for velocity error only measurement case and additional gyro output measurement case with known input bias

165 165 167 168 168 168 169 169 170 171 171 172 173 173 173 175

Figure 9.18 RMSE of azimuth estimation for different unmodelled gyro biases. Figure 9.19 RMSE of azimuth estimation for different unmodelled gyro biases for higher value of R.

176 176

Figure 9.20 Figure 9.21 Figure 9.22 Figure 9.23 Figure 9.24 Figure 9.25 Figure 9.26

177 179 179 179 180 181 181

KF and RSKF for gyro output measurement situation RMSE of east misalignment angle RMSE of north misalignment angle RMSE of azimuth misalignment angle RMSE of east misalignment angle with unmodelled bias RMSE of north misalignment angle with unmodelled bias RMSE of azimuth misalignment angle with unmodelled bias

xiii

LIST OF TABLES Table 7.1

Time Calculation for a single run of RSPF and AGRSF for different number of grid points and particle

138

Table 9.1

RMS error settling values for different unmodelled gyro and accelerometer bias

166

xiv

1 Introduction 1.1

Motivation Many real world problems need estimation of unknown states and other parameters of

dynamic systems using available noisy observations. Such problems include tracking an intruding aircraft or missile using radar or seeker measurements, filtering of digital communication signals using noisy measurements, monitoring, diagnosis and prognosis of industrial equipments, speech processing and image processing from noisy data, localization of mobile robot, navigation using GPS INS fusion, or estimation of the volatility of financial instruments using stock market data. In most of these applications prior knowledge of the system is available although the knowledge may not be accurate. So the challenge is to estimate the values of the state variables of the system accurately from the noisy observation and prior inaccurately known process dynamics. The broad heading under which most of such topics are discussed is called Signal Processing. If the data and system dynamics can be modelled in linear Gaussian state space, it is possible to derive an exact analytical expression to compute the optimal posteriori distribution at any instance of time. If the problem is formulated in discrete domain for computational simplicity, the state can be obtained recursively. This recursion is well known as the Kalman filter. Optimality of Kalman filter depends on various assumption such as (i) linearity of the model, (ii) whiteness and Gaussianity of the noise sequence and (iii) absence of correlation between process and measurement noise. Violation of these assumptions may lead to non-optimal solution of the estimator. In real life application most of the systems do not follow the above assumptions due to nonlinearity and non-Gaussianity. During the last four decades considerable amount of research has been done to estimate the states in an approximate way when the signal model is nonlinear and/or non-Gaussian. Many approximation schemes such as extended Kalman filter (EKF), central difference filter (CDF), Gauss-Hermite filter (GHF), unscented Kalman filter (UKF) etc are proposed for this purpose. With the increase of computational power, the possibility of computationally intensive but more accurate numerical solutions for such problems (nonlinear and/or non-Gaussian) have also been explored and as a result estimators such as grid-based filter, particle filter have been proposed. All these techniques for nonlinear estimation problems, are sometimes collectively referred as “Post Kalman” filtering technique. Although substantial work has been done on nonlinear filtering techniques, but still as of now, no optimal estimator exists for nonlinear estimation problem. So estimation of states in real life still remains a challenging job to a practitioner

Chapter 1

especially for the system with high nonlinearity and poor observability. Many of such techniques have been reviewed in the recent monograph by Ristic et. al [Ristic 2004a], entitled as “ Beyond Kalman Filter”. It may be noted that the present work was started before the book has been published. So from the above discussion it is clear that the post Kalman filtering techniques are capable of accommodating nonlinear systems and non-Gaussian noise and are therefore considered or claimed to be powerful. The advanced techniques in post Kalman filtering method such as unscented Kalman filter (UKF), particle filter (PF) etc are claimed to be superior in terms of accuracy in estimation. Although it should be kept in mind that the superiority of the proposed novel technique can wrongly be demonstrated by choosing a problem carefully. In real life scenario the choice of appropriate filter for specific mission is again very difficult. There is no direct method by which the best filter for the specific problem can be chosen. The designer needs to first list down the probable techniques and implement them for that specific problem or similar problems. A comparison table regarding the accuracy and computational time need to be built. From that table the designer can choose the most suitable filter for that particular mission profile. Nonlinear filtering therefore calls for substantial experimentation. Again it has been realised that if the system and or noises are not perfectly modelled (that means if the system contains modelling uncertainty) the traditional filters may not track the truth accurately enough. The so named “robust filters” have been proposed to deal with the situations where modelling uncertainty is substantial. Some robust estimators such as robust Kalman filter, H2 and H ∞ filter, risk sensitive filter etc are available in literature to cope up with the problem due to uncertain truth. Most of these filters have been studied for linear Gaussian cases. The present work attempts to extend such techniques for nonlinear and possibly non-Gaussian problems. In particular, for this thesis work, risk sensitive filtering has been chosen for the subject of study and further enhancement. The risk sensitive estimator originates from risk sensitive control law, which minimises an exponential cost criterion. Like Kalman filtering the closed form solution for risk sensitive filtering exists only for linear Gaussian case. Risk sensitive filtering theory has been extended to non-linear systems in [Jayakumar 1998] where an EKF like approach has been adopted. However, the well-known limitations of the EKF, including smoothness requirement for the functions and noise Gaussianity restriction, are also transferred to extended risk sensitive filtering. To cope up with the limitation of existing extended risk sensitive filtering (ERSF) in this thesis some novel techniques for nonlinear risk sensitive estimation have been proposed. So the primary motivation for the present work had been to

2

Chapter 1

(i) Investigate the properties of risk sensitive filtering (ii) Develop novel advanced methods for nonlinear risk sensitive estimation problems (iii) Investigate the scenarios where risk sensitive filter can out perform the non-robust filters. To obtain meaningful insight and properties of the proposed novel nonlinear risk sensitive filtering these methods are applied to (i) known nonlinear poorly observable state estimation problems, and (ii) selected tracking and navigation problem.

1.2

Thesis Objective With the above motivation and background, the objective of the present work was cast as, to (i)

Develop novel estimation and filtering methods for nonlinear problems with emphasis on risk sensitive filters.

(ii)

Investigate the properties of risk sensitive filtering

(iii)

Investigate the scenarios, with emphasis on the aerospace problems where the developed filter algorithms can out perform the existing filters.

(iv)

To examine whether the Crammer Rao or similar bound can be utilized for system level choice of sensors and algorithms.

To obtain meaningful insight and properties of the proposed novel nonlinear risk sensitive filtering these methods are applied to (i) known nonlinear poorly observable state estimation problems, and (ii) selected tracking and navigation problem.

1.3

Background on Aerospace Problem Domain The area of interest of the present work is in the domain of aerospace. However the scope

must be further narrowed down. In aerospace specifically we are interested in two problem areas, namely target tracking, and navigation where the estimation techniques play key roles. For reasons other than pure technical, the application area of this work had a bias towards defence related application. A brief background of the domain is covered in this section. 1.3.1

Target Tracking

In the next chapter a comprehensive literature survey on target tracking has been presented and hence the discussions here would be brief. In our present context target tracking means to know the position about an enemy aircraft or enemy ship, missile, or other potentially moving object, which needs to be intercepted and or destroyed from the military point of view. The tracking system means the system from which the target is being tracked. The tracking system may be stationary, on ground or mounted in a moving

3

Chapter 1

platform like ship, aircraft, submarine, missile or torpedo. As an example for target tracking anti aircraft missile continuously track the path of the enemy aircraft. The position of the target can be up linked from the ground station or can be determined on board using on board seeker. Now a days generally the initial phase target tracking is done with the help of ground radar, while the seeker continuously searches for the target and once located, the seeker keeps the target in its field of view (tracking). Once the seeker in the missile is locked to the target the missile tracks the target on its own. Ground radar can track multiple targets. Once the tracks are generated the number of targets, and states of the target such as position, velocity, acceleration can be estimated and predicted with the help of state estimators. The topic of target tracking involves a broad area of research such as (i) target modelling and dynamics, (ii) measurement models (iii) Filtering and prediction (iv) sensor data fusion So the key of success in target tracking lies in effective extraction of the information about the target state from the available observation from sensors. The sensor data may be obtained from the ground radar or on board seeker. In the initial stage of the flight, the radar data are important. The sensor data may be the bearing angle, angle rate, range, range rate or combinations of the above. The effectiveness of target tracking depends on the assumed target model’s good ness of fit for the particular engagement scenario. It is generally argued that a good model is worth than a thousand pieces of data. Most tracking algorithm are model based because prior knowledge of the target is available. It is observed that a good model based tracking algorithm will out perform the tracking mechanism without model. And a large amount of effort has been devoted to model the target in different engagement scenarios. For example if it is assumed that the target will not manoeuvre, then non manoeuvring model can be used for the target to be tracked. If it is assumed that target may manoeuvre, different manoeuvring models can also be used. Some times, decision based method is used to detect the manoeuvre of the target and switching between the two types of model is also very common. A comprehensive literature survey has been done by Li and Jilkov [Li 2003] on the different model for both manoeuvring and non manoeuvring target tracking. Successful target tracking also depends on good measurement model and less noisy sensor data. For example, if the measurement model of the system is highly nonlinear, the accuracy in target tracking degrades considerably. In fact, it is practically observed that in different co ordinate system, the accuracy of estimator is different. It is also important to know the noise characteristics of the sensors. It has been observed that more sensors or more observations some times help to improve the estimation accuracy about the state of a target.

4

Chapter 1

Another important aspect for successful tracking of the object is to choose efficient filtering algorithm to estimate the state of the target. As the target and sensor measurement model are generally nonlinear and no optimal filter exists for non-linear system the choice of estimation technique is not unique. The choice of filtering algorithm for a particular engagement scenario requires high insight and understanding of the problem as well as well awareness of the properties of different filtering techniques exist for non linear estimation. It is important to note that the assumed dynamics of the target cannot always be captured through model and the noise associated with the sensor cannot always be known perfectly. So the performance of the filtering algorithm may degrade in real life scenario due to mismatch in modelling. In this work keeping these factors in mind, some novel methods for nonlinear robust filtering which might be useful in target tracking problems have been proposed. 1.3.2

Navigation and its Use

The basic function of a navigation system is to accurately determine the position of a moving vehicle in which it is mounted. The navigation system has three functions (i) sensing the motion (ii) computing and (iii) outputting the information periodically. The history of evolution of navigation technology is very interesting and it has been well documented in [Kayton 1988]. Although, now a days, the technology for navigation of terrestrial, subsurface and airborne vehicles is very similar, we will keep focus on navigation technology for aerospace vehicles. The mainstay of modern navigation system is Inertial Navigation (INS), which uses accelerometers and gyros (Gyroscopes, rate gyro). The fundamental principle of inertial navigation system is fairly simple. An accelerometer measures the acceleration of the vehicle while the gyroscope measures the angular rate. If we know the initial position, velocity and attitude very accurately, integrating the output from the accelerometer and gyro, we can be able to know the position velocity and attitude of the flight at any instance of time. INS system may be gimbaled (mechanized platform) system or strap down system. In gimbaled system, a stabilized physical platform is maintained through a servo system on which the accelerometers and gyros are mounted and thus isolated from the rotations of the vehicle. If this mechanism can be achieved, the platform does not experience any rotation relative to navigation frame in spite of vehicle motion. In this approach, accelerometers aligned with the platform measures along the navigation co ordinate system axis. Vehicle attitude is determined by measuring the relative angles between the vehicle and platform axis. In “strapdown” INS there is no stabilized platform. The accelerometers and gyros are mounted on the vehicle body directly. The required reference frame is to be maintained mathematically by a computer. This, however, increases the on board computation load.

5

Chapter 1

Being an integrating system, the position output must be initialised. That means the initial angular and linear positions must be known. The method by which the angular orientation of an inertial navigation system (INS) is determined with respect to the reference axis system is called alignment procedure. There are two fundamental types of alignment process: self-alignment and transfer alignment [Kong 2000]. Self-alignment is carried out when the platform is nearly stationary. Normally, the self alignment process is divided into two phases, namely coarse alignment and fine alignment. The purpose of coarse alignment is to provide fairly good initial condition for fine alignment process. Coarse self alignment often uses raw gyro and accelerometer outputs (from IMU) and compares them with gravity and earth rate. Fine self-alignment is not carried out using IMU output but with its own navigation computer output (INS) of velocities. In transfer-alignment, one INS is aligned with the help of another instrument (say INS). As an example, we can take a fighter aircraft carrying missiles in it. Here the carrier aircraft has its own INS and the missiles also have their own INS. It is necessary to align the INS (slave INS) of the missile with the aircraft INS (Master INS). Transfer-alignment may be carried out in quasistationary as well as in motion. It is traditionally carried out by comparing linear velocities of the master and test INS's. So estimation methods can be applied to determine misalignment angle between MINS and SINS by fusing the velocity mismatch measurement with navigation dynamics. Motion (intentional manoeuvre or translatory motion) may be designed to improve observability and quicker convergence of the estimators. As the navigation equations are nonlinear, gyro and accelerometer contain biases and observability is not very good for all alignment problems, there is immense scope to improve alignment procedure by applying advanced filtering technique. In the present work, we focus on the application of nonlinear risk sensitive filtering technique for the alignment problem.

1.4

Background in Solution Domain The dissertation is on the development of novel techniques for nonlinear risk sensitive

estimation problem and application of such filterings in target tracking and navigation. In this section a very brief background has been provided on state estimation techniques. For more appreciation, this background information has been provided in a simplistic non-mathematical way with few references. References for background work have been provided in “literature survey” in the next section of the thesis.

6

Chapter 1

1.4.1

Background on Filters and State Estimators

Many excellent textbooks are available [Anderson 1979] on state estimation. Here some important features on (non-robust) estimation are mentioned as background. •

State estimators require (i) state equation or process dynamics of the system, called the state model (ii) measurements equation or the measurement model. In real life problems estimation of state becomes difficult due to nonlinear system dynamics and measurement system.

•

The state dynamics and measurement model can be represented in continuous time or discrete time. Here in the thesis mainly the discrete mode of representation has been taken.

•

In a general way, the process dynamics and measurement model can be written as State Equation: x k +1 = f k ( x k , u k , wk )

(1.1)

Measurement Equation: y k = hk ( x k , v k )

(1.2)

Here x k is the states of the system, u k is the known input, wk is random unknown input known as process noise, y k is the measurement and vk is measurement noise. •

In the special simplified case, the noise may be additive. The above two equations can be written as x k +1 = f k ( x k , u k ) + wk (1.3)

y k = hk ( x k ) + v k

(1.4) From now on, we shall only consider the discrete additive noise situation. The noise

wk and vk may or may not be uncorrelated and may not be white Gaussian sequence. •

To date, no general closed form solution for nonlinear problems is available. Most filtering solutions to such problems are suboptimal or approximate.

•

If the noises are not white, a method is available to whiten the noises and after that standard Kalman filter can be applicable. Similarly if the noises are correlated then also the Kalman filter can be applied with some modification in algorithm.

•

The process noise has great significance in real life cases. It represents the unknown and random nature or the modelling error of the process dynamics. In particular target tracking scenario process noise may signify the unknown maneuver due to pilot or due to pre program stored on onboard computer of the target. If the plant is substantially deterministic (means that having no modelling error or unknown excitatory) then that process noise part can be set as zero. In simplified cases the process noise is assumed as zero mean white Gaussian noise, its covariance generally expressed with the symbol Q.

7

Chapter 1

•

The sensor measurements are corrupted with the sensor noises. Sensor noise characteristics can be found out from the experimental data. Generally the sensor noise is assumed as zero mean white Gaussian noise sequence. The covariance matrix of the noise is generally symbolically expressed with R.

•

To run the filter, the state variables need to be initialised with the prior knowledge. Also initial error covariance of the states needs to be assigned. More the confidence designer has with initialisation less will be the error covariance and vice versa. More accurate is the initialisation quicker is the convergence.

•

It is worthy to mention here the statistical significance of covariance matrix for multidimensional Gaussian distribution. The diagonal elements of the covariance matrix represent the square of the standard deviation of the respective variable. The off diagonal term represents correlation coefficients between the corresponding variables. For Gaussian probability distribution more is the value of cross correlation coefficients more the bulginess of the probability density curve.

•

In real life application of the Kalman filtering the big challenge is to select Q and R. In real life scenarios, sometimes it happens that the noise covariance or the characteristics of the process and measurement noise remains unknown. In such a situation, estimation of noise covariance is necessary before implementation of estimation technique. That type of filtering where the noise covariances are also estimated online, is known as adaptive filtering [Mayer 1976, Noriega 1997].

1.5

Problem Statement and Scope The dissertation is on the development of novel techniques for nonlinear risk sensitive

estimation problem and application of such filterings in target tracking and navigation. Comparatively more emphasis has been given in formulation of novel filtering techniques rather than its aerospace application. In the present work some nonlinear advanced filtering methods, which has also been claimed to be robust under process uncertainty, have been proposed. Prior to this work and work done by the co-worker in the same laboratory [Srinivasan 2007] no good method was available in literature for nonlinear risk sensitive filtering. Initially an existing technique based on extended Kalman filter had been examined which is known as extended risk sensitive filter (ERSF). However it was noted that the ERSF contains all the limitation of EKF and does not perform well in the nonlinear estimation problem. This limitation motivated the author to develop novel techniques, which will provide better results than ERSF.

8

Chapter 1

The methods developed include some approximate algorithms where noise excitation had to be Gaussian. However, system nonlinearities eventually produce non-Gaussian probability density function for the evolving states. The Risk Sensitive Particle filter developed however does not require Gaussian assumption / restriction. For the comparative studies of the filters and to understand the properties of the estimators in a better way, simple target tracking scenarios have been considered. The target tracking examples considered in two dimensional scenario. It is assumed that the target is a non manoeuvring one. In this work, a problem on self alignment procedure has been formulated. The problem is in general nonlinear in nature and under the assumption of small misalignment angle the formulated problem becomes linear. Both the linear and nonlinear problem have been solved using risk sensitive filters. In literature Cramer Rao Lower Bound (CRLB) is well known for theoretically best results if any optimal filter exist. As the selection of filtering technique requires substantial experimentation, a part of the study involved investigation of whether CRLB can play a role in selecting sensors and sampling times without committing to a particular filtering algorithm. Some interesting results have come out of this investigation. It has been established that in the preliminary phase of design of the tracking systems different conflicts can be resolved with CRLB without going in detail design of estimator. Notable exclusions from the scope of this thesis are (i) Manoeuvring target tracking and (ii) H ∞ robust estimators.

1.6

The Approach and Methodology The general approach had been to develop new algorithms by extending existing algorithms,

thereby removing some of the shortcomings. The proposed filters were then tested by using test problems. For the evaluation of the performance of the filters, accuracy in estimation and computational load have been considered. To compare the accuracy between different filtering methods, Monte Carlo runs have been executed. It has been ensured that the number of Monte Carlo population size is adequate. Most of the cases the simulation has been carried out using Matlab software. Although in case of particle filter and grid based filter, the computation became most intense especially in the time of Monte Carlo simulation. In those scenarios, the simulations are carried out using executable code generated by C++ compiler. From C++ codes developed by the worker, The possibility of implementation to on board computer has also been explored.

9

Chapter 1

In some cases the results obtained from different tracking filters are also compared with CRLB to know the degree of optimality of the estimator. These are mostly done in the papers contributed by the present worker for the non risk sensitive type estimators. This is because the CRLB bound for the risk sensitive case is not available.

1.7

Contribution This section will specifically state the contribution of the present work in the background of

earlier work. The contribution made by the present worker may be broadly categorized as follows •

Fairly extensive literature survey on estimation methods and tracking and navigation problems.

•

Choice of test problems.

•

Study the unknown properties of risk sensitive estimator. In this study it has come out that apart from model uncertainty, risk sensitive filters perform better under un modelled bias in simple cases.

•

LMI based formation of risk sensitive filtering for Linear Problems.

•

Formulating novel risk sensitive filtering algorithms for nonlinear problem. Developed novel estimation methods are (i) central difference risk sensitive filter (CDRSF), (ii) Risk sensitive unscented Kalman filter (RSUKF), (iii) Adaptive grid risk sensitive filter (AGRSF), (iv) risk sensitive particle filter (RSPF) both prior and posterior version. The applicability of optimal proposal for RSPF and AGRSF has also been verified.

•

Formulating an approach for preliminary design of target tracking systems using CRLB.

•

Application and evaluation of nonlinear and risk sensitive filtering to tracking problems.

•

Application and evaluation of nonlinear and risk sensitive filtering to alignment of inertial navigation systems.

The first application of nonlinear risk sensitive filtering in BOT problem has been reported in the thesis submitted by Srinivasan [Srinivasan 2007] who worked closely with the present author in the same laboratory. Perhaps the work done by the present author and Srinivasan [Srinivasan 2007] is the first attempt to apply the risk sensitive filtering in target tracking scenario. In the early phase of design life cycle of the interceptor it is necessary to select the filtering strategy and feasibility of the demand made by the designer. It has been established that in this

10

Chapter 1

scenario CRLB helps designer to test feasibility of the mission without going in detailed estimator design. This concept was first reported in [Sadhu 2004a] and later extended by Srinivasan. In another application a problem for inertial navigation system alignment procedure has been formulated. Here risk sensitive filtering has been applied to self alignment problem for inertial navigation system. This can also be claimed as the first attempt to apply risk sensitive filtering in alignment problem. Different aspects of self alignment problem including observability, bias sensitivity have been discussed. The performance of risk sensitive filtering has been compared with their risk neutral counterpart under different situations. 1.7.1

Publication Generated from this Work •

Formulation of Risk sensitive unscented Kalman filter (RSUKF) has been accepted for publication in IET Control Theory and Applications, UK, 2008.

•

Formulation of central difference risk sensitive filter (CDRSF) has been published in IEEE Signal Processing Letters, 2007.

•

Developed Risk sensitive particle filter (RSPF) has been accepted for publication in Elsevier

Journal

of

Signal

Processing

and

available

online

since 1st October, 2008. It has also been presented in IEEE INDICON conference 2006. The publication has been cited and critically appreciated by

international researchers [Orguner 2007, 2008] •

Developed Adaptive grid risk sensitive filter (AGRSF), was published in (i) the proceedings

of

the

IEEE

Nonlinear

Statistical

Signal

Processing Workshop, Cambridge, UK, 2006 (ii) IEEE INDICON, 2005 and also communicated for publication to International journal of modelling and simulation.

•

An approach for preliminary design of guidance system for homing missiles by using the Cramer Rao Bound (CRB) has been presented in the proceedings of the IEEE

International

Symposium

on

Signal

Processing

and

Information Technology (ISSPIT), Rome, Italy, 2004. The work also

published in the Journal of the Institution of Engineers, India, 2006.

1.7.2

Credits to Co-Workers

Sharifuddin Mondal [Sadhu 2006] should be credited to initiate the work on target tracking by applying UKF for the BOT problem. Co-worker Srinivasan worked closely towards the

11

Chapter 1

development of CDRSF algorithm. He also contributed towards the adaptive grid filtering which is taken as the basis to develop AGRSF algorithm by the present author.

1.8

Organization and Content of the Thesis The thesis consists of ten chapters including this present one. Effort has been made to reduce

the interdependencies between different chapters. Apart from this chapter next two chapters can be considered as introductory chapters. Among them second chapter contains a detailed literature survey on different types of estimators as well as target tracking and navigation problem. In third chapter several test problems have been discussed which were used in successive chapters for comparing the accuracy of different proposed estimators. Different properties of risk sensitive Kalman filter (RSKF) and its alternative formulation have been discussed in chapter 4. In Next three chapters various novel nonlinear filtering techniques, which may be considered as main contribution of the thesis, have been described. In chapter 5 two types of sigma point risk sensitive filter named as central difference risk sensitive filter (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) have been developed. In next chapter another nonlinear risk sensitive filter based on grid adaptation method has been formulated. In chapter 7 one more variety of nonlinear risk sensitive filtering based on particle filtering technique has been proposed. Chapter 8 deals with Cramer Rao Lower Bound for bearing only target tracking problem. It describes how that bound could be used in preliminary design stage of a target tracking problem. In chapter 9 a self alignment navigation problem has been developed and risk sensitive filters have been applied to solve the problem. The last chapter is on Discussions, Conclusions and Scope for further work. The concluding part is followed by a list of literature cited. The Appendices provide flow charts and algorithms, which have been used in this work but not embedded within the earlier chapters.

12

2 Literature Survey 2.1

Introduction to Literature Survey There are two aspects of this thesis. One is to develop new improved nonlinear filtering

techniques for nominal as well as uncertain system dynamics. Another aspect is to apply the nonlinear improved filtering techniques to real life aerospace problem such as guidance, target tracking and navigation. So the surveyed literatures are broadly classified as problem domain and solution domain. Problem domain literature means the literature where applications have been discussed and solution domain literature means where the new improved filtering techniques by previous workers have been developed. Although the distinction between the two classes is not rigid as in many papers the developed methods have been applied to newly formulated real life problems. In this survey an attempt has been made to provide a summary of recent techniques for nonlinear filtering and their application into target tracking, guidance and navigation problems.

2.2

Problem Domain Literature Survey In this section we’ll discuss about the existing literature on aerospace problem with

emphasis on the topics of target tracking and Navigation. In target tracking different methodologies on ballistic target tracking and bearing only target tracking have been discussed. Under the heading of navigation we summarized previous works on alignment problem and GPS INS estimation data fusion. 2.2.1

Tracking of Maneuvering Target Prior to this work Lie and Jilcov [Li 2001a, Li 2001b, Li 2002, Li 2003, Li 2005] have

published comprehensive survey of various models and methods on Manoeuvring Target Tracking in five parts. In the first part [Li 2003] different models used for target modelling have been summarised. In the second part [Li 2001a] survey on ballistic models has been presented. Part III [Li 2001b] of this survey deals with measurement models. Survey on multiple model method has been discussed in part V [Li 2005]. The survey series is helpful for the beginner for a broad over view on the model and methods used for maneuvering target tracking. Bar Shalom also in his book [Bar Shalom 1993] discussed about the state estimation theory with application to target tracking. In that book basic concepts and different aspects of the estimators have been illustrated.

Chapter 2

2.2.1.1 Bearing Only Tracking Target tracking in aerospace is generally carried out with the data obtained from the seeker. Such seekers, depending on the sensors used may provide one or more of sight line angle (loosely speaking bearing), sight line rate (SLR) range and range rate. However a challenging tracking problem is no-named bearing only tracking (BOT). Though not favoured for aerospace tracking, this tracking has become a benchmark problem for testing nonlinear filters (estimators). BOT problem has attracted many researchers. In the brief discussion that follows, a subset of the literature is surveyed. A good starting point for the BOT literature is the monograph [Ristic 2004a] where the BOT problem for the 2D case has been formulated. The problem has been solved in various co ordinate system using different nonlinear filters such as UKF, PF, auxiliary particle filter (APF), IMM EKF. Simulation results obtained from different filters have been presented and compared with Cramer Rao lower bound. In [Song 1996] an observability analysis has been made for proportional navigation guidance using bearing only measurement. Based on this analysis a guidance law was proposed. Simulation results using modified gain extended Kalman filter (MGEKF) have also been provided to show that the proposed method produces small miss distance. Bearing only tracking in Cartesian and modified polar co ordinate system has been studied in [Peach 1995] using extended Kalman filter. A new approach named as Range parameterized tracking (RP) has been proposed. It is claimed that the RP tracker performs better than modified polar (MP) or Cartesian tracker in certain tracking scenario. Passive BOT tracking was also subject of interest in [Jauffret 1996]. Observability in such target-tracking scenario has been analysed. The advantage of such observability analysis is its simplicity over the analysis based on lie algebra. In the paper [Nardone 1997], it has been stated that the available pseudo linear solution suffers with poor observability condition. Here a practical pseudo linear solution, which does not suffer from the observability problem, has been proposed. Algorithm, performance and results are also presented in this paper. BOT problem has been extended in [Cadre 1998] for the source having maneuvering abilities. In this scenario source trajectory has been modeled using Markovian model. Applicability of dynamic programming where no prior information is available has been pointed out. Next the general framework of Markovian decision process has been provided for complete information case.

14

Chapter 2

In [Kronhamn 1998] the multi hypothesis Cartesian Kalman filter (MHCKF) has been applied to the problem of bearing only target tracking. MHCKF has been used adaptively to find an own ship motion trajectory. The simulation results compare the estimated range with the results from a fixed, two-leg own ship trajectory. In [Karlsson 2000] auxiliary particle filter (APF) has been used to track a maneuvering target. Here the truth motion of target is assumed to follow an “S” shaped curve. Simulation results show that APF gives better results compared to IMM filter. It is well known that convergence rate of the extended Kalman filter is poor and batch estimator cannot handle the maneuvering target. To rectify these shortcomings in [Kirubarajan 2001a] batch maximum likelihood probabilistic data association (ML-PDA) estimator has been combined with the recursive interacting multiple model (IMM) estimators. In an earlier publication [Aidala 1983] bearing only target tracking has been considered in modified polar co ordinate system. In [Nardone 1984] Modified gain extended Kalman filter has been developed and its stability has been studied. The filter is applied to solve bearing only measurement problem. The Monte Carlo simulation results for 3D BOT obtained from different filter (pseudo measurement filter, MGEKF, EKF) have been provided. The paper [Ho 2003] proposed an estimator based on the pseudo linear measurement equations, which can produce an unbiased solution for bearing only tracking and Doppler bearing tracking. Simulation results have been provided to illustrate the performance of the proposed estimator with respect to CRLB. In [Clark 2005] the novel Rayleigh shifted filter, has been applied to bearing only problem with clutter. Simulations indicate that it gives good accuracy, even in adverse circumstances. Currently some efficient nonlinear filters are available in the literature, researchers are investigating the applicability of such filters in the BOT problem. Such an attempt has been made in [Sadhu 2004b] where sigma point Kalman filter (SPKF) has been applied to 2D BOT problem. The SPKF is found to more robust against track loss criteria. In a similar paper [Sadhu 2006] the performances of iterated EKF, different version of SPKF, and EKF are studied in detail in terms of computational load, efficiency, track loss etc. It has been reported that the use of SPKF reduces the track loss. Finally it is recommended to use SPKF for BOT problem considering all aspects. BOT problem has also been solved in [Hou 2006] using joint sigma point Kalman filtering technique. It is reported that the algorithm can eliminate the effect of systematic error in state estimation as well as reduce linearization error. The simulation results are also provided.

15

Chapter 2

The problem of maneuvering target racking has been the subject of study in [Ristic 2003b]. The target dynamics has been modeled by using multiple switching models. Three tracking algorithms namely (i) IMM EKF (ii) IMM UKF (iii) IMM PF have been developed for BOT problem. The results obtained from three algorithms have been compared with theoretical lower bound. In [Lin 2002] a simple 2D BOT problem has been solved using EKF, pseudo measurement filter and particle filter. The simulation results show the RMS error obtained from different filter. It is reported that EKF diverges while the other two filters converge nearly the same manner. In this thesis this problem is reformulated and several systematic studies have been done using different types of estimators. In [Farina 1999] BOT tracking problem has also been solved using batch processing and recursive processing filter. Monte Carlo simulation is performed for straight line and accelerating targets. In recent years numerical integration based filter known as particle filter (PF) has been evolved for nonlinear estimation problem. The filter has also been applied for BOT problem by researchers. In [Ristic 2004b] different verities of particle filters such as (i) multiple model PF (MMPF), (ii) auxiliary MMPF (AUX-MMPF), and (iii) jump Markov system PF have been applied for nonlinear target tracking problem. The performance of these filters is compared with IMM-EKF, IMM-UKF and CRLB for three separate cases, namely (i) single-sensor case, (ii) multi sensor case, and (iii) tracking with hard constraints. In [Qu 2006] SMCEKF (sequential Monte Carlo EKF) and SMCUKF (sequential Monte Carlo UKF) have been used for BOT problem. It has been shown that the SMEKF and SMUKF provide better results compare to ordinary UKF and EKF.

2.2.1.2 Ballistic Object Tracking Tracking ballistic object is one of the important topics for application of estimators that are being studied during past three decades by researchers. The goal of the research is to track the ballistic object and destroy it before they hit the ground. The ballistic object may be the enemies’ missile or other terrestrial object like meteor that enter into the earth atmosphere. The interception may be carried out at exo atmospheric or endo atmospheric region. The methods and models for the both type of interception are different. Recently a survey has been made in [Li 2001a] where the authors reviewed more than 50 articles published over nearly three decades. Here the modelling issues in three phases of a ballistic target (namely boost, ballistic, reentry) have been summarized in various co ordinate

16

Chapter 2

systems. The review will be helpful for broad overview on ballistic target tracking for the beginners. The work on ballistic target tracking is mainly based on the modeling of the ballistic target and estimation of the position, velocity, acceleration, ballistic coefficient etc using different nonlinear filtering and compare the performance of the filters. Earlier when EKF was the only choice for nonlinear filtering, several variants of it have been applied to the said problem and compared. In [Mehera 1971] several variants of extended Kalman filter have been implemented in various co ordinate systems and compared based on estimation accuracy and computational time. In the monograph [Ristic 2004a] one chapter has been dedicated for tracking ballistic object on re entry phase. Here a simple vertically falling ballistic object has been considered. Extended Kalman filter (EKF), unscented Kalman filter (UKF) and particle filter (PF) have been used to estimate attitude, velocity and ballistic coefficient of the target. The simulation results obtained from the estimator have been compared with Cramer Rao lower bound (CRLB). Actually the chapter elaborately described the subject matter published in [Ristic 2003a]. The same problem of falling object has been considered in [Srinivasan 2006] where the problem is solved using EKF, unscaled square root UKF, central difference filter (CDF). The estimators are compared in terms of estimation accuracy and computational load. In [Yeddanapudi 1995] the ballistic target tracking problem has been considered using line of site (LOS) observation obtained from satellite. Gauss Newton iterative least square minimization algorithm has been applied for orbit determination. It is reported that the convergence of the algorithm depends on the initial guess. In [Sioris 1997] the tracking of incoming ballistic object has been presented using radar measurement. Theory of Extended Interval Kalman filter (EIKF) has been presented and the filter is applied to track ballistic target in uncertain environment. Simulation results show the filter can successfully track the incoming ballistic object. Paul Zarchan [Zarchan 2000] considers the tracking of ballistic missile with spiraling manoeuvre. It has been seen that the miss distance increases considerably in presence of spiraling manoeuvre. It has been also shown that using compensated wave guidance law with Kalman filter the miss distance can be reduced. In [Cardillo 1999] essential features necessary for a filter to be used for ballistic tracking have been discussed. A Kalman filter has been formulated for estimation of states where air drag is considered as one of the state. The measurements used for filter are azimuth, range and

17

Chapter 2

elevation. Simulation results show the filter can track both endo and exo atmospheric target for a wide verity of trajectory. In [Kirubarajan 2001b] probabilistic data association (PDA) algorithm with maximum likelihood (ML) estimation has been used to track the incoming ballistic missile using bearing only measurement. The Cramer Rao lower bound (CRLB) has also been simulated for that target. It is shown that the proposed estimator meets the CRLB, even for low observable condition. An interactive multiple model algorithm has been used in [Cooperman 2002] for tracking a tactical ballistic missile (TBM) using multiple sensor data. Tracking efficiency has been increased using multiple sensors in all three phases of the ballistic missile. Ballistic target tracking in re entry phase was also subject of study in [Farina 2002a]. The target has been tracked using EKF for both known and unknown ballistic coefficient case. The results are compared with CRLB. A similar problem has also been formulated in [Farina 2002b]. Newly available nonlinear filters such as UKF, PF have been applied to ballistic target tracking case. The filters have been compared with CRLB. In [Bruno 2004] the same problem has been solved using sequential importance resampling (SIR), particle filter (PF) and auxiliary particle filter (APF) along with CRLB. Extensive simulation results show that, the SIRPF performs nearly well with CRLB even in difficult scenario like an unknown, time-variant ballistic coefficient condition. Also in [Bruno 2005] nearly the same problem has been solved using density assisted particle filter along with posterior CRLB. Ballistic missile detection and tracking algorithm along with its kinetic model was also the subject matter of [Farina 2002c]. IMM tracking algorithm is applied to ballistic missile problem and Monte Carlo simulation results have been presented. P. Minvielle, [Minvielle 2005] lucidly described (i) ballistic dynamics during re entry phase (ii) radar scanning process (iii) the development of various nonlinear filtering techniques applied to ballistic target tracking scenario. The paper may be considered as a very good introduction to begin with in the field of ballistic target tracking. A ballistic target tracking problem has been solved in [Zhao 2006] using UKF, PF, and linear minimum mean square filter (LMMSF) and Gaussian PF. The authors concluded that LMMSE is preferred for ballistic missile target tracking scenario considering the tracking accuracy, robustness and computational load. Exo atmospheric ballistic target tracking problem has also been discussed in [Srinivasan 2005] using bearing only measurement.

18

Chapter 2

The tracking of re entry ballistic object has also been studied in [Angelavo 2003]. Sequential Monte Carlo based multiple model filter is used for dealing with high nonlinearity of the object dynamics. The performance of the filter has been evaluated by using RMSE. Predecessor M Srinivasan, from the same laboratory where the present author is working, in his thesis [Srinivasan 2007] also considered both the engagement on endo atmospheric and exo atmospheric for tactical ballistic missile. Advanced nonlinear filtering methods have been applied for both the problems and performances obtained from different methods have been compared. 2.2.2

Navigation

The term navigation had passed through several shades of meaning. Literally navigation means passing through waterways. This meaning has now been overshadowed by the advent of navigation instruments and aids. In the context of this survey, navigation means finding the current position (with velocities and attitudes) of a vehicle, robot or any moving body. A quick overview of terrestrial navigation concepts may be obtained from [Kayton 1988]. This study is confined to a subclass of navigation instrumentation system called strap down inertial navigation system (SDINS). This survey attempts to analyse the state of knowledge in application of filtering and state estimation techniques for SDINS (also called SINS) application. Main focus would be Aerospace navigation, occasionally there are digressions to space and terrestrial navigation. As SDINS is often used in conjunction with GPS, we shall provide some elementary references to the GPS literature as well. A navigation system consists of sensors, which generate signals and the processor, which determine position coordinates, velocity vector and attitude from such signals. For INS, the sensors are three gyros, which measure angular rates in three mutually orthogonal axes and a set of three accelerometers aligned with three gyro axes. These set of instruments along with their electronics are called Inertial Measuring Unit (IMU). Description of various inertial sensors may be obtained from standard textbooks like [Pitman 1962, Merhav 1996]. Conventional gyros are being replaced in some cases by fibre optic gyros and ring laser gyros [Merhav 1996]. Special low-cost, low accuracy MEMS IMU components [Rios2000, Randle1997, Godha 2006] are finding use in land vehicle, UAV and underwater manipulators. The signal obtained from the accelerometers and gyros need to be processed. The signal processing consists of noise filtering and integration. To obtain attitude information, the rate gyro outputs need to be integrated. For velocity information, attitude information and integral of accelerometer signal would be necessary. For position information one needs attitude information

19

Chapter 2

and integral of velocity signals. This double integration means that any bias in the IMU output may manifest as positional error, which increases with the square of elapsed time. Inertial navigation find wide applications in space missions, satellites and spacecrafts [Cheng 2004, Crassidis1997, Crassidis2003], surveying [Hewitson 2003], aircraft [Enge1990, Frykman 2003], weapon systems [Hyslop1990, Groves 2003, Moody 2003], ships [Scheninge1997, Banbrook 1970], UAV [Kim 2006, Kau2006], submarines [Karlsson 2003] land vehicles [Dissanayake 2001, Hong2003] and autonomous underwater vehicles [Huster2002]. The accuracy requirements for different applications may vary widely. For a planetary mission, one needs that the errors of navigational information should remain within acceptable limits for several months. For tactical applications, the duration of flights may be within minutes. Early INS systems were stabilised platform based. Accelerometer and gyro triads were mounted on a 3DOF gimbals. A pure inertially stabilised platform would maintain the attitude of the platform fixed with respect to the remote stars. This was achieved by a feedback system, which maintained the rate gyro outputs to null [Britting 1968]. Advantage about this system had been lower computational requirement which could be tackled by analog computers and simpler stabilisation electronics. The next version of platform based INS keeps the platform locally level and the north gyro pointing to the geographic north. This required moderate computation which was carried out by on board computer. This system was termed as “semi-analytic” [Britting 1968] platform. The truly analytic platform did not have any gimbals but the IMU was rigidly mounted to the body. The term strap-down possibly appears first in the same reference [Britting 1968]. In a follow-up paper [Britting 1969] an early error analysis for SDINS was reported. For constructional details of platform and strapdown INS the references [Pitman 1962, Lawrence 1998, Barbour2003] can be seen. Now a days global positioning system (GPS) signal is available from the dedicated satellites. GPS signal provides very accurate position (in terms of latitude and longitude). The position of a vehicle obtained from the INS may be corrected using the GPS data, which is free from drifts (which occurs in INS due to integration of bias). Earlier GPS was only used by the military aircraft but now a days as GPS receivers are readily available and it is extensively used by civil air craft, and many other application even by the automobiles. While the GPS system is operated and maintained by the US military, the Russians have their own equivalent called GLONASS (Global Navigation Satellite System). The European Community is planning their own version called the Gallelio.

20

Chapter 2

Before GPS was available other ground based systems such as LORAN (Long Range Navigation) which were in use, which have now been nearly discontinued. There were some effort going on to popularise the LORAN. In [Enge 1990] the authors have analysed the performance of services, which could be provided by a combined GPS/Loran receiver. In a similar paper [Nagle 1990] the authors proposed to implement combined service of the GPS and GLONASS. This paper includes a discussion of the integrity, coverage, availability and timing enhancements that are possible from the combined services. The monograph [Farrel 1998] presents detailed background material, case study relating to navigation and global Positioning system design and analysis. The book [Borre 1997 Part II & Part III] describes the science of geodesy and various aspects of GPS. Processing of GPS data has also been discussed in that monograph briefly. T Upadhyay et al in [Upadhyay 1993] described an experiment with an autonomous integrated spacecraft navigation system using onboard GPS and INS measurements. J. Lobo et al in [Lobo 1995] describe a prototype inertial navigation system, which may be used in land navigation. The complete system including sensors (both their characteristics and performance), their mechanical mount, pc card, cabling and micro controller has been described in detail. The paper [Tan 2001] examines whether angular motion of a rigid body can be computed using accelerometer only. If it can be done, the gyroscope free inertial navigation system can be designed. Here it has been shown that output equation relating to the linear and angular motions of a rigid body relative to a fixed inertial frame can also be derived if a condition is satisfied. In [Napolitano 2002] navigational algorithm of fibre optics gyroscope has been presented. It is pointed out that like all other type of gyroscope it can be used either as an autonomous system or it can be integrated with external information based on Kalman filtering to achieve the best performances. The technical report [Meyers 1988a] specifies the functional performance and specification required for INS simulator, which can be interfaced with the external computer system. Similar technical report [Meyers 1988b] provides functional specifications for a shipboard inertial navigation system (INS) simulator, which can be interfaced with the external computer system whose functional specifications are defined in [Meyers 1988c]. 2.2.2.1 Estimation Problems in Inertial Navigation Inertial Navigation had been a fertile application area for estimation techniques (like Kalman filter) as soon as such techniques were formulated. Many of the early application papers for Kalman Filters were from navigation [Barham1970, Banbrook1970, Batin1970, Bellantoni1970,

21

Chapter 2

Crocker1970, Halamandaris1970]. Estimation techniques are primarily employed in the following navigation problems: •

Alignment

•

Calibration

•

Fusion

Alignment problem and fusion problem in the context of navigation will be discussed in detail in subsequent sections. Calibration problem in navigation context means estimation of gyro and accelerometer bias, nonlinearity and scale factor errors. This problem is closely related to the error modelling and parameter estimation. Calibration is often carried out at the factory or base station and may take a long time. Deviations from calibrations may be modelled with small signal approximation and be carried out during alignment. To formulate alignment problem it is necessary to develop error model. So before going to alignment problem literature survey, it is better to review the work done on error model. 2.2.2.2 Literature on Error Models Error models of the SDINS are important for alignment as well as fusion. The degree of details and the choice of coordinate system may be different. Error models for SDINS have been reported since seventies [Krishnan 1970, Britting 1969]. One of the earliest but very readable treatments of gyro and accelerometer error and methods for compensation was given by [Erzberger 1967]. It is interesting that Kalman filter, which was very new at that time, was applied. In [Park 1995], covariance analysis for SINS system has been presented (i) using existing conventional SINS error model (ii) using modified SINS error model. In modified SINS error model attitude errors are transformed into new state variables, so called pseudo states. Here the concept of equivalency and difference between the two models have also been discussed. Conclusion has been drawn that the two models will give the same result. The analysis takes into account gyro compassing characteristics. [Nebot 1999] provides an error model and also analyses the impact of INS errors on trajectory. The paper [Scherzinger 1996] discussed psi angle error model for a strapdown inertial navigator whose platform azimuth is completely unknown. Error modelling for large initial misalignment has been given in [Hong 2003]. [Nassar 2003] provides a good introduction on INS stochastic error models. In this thesis, the stated objective is to improve SINS error modelling such that a major improvement of navigation parameter estimation could be obtained in different SDINS stand alone applications. Different approaches are investigated for improving inertial error modelling in SINS stand alone

22

Chapter 2

applications. The proposed SDINS error model contains deterministic as well as stochastic errors. Position, velocity and attitude bias errors are usually modelled as deterministic errors while the SINS sensor residual biases are often modelled as stochastic errors. The actual behaviour of SINS sensor errors is investigated by computing the autocorrelation sequence using long data records. Autoregressive (AR) process is introduced as an alternative approach in modelling SINS sensor residual biases. Different methods for the optimal determination of the AR model parameters are studied. A recent work on error modelling is [Eduardo 2006], where the effect of errors on trajectories are also analysed, like in [Nebot 1999]. In [Wu 2006] dual quaternion algebra has been adopted for representing the general displacement of a rigid body and to analyse error characteristics of the strap down INS. Two new error models (i) additive dual quaternion error (ADQE) model and (ii) multiplicative dual quaternion error (MDQE) model have been developed using quaternion algebra. The authors expected that future integrated navigation filter would be facilitated using those error models. 2.2.2.3 Alignment Problem Strap down INS needs to be aligned before they may be used for navigation. Alignment means finding the instrument axis system vectors with reference to a known coordinate axis system. Possible standard/reference axis systems are described in [Stimac2004]. It must be remembered that when the gyros in the platform are made inertially stable and properly aligned, all the gyros would sense zero rotational rates. If on the other hand, the platform is to be kept aligned with local North, East and Vertical axis, the platform must have an inertial rate corresponding to the rotation of the earth. At the equator the north axis would always point towards the north and the earth rate need to be applied only about the north axis. However, at any other latitude, components of earth rate must be added to both North and East axes. This process used to be called the gyro compassing. As would be seen that the terms for stabilised platform based INS had been borrowed even for SDINS alignment. The SDINS instruments are rigidly mounted to a frame. For alignment, one needs to know the relative angles between (and corresponding transformation) the IMU and the reference axis. As the INS is not physically rotated, SDINS alignment is sometimes called “alignment of analytical platform” [Britting 1968]. The earliest literature on the alignment of SDINS is possibly the Ph. D thesis of V. Krishnan in the University of Pennsylvania [Krishnan 1970]. Alignment is typically done in two phases, namely, (i) Coarse Alignment, (ii) Fine Alignment. In coarse alignment the INS axis (the analytic axis) is coarsely aligned [Ali 2004] with respect to the reference axis. Typically the misalignment errors are less than 6 degree that is 0.1 radian (100 mRad). The advantage of the coarse alignment is that after this phase, small angle approximations

23

Chapter 2

may be employed and the resultant equations are often linear. This allows application of linearized filters like EKF to be successfully employed for the fine alignment. However there are methods where such distinctions between coarse and fine alignments need not be made [Cheng 2004, Ali 2004]. When the IMU is of precision grade, SDINS may be aligned from the gravity vector and the earth rotation vector. Such procedures are often called self-alignment. Self-alignment is appropriate when the SDINS mounting platform is stationary. However if the mounting platform motion is small and the oscillation is zero mean, self-alignment is still possible [Ali 2004]. The other possibility of alignment is aided-alignment when help is taken from other instruments. Aided-alignment with the help of an already aligned (and possibly of better accuracy) INS (or INS-GPS) combination is often called transfer alignment [Ali 2004]. Transfer alignment is typically carried out for missiles where the reference INS is mounted on the mother vehicle. However, due to elastic motion of the platform and the missile, filtering needs to be employed. Apart from taking aids from another INS, aided-alignment may also be carried out with the help of GPS [Borre 1997]. When the mother aircraft is in flight the alignment is sometimes called “in flight alignment” [Hong 2003]. However there is a scope for misunderstanding. In flight alignment is generally transfer alignment but it could also be self alignment (rare). 2.2.2.3.1

Self Alignment:

Coarse

In earlier section it has been discussed that in self-alignment in SDINS is the process of alignment from the gravity vector and the earth rotation vector. A good starting point on alignment literature is the survey paper [Ali 2004]. It has been discussed earlier that self alignment can be done in two phases namely (i) coarse alignment (ii) fine alignment. In [Schimelevich 1996] two methods have been discussed for coarse alignment. In one method attitude was estimated using accelerometer and gyro measurements. Gyros are required for azimuth estimation and accelerometers are required for solving the levelling problem. In other method, called gyro compassing, the gyros are used for continuous tracking of Instrumental axes attitude and the accelerometer signals used for determining the DCM. Use of DCM linearises the equations but number of state variables increase. The filtering method used was simple averaging. Readers should be careful because this paper contains printing errors; again there are no references cited. Two coarse alignment methods have been provided in [Jiang 1998] for SINS using the measurements of the local gravity vector and earth rate. Drift, skew, and scale alignment errors associated with course alignment method are also evaluated.

24

Chapter 2

Coarse alignment technique has also been described in [Pham 1992] using traditional INS extended Kalman filter applied for helicopter application. Here common limitation of EKF arises from first order linearisation process have been pointed out. In gyro compassing process the inertial platform is aligned to the external reference frame. Gyro compassing error is affected due to gyro non orthogonality. In [Priel 1992] two-step gyrocompassing process is analyzed and the effect of gyro nonorthogonality on gyrocompassing has been discussed. A scheme to eliminate this error is presented here. This method can be used for high quality platform alignment. The paper [Um 2000] proposes an approach to estimate noise covariance using least square algorithm for a linear, time invariant stochastic system with constant but unknown bias. It is claimed that (i) the proposed method requires no a priori estimation of noise covariance, (ii) it provides consistent estimates, and (iii) it can also be applied when the relationship between bias states and other states is unknown. The method has been applied to strapdown inertial navigation system initial alignment scenario. Simulation results have been provided to show satisfactory performance of the proposed method. Chuanbin [Chuanbin 2004] describes an initial alignment scheme using modified Kalman filter, which takes care of the east gyro drift, which tends to affect simple alignment procedures. 2.2.2.3.2

Self Alignment:

Fine

In [Grewal 1988] a smoothing technique has been presented to calculate the relative azimuth of inertial measurement unit (IMU). It is reported that the algorithm has been implemented on an IBM computer and alignment data from an IMU simulator has been post processed. The smoothing results have also been presented in the paper. The paper [Grewal 1990] discusses about the calibration and alignment scheme for an inertial navigation system. Here the error model has been developed and dual extended Kalman filter is applied to estimate the gyro and accelerometer parameters. It may interestingly be noted that the direction cosine (which method is not generally used now a days due to singularity problem) differential equation and their solution have also been presented. Simulation data have been provided to demonstrate the accuracy and convergence of dual extended Kalman filter used for parameter estimation. In the article [Jiang 1992] an estimation algorithm for computing the misalignment angles is provided. Here also a systematic analysis of the observability of an inertial navigation system in ground alignment situation is presented. From simulation results it has been revealed that azimuth error can be entirely estimated from the estimates of levelling error and its error rate, without using gyro signals.

25

Chapter 2

In [Helmick 1993] an algorithm has been presented where two 3-D sensors have been aligned relatively using information of the targets that are being tracked by both sensors. The algorithm estimates sensor biases and sensor frame orientation errors for a tracking system. A Kalman filter has been used [Saab 1994] to investigate the calibration and alignment of a terrain vehicle navigation application problem when the system is at rest. Simulation results illustrate the performance of Kalman filter. SINS error model using tilt angle has been derived in [Chung 1996] and it is shown that this is equivalent to the conventional error model. The inter conversion of the tilt angle and quaternion errors have also been derived using DCM. Based on the error model the quaternions are computed and analysed during multi position alignment problem. In [Fang 1996] SINS error model for the stationary alignment has been introduced and observability of the SINS error model has been analyzed. Then, on the basis of SINS error model estimation of the azimuth error has been done for the initial alignment of SINS on stationary base. Simulation results show that estimation method is efficient and it greatly reduces the initial alignment time. In the article [Eduardo 2006] initial calibration and alignment algorithm for a six-degree of freedom inertial navigation unit have been presented. After the illustration of individual error models for the gyros and accelerometers full error model of SINS has also been presented. Finally, experimental result based on the initial alignment and calibration has been presented which shows the proposed algorithm is able to obtain position and velocity information. The work [Kong 1999, 2000] discusses a general nonlinear psi angle approach that does not require coarse alignment. In this approach three psi-angles are described using three states. Extended Kalman filter is used to integrate the data obtained from a low cost IMU with GPS. In the work [Zhao 2000] multiple sensor alignment problem has been formulated in an ECEF co ordinate system. Sensor biases are estimated using least square technique. Simulated and realtime radar data are used to evaluate the performance of the proposed algorithm. The performance of proposed algorithm has been compared with the standard one. A completely different approach has been used for fine initial alignment problem in [Yong 2002]. Here the dynamic programming theory has been applied to the problem of alignment of SINS. The authors discuss how to apply dynamic programming (optimal control) strategy taking the state as the feedback and how to find the strategy to achieve the alignment on the condition of the quadratic performance index. A patented method for alignment of seekers (mounted on missiles) has been disclosed in [Perkins 2003]

26

Chapter 2

The article [El-Sheimy 2004] describes the method to processes the gyro and accelerometer measurements with multiple levels of wavelet decomposition. The proposed method has been applied to a navigational system. The results show quite accurate alignment, fast convergence and reduced error covariance of three attitude angles. 2.2.2.3.3

Transfer and Aided Alignment

In many applications it is necessary for a vehicle to carry more than one IMU for various reasons. Command module IMU is called master IMU and other IMU is called slave IMU. It is necessary to align the slave IMU to the master IMU assuming master IMU was itself inertially fixed. This process is known as transfer alignment. Here brief review of some publication on transfer alignment has been presented. In [Hagan 1992] a transfer alignment method is presented using only accelerometer measurements. The benefits are claimed in this scheme are (i) freedom to choose IMU gyros solely based on in-flight considerations (ii) the ability to update the transfer alignment by simply making new measurements of gravity and processing the measurements. In this paper the developed algorithm, error analysis and laboratory results are presented. Results of various transfer alignment techniques using Kalman filter have been presented in [Ross 1994, Rogers 1996]. In that publication various transfer alignment issues such as the effect of wing flexure vibration on instrument error estimation and rapid alignment concepts have been addressed. The implementation of Kalman filter in inertial guided weapons is also included. In [Rogers1998] the two models based on the computer frame and perturbation error using position observation are formulated in transfer alignment Kalman filter algorithm. Performances are compared between the two INS error models in terms of (i) filter convergence rates, (ii) IMU navigation solution heading compared to the reference IMU heading, and (iii) post-alignment issues. Between the models the perturbation error model shows the better result. Transfer alignment for a weapon has been discussed in [Groves 2003]. In [Lim 2001] an error compensation method is proposed to reduce the alignment errors in transfer alignment problem of INS subjected to roll and pitch motion of the ship. In this method a linearised error model for the velocity and attitude matching transfer alignment has been derived by linearising the nonlinear measurement equation. The simulation results demonstrate the effectiveness of the present method in azimuth alignment. A recent US patent [Buchler 2006] has been awarded for transfer alignment. Gao [Gao 2007] used velocity matching algorithm for transfer alignment problem. Robust H ∞ filter and Kalman filter have been used for estimation of error angles. It is claimed that H ∞

filter converges faster than Kalman filter. Readers should be careful because there are errors in

27

Chapter 2

navigation equations and some simulation parameters are missing in that paper. Also in simulation result it is not clear what kind of plot has been shown. In [Zaho 2005] transfer alignment for ship has been presented. Transfer alignment has been achieved based on angular velocity mismatch. Kalman filter has been used to estimate misalignment angle. It has been claimed that the method is effective and alignment can be achieved preciously. Hao [Hao 2006] applied unscented Kalman filter in transfer alignment problem. UKF may be advantageous for large initial heading error uncertainty. 2.2.2.3.4

In Flight Alignment

As discussed earlier initial alignment is performed when the vehicle is at rest. But many missile systems require accurate alignment of the on board INS during flight in order to achieve accurate target tracking. So in-flight alignment (IFA) has been developed to estimate the navigational error while inertial sensors are on moving base. In this case the alignment process is performed after launching using accelerometer, gyro data and the data provided from external source during flight. In flight alignment for weapon systems may be transfer alignment with special methods to take care of elastic vibrations. It may also be performed with the help of other aids like Doppler radar or laser based auxiliary equipment. Here brief review of some publication on in flight alignment has been presented. In the case of SDINS attitude errors, the uncertainty caused by linearization of the system degrades the performance of the filter. Robust filters are often applied to overcome this problem. In [Meskin 1992] an error model has been formulated that appropriately describes the INS behaviour during IFA. Observability analysis has been made for vertically damped and undamped INS during IFA. The analysis demonstrates the observability enhancement of IFA. Simulation results have also been presented which confirm the analytical results. Rapid transfer alignment algorithm using Kalman filter for inertially guided air launched munitions has been presented in [Shortelle 1998]. In this alignment procedure it is required to execute only a brief wing rock manoeuvre. The simulation results demonstrate the rapid convergence of the estimator. It has been discussed earlier that psi angle model has been used in initial alignment. Similarly in [Yu 1999] psi angle model and the equivalent tilt (ET) model have been developed and applied to IFA in presence of large attitude errors. To obtain equivalent velocity and attitude error models, the quaternion error is divided into (i) additive quaternion error and (ii) multiplicative quaternion error. The rotation vector error is introduced as a new variable. Exact relationships between these attitude errors are also derived. Proposed error model has been validated for IFA with large

28

Chapter 2

attitude errors. [Brown 2002] describes an in-flight GPS aided alignment using kinematic carrier phase information and a conventional Kalman filter. A robust extended filter for in flight alignment has recently been described in [Yu 2003]. Various velocity aided in-flight alignment methods are proposed by combining the robust filter H 2 / H ∞ with the error models. Simulation results demonstrate that the proposed filter effectively improves the performance. In this paper, a robust filter and various error models for the uncertainty are presented. The analytical characteristics of the proposed filter are also investigated. The results show that the filter does not require the statistical property of the system disturbance and that the region of the estimation error depends on a freedom parameter in the worst case. Then, the uncertainty of the SDINS is derived. Depending on the choice of the reference frame and the attitude error state, several error models are presented. Jaacov [Jaacov 2003] in his patent describes a scheme for in flight alignment when the aircraft measurements get delayed. A complete departure of alignment technique, which involves in-flight and transfer alignment is proposed in a US patent [Kau 2006], where recursive inversion of matrices have been applied. The scenario is a ballistic missile where the INS of the reentry vehicle is aligned with the INS of the launch vehicle before these are separated. 2.2.2.3.5

GPS Based Alignment

In [Park 1998] the SINS error model for stationary and in-flight alignment has been described and it is shown that by employing GPS data the observability and performance of the alignment process can be improved. Covariance analysis result has been presented to analyse the observability and performance of GPS INS system. In [Hide 2003] Adaptive Kalman filtering technique has been used for GPS INS integration for dynamic alignment of the inertial sensor errors in a real life marine application. Results show that the integration significantly improves the dynamic alignment speed and overall navigation accuracy. 2.2.2.4 GPS INS Estimation and Data Fusion The data available from GPS can be integrated with INS system of the vehicle using estimators to obtained higher accuracy in position, attitude and other states of the system. This specific application for estimation technique started nearly three decades ago and attracted many researches. As an estimator mainly Kalman filter and its extended version is being used due to its simplicity and time efficiency. Although this filter has many limitations and now a days other efficient nonlinear filters and high-speed computers are available to designer. These motivate the designers to apply other nonlinear filters such as UKF, PF, to the navigation system. It should be

29

Chapter 2

pointed out here that no robust filter such as risk sensitive filter, H ∞ filter has been applied to navigation system to find out the estimators performance under model mismatch and large initial condition mismatch cases. Here review of the literature related to GPS INS estimation and data fusion is briefly summarized. 2.2.2.4.1

GPS INS Estimation Using Kalman Filter

Here the literature related to GPS INS estimation and data fusion using Kalman filter and its extended version are reviewed. Newton [Newton 1990] compares four filters (least square filter, Kalman filter, two Kalman polar filters) used to smooth the angle only data received by a sensor on a moving platform. In the paper [Poor 1992] the algorithm to estimate the statistical parameters such as system covariance matrix, bias, drift parameters has been presented. Estimation of these parameters are necessary to estimate the position better. Kalman filter’s robustness with respect to adverse environment and non-favorable input condition has been enhanced in [Karatsinides 1994] by successfully identifying that situation. Two robustification techniques such as (i) handling of GPS position bias (ii) proper identification of measurement noise have been discussed. In [Doraiswami 1996] navigational states of a Vehicle are estimated using Kalman filter from the range measurements obtained using beacons (transmitter receivers). The paper [Um 2000] presents least square algorithm based noise covariance estimation technique for a linear, timeinvariant, stochastic system with constant but unknown bias states. It is claimed that the algorithm requires no a priori estimation of noise covariance, and can also be applied when the relationship between bias states and other states is unknown. The proposed method has been successfully applied to strap down inertial navigation system. A modified Kalman filter is used [Song 2000] for filtering of target glint in homing engagement of a ship-to-ship missile. The proposed filter has decoupled range and angle channels so that it enjoys computational efficiency. The proposed algorithm has been tested by a series of simulation runs and its superior performance compared with the other filter structure has been reported. Honghui and Moore [Moore 2002] suggest processing of GPS INS nonlinearties prior to Kalman filtering. The preprocessed INS equations act as process dynamics where as GPS preprocessed data are used as measurement inputs. The advantage of this process is that KF algorithm can be implemented to achieve significant computational saving. Y. H Kim et al [Kim 2003] compare the results obtained from covariance analysis with the results of Monte Carlo simulation for the performance of spacecraft attitude control systems.

30

Chapter 2

In [Portas 2004] four techniques such as estimation with blocks of consecutive, correlated measurement difference, least square and Kalman filter are applied and compared for correcting radar biases for air traffic control (ATC) applications. The comparison is based on accuracy and cost efficiency and implementation issues. An optimal and efficient algorithm has been proposed in [Crassidis 2000] for attitude and position determination from line of sight measurement. It has been claimed that the new algorithm is optimal, non iterative and computationally efficient. The attitude and position error covariance matrix is shown to be equivalent to the Cramer Rao lower bound, which established that the proposed method is optimal. Hanlon et al [Hanlon 2000] propose estimation in the scenario where the true system model is unknown. In this scheme multiple model adaptive estimation (MMAE) which is nothing but a bank of Kalman filter has been used. Each filter assumes a different internal model. Hypothesis testing algorithm analyze the residual from this bank of Kalman filter and determine which Kalman filter model will exactly match with the true model. In [Chatterji 1997] the position of a commercial aircraft with respect to the runway during night landing and takeoff is determined by Kalman filter based fusion of multi sensor data obtained from different source. The data source may be (i) Known structure of the airport lights, (ii) video images acquired by an onboard video camera, (iii) position estimates from an onboard GPS, and (iv) data from an attitude indicator. The paper [Omedo 1998] provides performance of GPS aided inertial navigation system (GAINS) for missile systems made from Raytheon Systems Company. In a case study the paper discusses about the situation where a missile is fired from a ship to intercept an incoming target. The missile and the target are tracked by a ship-based radar system. A Kalman filter is implemented on the interceptors to integrate the data obtained from on-board inertial navigation system, GPS, and ship-uplinked radar. Simulation results show that the accuracy is improved by using both radar and GPS measurements compare to radar only measurement. The paper [Sukkarieh 1999] combined Global Positioning System (GPS) data and an inertial measurement unit (IMU) data for autonomous land vehicle applications. Detection of possible faults both before and during the fusion process has also been discussed. Results of the fusion process are also presented. In [Jang 2000] Kalman filter has been used to estimate the covariance in response to the changing dynamics in GPS navigation. It had been pointed out there that this method can also be applied for fault detection.

31

Chapter 2

Different design and implementation aspect of inertial navigation system, inertial measurement unit (IMU) and GPS have been discussed in [Walcho 2000]. It has been shown that the combination of the sensors data will provide good estimates of position at each instance. The paper [Randle 1997] describes the data fusion of GPS/inertial navigation system (GPSI) with a low cost inertial measurement unit (IMU) using Kalman filter algorithm. Case study shows that the IMU can be successfully integrated with GPSI system using Kalman filtering which improves the system performance. In [Dissanayake 2001], an algorithm and simulation results for IMU GPS inertial navigation system have been provided for land vehicles using information filter. The real life implementation has also been discussed through a multiple observation inertial aiding algorithm. In [Kim 2005], LOS, gyro measurements and dynamic models are coupled using extended Kalman filter to determine relative attitude, position and gyro biases. The quaternion is used to describe the relative kinematics. Some important quaternion identity and quaternion linearisation technique have been derived which may be helpful for beginner. Simulation results show that the estimator satisfactorily determines relative position and attitude. The paper [Roumeliotis 1997] provides interesting discussions on the navigation on the Mars where GPS data is not available. Here the measurement from the Sun sensor has been fused with INS using EKF to obtain the higher accuracy. The estimation accuracy exponentially decreases with decrease in frequency of the sun sensor data. In [Roumeliotis 1999], filter combines the attitude rate information from the gyros and the absolute orientation measurements through error state form of the Kalman filter to obtain attitude estimation. Sukkarieh [Sukkarieh 2000] in his thesis develops a low cost, high integrity inertial navigation system using global navigation satellite system (GNSS) along with Kalman filter for use in autonomous land vehicle. Benlin [Xu 1996] has proposed a new self-learning navigation filter based on probability space and non-Newtonian dynamics. To make the new filter flexible and responsive to a navigation environment, some parameters have been left for filter to choose their values. The proposed filter has been tested with both simulated and real time navigation data and compared with the Kalman filter. The paper [Moon 1999] loosely integrates GPS and INS data and it has been shown that the integrated system provides more accurate navigation solution than the inertial or the GPS-alone. In [Hyslop 1990] the integration of GPS/INS for standoff land attack missile (SLAM) has been discussed in detail.

32

Chapter 2

In [You 2001] vision and gyroscope sensor data are fused with EKF to improve tracking stability and robustness. The paper [Vik 1999] presents an observer based on nonlinear control theoretic approach to estimate gyro biases, scale factor errors and misalignments using GPS measurements. The Kalman filter theory is presented in a very lucid manner in [Levy 1997]. The applicability of Kalman filter in GPS/INS navigation system and GPS only navigation system has also been discussed. 2.2.2.4.2

GPS INS Estimation Using Post Kalman Filter

As discussed earlier availability of efficient nonlinear filtering technique and high speed computers are encouraging the designers to design post Kalman nonlinear filters for GPS INS data fusion. Mainly UKF and PF have been reported in the literature to integrate the GPS INS data. Crassidis [Crassidis 2005] discussed different reference frame and their inter conversion which may be helpful for beginner. UKF and EKF have been used to integrate the INS and GPS to obtain position, velocity, attitude, gyro and accelerometer bias and scale factor estimation. Robustness of UKF with compare to EKF has been reported. In [Vandyke 2005], a similar paper unscented Kalman filter (UKF) has been used to solve the spacecraft dual estimation problem with greater accuracy than that obtained from extended Kalman filter (EKF). In [Crassidis 2005] a spacecraft attitude is estimated using UKF. The filter formulation based on standard attitude measurements using a gyro-based model and attitude dynamics is described using quaternion. Simulation results indicate higher robustness and faster convergence compare to extended Kalman filter under large initial condition. Now a days with the increase of computational power of on board computer particle filter is now being considered for GPS INS data fusion problem. In [Dezert 1999] PDAF (probability data association filter) has been applied for improving the accuracy of autonomous strap down inertial navigation systems (SINS). The proposed method has been applied to terrain aided navigation (TAN) system. Simulation results significantly show improvements can be achieved using PDAF. In [Boberg 2002] PF and EKF have been applied to integrate the data obtained from GPS and INS. The results obtained from two estimators have been compared. In the paper [Iremus 2004], Rao-Blackwellization and particle filtering have been applied for an INS/GPS scenario. Simulation results compare the variance of the estimates with posterior Cramer-Rao bound. Spacecraft attitude has been estimated using particle filter in [Cheng 2004]. The filter formulation is based on the star camera measurements using attitude dynamics-based model for attitude

33

Chapter 2

propagation. It should be pointed out that Rodrigues parameters are used for attitude parameterization. It is reported that PF converges even for large attitude initial error. In [Crassidis 1997], a real-time estimation algorithm called predictive filter algorithm is derived for spacecraft attitude estimation without the utilization of gyro measurements. Predictive filter algorithm is combination of KF and MME algorithm and it can be used for non Gaussian noise sequence. The algorithm indicates that the proposed filter estimates the attitude of a spacecraft quite accurately. A constrained filtering method is developed in [Chiang 2002] to avoid the problem due to singularity of covariance matrix in classical filtering methods. The proposed estimator has been applied for attitude estimation problem.

2.3

Literature Survey Solution Domain

In this section a brief literature survey on the nonlinear filtering techniques has been presented. Here the survey highlights about the previous attempts to improve the efficiency of nonlinear filtering methods both robust and non-robust cases. On the light of this survey it can also be justified the necessity of the novel filtering algorithms which are proposed in the later chapters. For linear Gaussian cases Kalman filter gives the optimal estimation of states. For nonlinear and or non Gaussian cases no general optimal solution exists in the literature till date. The very first attempt for nonlinear filtering has been made using extended Kalman filtering techniques and its different variants. These methods are already subject matter in many books [Brown 1996, Grewal 2001]. So the literature available on Kalman filter and extended Kalman filter and their variants have not been discussed in this chapter. Many techniques for nonlinear filtering other than extended Kalman filtering have been proposed in literature. These techniques as a whole are known as post Kalman filtering techniques. Survey presented in this chapter takes up recent filters such as (i) unscented Kalman filter (UKF), (ii) pseudomeasurement, (iii) particle filter, (iv) mixed Gaussian, (iv) adaptive grid etc. Some of the post Kalman filtering such as particle filter, Gaussian sum filter and UKF have been described briefly in the book by Ristic [Ristic 2004a]. Another aspect of filtering is to estimate the states under uncertain plant dynamics. Those types of filters are known as robust filtering techniques. Literature available on that has also been reviewed and summarize here. Finally the literature on theoretical lower bound (known as CRLB), which can be achieved by any unbiased optimal estimator has been reviewed.

34

Chapter 2

2.3.1 Linear Regression Kalman Filter (LRKF) Techniques For nonlinear Gaussian model it is common practice to apply simple extended Kalman filtering techniques for estimation of states. The EKF has two major disadvantages namely (i) the filter uses 1st order linearisation (ii) the computation of Jacobian. To avoid these disadvantages some methods of filtering such as unscented Kalman filter (UKF), central difference filter (CDF), divided different filter (DDF) etc are proposed. These methods as a whole are known as linear regression Kalman filter. Like EKF, in LRKF the probability density function are also approximated as Gaussian one.

2.3.1.1 Unscented Kalman Filter (UKF) The UKF is first proposed by Julier and Uhalmar [Juiler 1996] and after that many applications of this filter have been made in the field of target tracking, navigation etc. The literatures on application of UKF in the field of target tracking and navigation have been summarized in previous section. Here we review the literature related to theoretical development of unscented Kalman filter, or sigma point filter and their variants. In the paper [Juiler 1996] the authors proposed a new algorithm for nonlinear filtering where a general method for predicting mean and covariance has been proposed by a nonlinear transformation. Using this transformation a new filter has been proposed. The transformation is known as unscented transformation and the filter is known as unscented Kalman filter (UKF). After that a series of paper have been published by the same authors, which established the new filtering theory in a solid foundation. In a similar paper [Juiler 1997] Julier and Uhlmann have described the unscented transformation and algorithm of the newly proposed filter named as UKF. An application of this filter to a real life system has been presented. In [Juiler 2000] the algorithm of UKF developed previously has been presented and discussed. The performance of EKF and UKF has been compared for a test system. In appendix (i) properties of the higher moments of the sigma points, (ii) justification for approximating a pdf by a set of samples, which match the moment, (iii) modified form of the algorithm have been presented. In general for an nth order system the number of sigma points required for computation are 2n+1. In [Juiler 2002a] it has been shown that the number of sigma point can be reduced to (n+1). The major advantage of this reduction is the reduction of computational load of the filter. In [Juiler 2002b] the generalization of the unscented transformation has been made which is known as scaled unscented transformation. It is claimed that the method calculates the mean and covariance with an accuracy of second order with out calculation of Jacobian or Hessian. In

35

Chapter 2

[Juiler 2004] the advantage of UKF over EKF has been discussed. The motivation, development and implementation of unscented transformation have also been discussed. After UKF has been proposed, the filter is being extensively used in various field of application. Most of the applications related to the thesis have been discussed in the previous section. [Wan 2000a] lucidly illustrated the concept of sigma points using attractive figures. The estimator has also been used in dual estimation problem. In [Wan 2000b] the authors advocate the use the UKF as a better substitution of EKF at a comparable level of complexity. In [Merwe2001a, Merwe2001b] the square root unscented Kalman filter (SRUKF) has been proposed and the filter has been applied to a robot arm problem. The simulation results show that the SRUKF show better results than EKF and UKF. In the paper [Briers 2003], Rao Blackwellisation technique has been introduced to calculate the tractable integrals found in the unscented Kalman filtering technique. The newly developed filter has been named as Rao Blackwellised UKF filter. The advantage of this new variant of UKF has been stated as (i) reduction in quasi Monte Carlo variance (ii) decrease in computational load. Wu [Wu 2005] discuss the augmented and non-augmented unscented transformation for nonlinear system with additive noise. Based on the two types of transformation augmented and non-augmented UKF has been derived. It is shown that the basic difference between them is that the augmented UKF draws a sigma set only once while the non-augmented UKF has to redraw a new set of sigma points after the time update to incorporate the effect of additive process noise. An example has been provided and the performances of these two filters have been compared.

2.3.1.2 Central Difference Filter (CDF) and Divided Difference Filter (DDF) In the paper [Lefebvre 2002] linearization process using statistical linear regression has been discussed through selected points as used in CDF/DDF/UKF. It is shown that UKF is a special case of LRKF. Looking the UKF as a special case of LRKF the performance of estimator can be well understood and the adaptation of the estimator can be made where the original form does not give good result. The linearization process based on square root factorisation of the output covariance matrices has been proposed in [Schei 1997]. The advantage for this type of linearization technique has been stated as (i) no Jacobian or Hessian matrix need not to be computed; (ii) the proposed linearization process is more accurate. In [Niels 2000] state estimators for nonlinear systems have been derived based on polynomial approximations obtained rather than using usual Taylor series approximation. Using this

36

Chapter 2

technique two state estimators DD1 and DD2 (DD means Divided difference) have been derived. It is shown that under certain assumptions the estimators perform better than estimators based on Taylor series approximations. A vertical falling body problem has been solved using EKF, DD1, and DD2 filter. It was reported that among them DD2 filter gives the best performance. A conference paper containing similar material has appeared in [Schel 1995]. Gaussian filter and mixed Gaussian Bayesian filter have been formulated in [Ito 2000]. Based on this formulation two filters namely Gauss Hermite filter (GHF) and central difference filter (CDF) have been developed. The GHF, UKF, CDF, EKF have been implemented for highly nonlinear system. The simulation results show that the CDF and GHF perform better than UKF. Interestingly in that publication UKF is called as Julier Uhlmann filter. In the paper [Merwe 2003], limitations of EKF have been highlighted. The different type of LRKF such as UKF, SRUKF, CDKF, SRCDKF, and particle filter with different type of proposal have been presented. The filters are applied to state, parameter and dual estimation problem.

2.3.2 Pseudo Measurement Filter In this type of filter nonlinear measurement equation is manipulated to a linear like structure measurement equation. It is not always possible to transform a nonlinear measurement equation into a linear one. If the linear manipulation is possible the modified measurement is known as pseudo measurement and the estimator known as pseudo measurement estimator. Here literature related to pseudomeasurement filter (PMF) has been summarised. The pseudolinear filter has been developed and applied to bearing only tracking in [Aidala 1982]. In [Song 1985] the pseudo measurement has been derived for the homing missile problem applied to bearing only measurement. Simulation result has been provided to compare the performance of PMF with EKF and Modified gain EKF. In [Song 1988] pseudomeasurement filter has been proposed for target tracking problem. The system dynamics is linear and measurement is nonlinear. The nonlinear measurement is manipulated as a linear one and PMF has been applied. PMF, EKF, MGKF, and conventional sub optimal Kalman filter (CSKF) have been applied. Among them the authors advocate to use the PMF as all the filters performed similarly but computational time is less for PMF. In [Tahk 1988] MGKF and PMF have been applied for filtering problem with kinematics constrain. RMSE for the two filters have been provided for position and velocity estimation. In [Lin 2002] PMF has been applied for 2D BOT problem. The filter performance has been compared with PF and EKF. It has been reported that among the three filters PMF performs as well as PF. The use of PMF has been preferred due to less computational load with respect to PF.

37

Chapter 2

Also in [Srinivasan 2005] PMF has been successfully applied for exo atmospheric IR tracking of ballistic object. UKF and EKF have also been applied to this problem and the performance of PMF is compared in terms of robustness, accuracy and computational load. Considering all three aspects the authors suggest the use of PMF in that particular problem. The algebraic manipulation has been provided to obtain linear measurement equation from nonlinear measurement equation in [Doraiswami 1996].

2.3.3 Approximate Grid Based Filter In grid-based methods, the posterior density function has been approximated in state space using grids and weights associated with each grid point. The grids should be sufficiently dense to get a good approximation of the posterior density function. Approximate grid based filters also called “point mass filters”. Hidden Markov model (HMM) filter is an application of such approximate grid based method. In [Rabiner 1989], the theoretical aspect of HMM in statistical modelling has been reviewed. It has also been shown how the HMM can be applied to practical problems. In a similar paper [Rabiner 1986] the theory HMM has been discussed lucidly. Grid based method has been discussed in [Arulampalam 2002] where emphasis has been given on implementation. The methodology has been used for speech processing [Xianya 1990] and image processing [Veldhuizen 1998]. The point mass filter has been applied to nonlinear navigation problem in [Bergman 1997]. The performance of the filter in terrain aided navigation system has been compared with CRLB. RMSE has been provided to show that the grid based point mass filter’s performance is close to achievable lower bound, which can be obtained by ideal optimal filter.

2.3.3.1

Adaptive Grid Filter

One disadvantage of approximate grid method is its high computational load. As the dimension of the state space increases the computational load increases dramatically. Another disadvantage is that the grids are placed in a predefined point and therefore cannot be partitioned unevenly to give greater resolution in high probability density region. To avoid this technique adaptive grid based filter has been introduced where the grid points can be adapted in each recursion. In [Bergman 1999 b] a truncation operation has been made which is equivalent to the grid adaptation technique. In truncation process the new grid point has been introduced in high probability region removing grid points in low probability areas. A new grid adaptation technique has been proposed in [Simandl 2002] where minimum but sufficient number of grid points necessary for estimation has been found out. It is claimed that the

38

Chapter 2

new algorithm reduces the computational load without loss of accuracy in the estimation process. A comparison has been made between proposed algorithm and the standard point-mass technique using a numerical example. In [Li 1996] the term “adaptive grid” has been used in the context of multiple model (MM) estimation. The main idea of the algorithm is to start with course grid points and the grids are adjusted according to current estimate and measurement residual.

2.3.4 Particle Filter Recently a more general numerical integration based approach has become very popular for state estimation in nonlinear non Gaussian system. The method numerically generates the probability density function of the state using some points called as particle. The method has been proposed by Gordon et al in 1993 [Gordon 1993]. The method is known as particle filter or Sequential Monte Carlo filter or Condensation filter or bootstrap filter. In the monograph [Doucet 2001 a] the theoretical development and application in various fields have been discussed. As soon as the method is proposed a large number of researchers have been attracted. As a result a huge literature exists on its theoretical development, variant and application to different field in science and technology. The paper [Carpenter 1999] may be considered as a good starting point on particle filtering. Here the authors presented the basic sequential importance resampling (SIR) algorithm in the back ground of Bayesian estimation. An example of BOT problem has been presented where the particle filter has been applied successfully. In the book [Ristic 2004a] a tutorial on particle filtering has been presented. Various resampling scheme choice of importance density function has been discussed in detail. Various variant of particle filter available in literature are summarised and discussed briefly. In the tutorial paper [Arulampalam 2002], algorithms of different numerical nonlinear filtering methods have been presented. The main emphasis has been given on the particle filtering methods and its different variants. Detail algorithm of SIR particle filter with different resampling scheme has also been presented. The algorithms of different variants of particle filter such as auxiliary particle filter, likelihood particle filter, and regularised particle filter have been summarised. The filters are applied to a very simple nonlinear Gaussian 1D problem. The performances of different filters have been compared. The paper should be very helpful for beginners and as well as designers. In [Doucet 2000], reason for resampling has been described. The possibility of existing optimal proposal has been addressed. It has been shown that for linear Gaussian process the optimal

39

Chapter 2

proposal density function exists. For nonlinear case similar proposal can also be used after linearising the process dynamics, although the proposal will no longer be optimal one. Rao Blackwellisation process for sequential importance sampling has also been discussed. Similar Discussion with elaborated algorithm has been made in the technical report [Doucet 1998] by the same author. In [Andriseu 2001], the Rao Blackwellised particle filter method has been applied to more complex hybrid models. The model can be augmented with artificial variables that enable to apply the said technique. Also in [Orton 2002, Hue 2002a, Hue 2002b] the developed theory of particle filtering has successfully been implemented for multiple target tracking. In [Doucet 2001 b], particle filtering and smoothing algorithm have been presented for Jump Markov linear system. It is claimed that the method can parallely be computed for real life application problem. In a similar publication by the same group [Andrieu 2003], again the particle filter for Jump Markov system has been discussed. The methodology has been applied to estimate the states from time varying auto regressive process. Applicability of the methodology to complex multi target tracking scenario has been investigated. In [Crisan 2002], some useful convergence results for particle filtering methods have been summarized for practitioners. The efficiency of the particle filter greatly depends upon the clever choice of the proposal. Using appropriate proposal, the higher efficiency can be achieved using small number of particle thus decreasing computational load. As discussed earlier for nonlinear system no optimal proposal exists. So the researchers propose different functions as a proposal density function. The simplest choice of proposal is the prior probability density function. In another approach [Merwe 2000], the proposal is assumed to be Gaussian with the mean and covariance obtained from other recursive nonlinear filter such as EKF and UKF. These types of particle filters with KF and EKF proposals are known as extended Kalman particle filter and unscented particle filter (UPF) respectively. The proposed UPF has successfully been applied to target tracking problem in [Rui 2001]. To decrease computational load of PF posterior pdf can be approximated by a Gaussian pdf and the particle can be drawn from that pdf for the next iteration [Kotecha 2003] thus avoiding the resampling step. This variant of PF is named as Gaussian sum PF. In [Huang 2004] a hybrid importance function has been proposed for particle filtering which encompasses the advantages of both the posterior and prior importance functions. It is shown through simulation that the PF with hybrid proposal density function will provide better results than PF with prior proposal. The paper [Pitt 1999] introduces an auxiliary variable to particle filtering problem to make it robust to an outlier, although the robustness of the proposed filter has not been clearly stated or exemplified in that paper. The proposed method is known as Auxiliary particle filter (APF). R.

40

Chapter 2

Karlsson at al [Karlsson 2000] applied the APF to a nonlinear target tracking problem immediately after it has been proposed by Pitt and Shepard. Very recently new interpretation of APF has been provided in [Johansen 2008]. Some times in the real application case, the marginal probability density of the filter is our only interest to know. This requirement motivated the authors [Klaas 2005] to design an algorithm, which provides marginal filtering distribution. The idea has also been extended to auxiliary particle filter to formulate marginal auxiliary particle filter. The computational cost has been reduced in this new variant of particle filter.

2.3.5 Robust Filtering Kalman filter (KF) gives the optimal estimate for the linear Gaussian cases when system model and noise covariance are perfectly known. The performance of the Kalman filter degrades rapidly for uncertain noisy system dynamics. These drawbacks prompted several researchers to formulate various robust estimators. In robust estimation, two approaches are used. One is to robustify the Kalman filter while the second approach is to derive robust estimation algorithms based on different types of cost functions. The Risk sensitive (RS) filter and H ∞ filter belong to the second category. As the thesis is focused on risk sensitive filter the literature available on it has been surveyed in detail while literature relating to robust Kalman filter and robust H ∞ filter has been surveyed very briefly.

2.3.6 Robust H2 and

H ∞ Filter

Robust Kalman filter or H2 filter has been designed for Gaussian linear system that will guarantee a minimum bound on the estimation error variance in the presence of system uncertainty. The H ∞ filter minimizes the worst case energy gain from the noise input to the estimation error. So H ∞ filter is a worst case design method that assumes no a priori knowledge of the noise statistics. While guarantying the worst case performance of the filter, it sacrifices the average performance. There are two approaches namely (i) Riccati equation based approach (ii) LMI based approach for solving both type of estimation problem.

2.3.6.1

Riccati Equation Based Approach

For a robust H2 filtering design general technique is to minimise the upper bound of the error covariance matrix. The paper [Peterson 1991] investigates the problem of state estimation in presence of plant uncertainties. In a theorem it has been stated that a stable quadratic state

41

Chapter 2

estimator can be obtained by solving a Riccati equation. Although the proof of the theorem has been omitted in that paper. Robust Kalman filter has been derived in [Xie 1994] for a discrete time system where norm bound parameter uncertainty is present. The cost function is derived as the error variance of the filter lies with in a bound for all admissible uncertainties. In [Zhu 2002] a similar cost function has been used for optimisation. The problem of both finite and infinite horizon Kalman filter for time varying norm bound parameter uncertainty has been discussed. The uncertainty may present in system matrix or output matrix or in both. A Riccati equation based robust filter has been derived for that type of uncertainties. Similar type of uncertainty has been considered in [Theodor 1996]. In that paper a priori filter for linear uncertain system that guaranteed a bound on error covariance matrix has been proposed. Here also both finite and infinite horizon cases have been considered. Similar type of filter for the similar type of system has been discussed in [Souza 1999] where the system is subjected to a measurable input. In [Saab 2004] an algorithm has been proposed where the states of a nonlinear system have been estimated to yield bound error covariance. It has also been shown that the upper bound of the error tends to zero when the state noise covariance tends to zero. A numerical example with nonlinearity has been presented in the paper. In a recent publication [Dong 2006] a robust Kalman filter has been derived so that upper bound of the error covariance matrix can be minimized. The system has been considered as a discrete one and the noises are considered as unknown covariance white and additive. Probably in [Nagpal 1991, Grimble 1990] the theory of H ∞ filtering has been proposed. In [Grimble 1990] a polynomial approach to the solution of the L2 linear optimal filtering problem has been discussed. Computational algorithm for the solution of H ∞ estimation has also been described. The difference between H ∞ and L2 filter has been illustrated in frequency domain. In [Nagpal 1991], system has been considered in state space domain. The necessary and sufficient condition for existence of H ∞ estimators for finite and infinite horizon cases has been derived for (i) known initial conditions and unknown plant and measurement noise (ii) both unknown initial condition and noises. H ∞ smoother has also been presented in the paper. Robust H ∞ filter has been presented in [Xie 1991] for a class of linear periodic system subjected to norm bound time varying parameter uncertainty. The filter guaranteed quadratic stability and H ∞ performance index.

In a tutorial [Shaked 1992] the different aspects of linear time varying H ∞ estimator for continuous and discrete time case have been considered. Basic preliminaries have been provided where the meaning of H ∞ cost function has been discussed.

42

Chapter 2

In the paper [Fujitat 1993] a necessary and sufficient condition for a finite horizon H ∞ filtering problem of linear discrete system has been derived. The filter has been applied to 3-D shape recognition. Theory of linear estimation in Krein space has been developed in [Hassibi 1996]. It has been shown that H ∞ filtering, quadratic game theory, risk sensitive control and estimation can be considered as the special case in Krein space estimation theory. H ∞ filter for continuous time process and sampled measurement has been considered in [Mirkin

1995] for known and unknown initial condition. Kalman filter, H2 filter and H ∞ filter have been presented in continuous time case in [Rawicz 2003]. The filters are applied to target tracking and the performances both in transient and steady state have been considered. Different design consideration of robust parameter has also been discussed.

2.3.6.2 LMI Based Approach In another approach H2 and H ∞ filtering problem can be solved using linear matrix inequality (LMI) approach. In the book [Boyd 1994] various common problems arise in the control theory has been reduced such that they can be solved using convex optimization problem. The book may be useful to learn the application of LMI in solution of real life problem. Using robust stability criterion a LMI based robust H2 filter has been proposed in [Boyd 1994] for LTI discrete time system for polytopic uncertainty. Some numerical examples have been solved to show the superiority of the method. In [Minyue 1999], a LMI based new technique has been provided to design a robust Kalman filter for norm bound uncertainty. An optimized scaling parameter has been introduced to improve the design methods. Robust H ∞ filtering problem has been solved in [Minyue 1999] for both continuous and discrete time system using LMI technique. It has been claimed that the method may be useful due to recent advancement in convex optimization. In [Gerome 1999] design problems in discrete-time systems subjected to parameter uncertainty have been solved for H2 and H ∞ filtering using LMI approach. Comparison between H2 and H ∞ filter has been provided there. In the paper [Shaked 2001] two methods for filter designing have been proposed for discrete time system with polytopic uncertainty. The first approach is a stationary robust filter designing method based on the relaxation of quadratic stability requirement. The second approach is solution of a SDP subject to LMI. The results are compared with different available robust

43

Chapter 2

estimator. Similar design methodology has been adopted in [Xie 2003] where the designed filter was claimed to be less conservative due to presence of free parameters for tuning. The new result merges with older one when the value of free parameter is set to zero. The similar concept has been discussed in [Xiea 2004] more elaborately. In [Hu 2003], H2 filter has been designed using two new H2 measures for uncertain linear system. The optimization of the cost function has been made using constrains in LMI form. An example to validate the method has also been presented. Observer based control-using LMI for uncertain linear system has been considered in [Lien 2004]. Few examples have been provided. The verification of results provided in the paper may be good exercise for the beginner. In [Feng 2005], H2 and H ∞ filters have been designed for piecewise discrete time linear system by solving a set of LMI. Two examples have been provided to demonstrate the advantage of the proposed algorithm. Now the researchers are trying to extend the robust H ∞ filter for nonlinear problem. Such an attempt has been made in [Coutinho 2003] where linear H ∞ filter has been discussed for a particular class of nonlinear system. Here LMI condition for H ∞ performance analysis has been derived. Based on this derivation an LMI based method for filter design has been proposed. In another recent publication [Zhang 2005] few methods of robust filtering techniques have been discussed for “affine nonlinear stochastic system” with external disturbance dependent noise. Mixed H2 / H ∞ filtering analysis has also been developed. Finally H ∞ filtering design has been considered using LMI toolbox for that type of nonlinear system.

2.3.7 Risk Sensitive (RS) Filtering The risk sensitive estimation originates from the risk sensitive control and supposed to be more robust compare to non-robust estimators. The risk sensitive cost function includes an accumulated exponential error cost. Probably the first publication on risk sensitive estimator reported in literature is from Speyer et al [Speyer 1992]. In this publication the risk sensitive cost that is considered as the exponential of a quadratic sum of the estimation error has been minimised to obtain a new type robust estimator named as risk sensitive estimator. It has been pointed out that the estimator is linear but it is not a conditional mean estimator such as Kalman filter. In that publication the correlation between the process and measurement noise has also been incorporated in deriving the filter equations.

44

Chapter 2

In another publication [Banavar 1994] the exponential cost criterion problem has been solved through quadratic kernel optimisation method. The scalar term appeared in the cost function has been named as “risk sensitive factor”. The physical meanings of the risk sensitive factor were also discussed there. In [Collings 1994], the risk sensitive filtering using information state approach has been developed. Results are presented to show the relationship between risk sensitive control and risk sensitive filtering. Simulation results have been provided to show the advantages obtained using risk sensitive control. In a similar paper [Collings 1996] the authors discuss the above concepts more elaborately using block diagrams. More examples and more elaborated simulation results have been provided to demonstrate the advantage of the proposed method. In [Dey 1995], the problem of risk sensitive filtering and smoothing has been solved for discrete time Hidden Markov Model (HMM) using reference probability model. The authors mentioned the term “risk sensitive filtering” and “Risk neutral filtering” and discuss the connection between them. The theory of change of measure has been discussed and applied to risk sensitive estimation problem. The same problem has also been addressed in [Dey 1997]. A brief discussion on the connection between the risk sensitive estimation problem and L2, H ∞ estimation problem has been discussed. An attempt has been made for stability and convergence analysis. In [Elliott 1996] the risk sensitive maximum likelihood estimation for HMM has been discussed. An algorithm has been formulated which shows the risk sensitive estimation is more robust compare to risk neutral estimation. It has been said that more researches are necessary to understand the behaviour of the estimator as well as risk sensitive parameter. In a similar publication [Moore 1997] risk sensitive filtering, smoothing and prediction problem have been studied for both continuous and discrete time for general finite dimensional signal models and HMM models. It has been shown that the risk sensitive filtering is generalisation of standard risk neutral filtering. R. N. Banavar in [Banavar 1998] well summarises the risk sensitive Kalman filter (RSKF) algorithm. It also precisely established the relation between the risk sensitive filtering and H ∞ filtering. Various properties of risk sensitive estimator were also summarised and the filter was compared with well known Kalman filter. Bound of the risk sensitive parameter has also been derived. In [Ford 1999] RSKF has been used for state and parameter estimation problem. Simulation example has been provided in the report. No better results than Kalman filter has been reported but it has been argued that in some situation risk sensitive estimation technique provide better result for the system with uncertainties.

45

Chapter 2

In the paper [Boel 2002] a more generalise cost function for risk sensitive filtering (RSF) has been discussed. It has been shown that for linear Gaussian system closed form results can be achieved. H ∞ filtering problem has also been discussed in this paper. A 2D example has been provided where RSF gives better results than Kalman filter. In [Lo 2002] the existence and uniqueness issues of risk sensitive estimation have been investigated. It has been shown that the optimal risk-sensitive estimates will be non unique. A risk sensitive filtering scheme has been proposed in [Ramezani 2002] for the estimation of partially observed Markov chains and structural results and behaviours of the filter bank have been provided. The relationship between risk sensitivity and information has been studied in [Ramezani 2004] via simulation. The authors introduce product estimator as a generalization of maximum a posteriori probability (MAP) estimator for hidden Markov models. Risk sensitive filtering is formulated for indirectly observed Markov state process in [Malcolm 2005]. The Markov process observed through Poisson process and Brownian motion has been considered. Computer simulation results are given which demonstrate the benefits of risk sensitive estimator.

2.3.7.1 Nonlinear Risk sensitive Filter Linear risk sensitive filter has been modified in [Jayakumar 1998] so that it can be used for nonlinear systems. Actually simple EKF like linearisation technique has been used and the filter was called as extended risk sensitive filter (ERSF). The ERSF gives the same result as EKF when the risk sensitive factor equals to zero. The ERSF has been applied to estimate the motion parameters from images. The performance has been compared with EKF. The result shows that ERSF converges in presence of high initial uncertainty whereas EKF diverges. In [Dey 2003] A Risk sensitive filter has been derived for a class of discrete time finite dimensional nonlinear plant. It is obtained by adjusting standard exponential cost index. The smoothing has also been described for that special type of plant. The proposed algorithm has been successfully applied to 1D nonlinear problem. The present author proposed an adaptive grid based [Bhaumik 2005b] and particle filter based nonlinear risk sensitive filtering [Sadhu 2005, Bhaumik 2006a, Bhaumik 2006b, Sadhu 2008], centre difference risk sensitive filter (CDRSF) [Sadhu 2007] and Unscented risk sensitive filter (RSUKF) [Bhaumik 2008]. These methods will be discussed in the appropriate chapter of the thesis.

46

Chapter 2

2.3.8 Cramer Rao Lower Bound (CRLB) CRLB is the least mean square error that can be achieved by any optimal unbiased estimator. As for linear Gaussian system Kalman filter is the optimal estimator and the result from Kalman filter algorithm matches with CRLB. For nonlinear systems as no optimal estimator exists, it can be used to gauge the performance of nonlinear filters. Some literature called it as Cramer Rao Bound (CRB). The concept of theoretical lower bound in the context of state estimation was first introduced by Taylor [Taylor 1979]. In that paper theoretical lower bound expression was derived for continuous time nonlinear deterministic system with discrete nonlinear measurement in additive Gaussian noise case. In [Kerr 1989] the utility, motivation and limitation of CRLB have been reviewed. The literature previous to this publication has been extensively reviewed. The results and discussion presented in this paper may be helpful for practitioner. In [Doreschuk 1995] Cramer Rao Bound for nonlinear auto regressive process with nonlinear observation sequence has been derived. The noises considered here are additive Gaussian sequence. In [Tichavsky 1998] Posterior Cramer Rao bound algorithm has been derived for nonlinear discrete time system and Gaussian measurement noise sequence. The derived algorithm becomes very useful for the practitioner. The papers [Taylor 1979, Kerr 1989, Tichavsky 1998] complete the development of the theory of CRLB for both continuous and discrete time systems, as a consequence it draws the immediate attention of the designer. Lot of researches have been done to apply CRLB methodology on bearing only target tracking [Rao 1998, Kutluyil 2005], ballistic target tracking and navigation [Bergman 1999 a] problems. These have been discussed in previous section in details and are not mentioned here. The papers [Farina 2002a, Farina 2002b] formulate the algorithm of CRLB precisely in a way, which is more convenient to designers. The developed algorithm has been applied to ballistic target tracking case and results obtained from the nonlinear filter have been compared with CRLB. In the paper [Hernandez 2004] two variant of Cramer Rao bounds for nonlinear filtering have been compared. The first bound has been called the information reduction factor, and the second bound is called as enumeration method. It has been reported that the second method is more accurate but computational load is also high compare to first method. It has also been stated that the two methods converge as the time increases. In literature CRLB is known to be a weaker lower bound. Recently a tighter lower bound has been proposed for practical problems, which is known as Bhattacharya lower bound. In [Reece 2005] the Bhattacharya lower bound has been discussed. It has also been reported that the

47

Chapter 2

Bhattacharya lower bound is as conservative as CRLB. Discussion on the new type of lower bound is beyond the scope of this thesis.

2.4

Discussion and Conclusion

A fairly elaborate literature survey on the topics related to the areas of interest of the thesis has been carried out. There are two major aspects of the thesis namely (i) advanced nonlinear filtering (ii) tracking and navigation of aerospace system. This survey covered more recent literature on advanced nonlinear filter like (i)

Linear regression Kalman filter such as UKF, central difference filter divided difference filter.

(ii)

Particle filters

(iii)

Mixed Gaussian filter

(iv)

Robust filter such as risk sensitive filter and robust H2 and H ∞ filter

(v)

Cramer Rao Lower Bound

Literature survey has also been done on target tracking and navigation problem. In target tracking prior works on (i) ballistic target tracking, (ii) Bearing only target tracking have been summarized. Detailed literature survey on navigation has also been done. In navigation prior work on (i) GPS INS data fusion (ii) initial, in-flight and transfer alignment problem have been summarized. The literature survey had been a continuous process which started around 2004 and was frozen a few months before completion of this dissertation. During the tenure of this work, the author contributed to a number of publications. As is customary, such publications are not reviewed (in detail). The literature survey, in its initial phase had helped the present worker to appreciate the concerned areas and to find out the technology gaps for aerospace problem in the area of nonlinear estimation where original contributions can be made. This chapter is expected to be helpful likewise to the newcomers in this field. In the later phase of the survey, it helped to place the work carried out by the worker in the broader perspective of the knowledge corpus contributed by other workers.

48

3 Test Problems 3.1

Introduction

In the later chapters different methods for advance filtering will be proposed. The proposed methods have been applied to simple linear and nonlinear problems chosen carefully for validation, and comparison purpose. In this chapter those simple test problems are summarized. In this thesis the problems described here are considered as benchmark for demonstrating advantages and disadvantages of proposed filtering technique. In this chapter the following test problems are considered.

3.2

•

First order linear system (L1)

•

Second order linear system (L2)

•

First order nonlinear system (N1-1 & N1-2)

•

Second order bearing only tracking (BOT) problem

First Order Linear System (L1)

Truth model of the system is taken as xk +1 = ( F + ∆F ) xk + bias + Bwk

(3.1)

Where the filter model is x k +1 = Fx k + Bwk , where wk ~ N (0,Q)

(3.2)

The measurement model is y k = Hx k + v k , where vk ~ N (0,R)

(3.3.)

To test the performance of robustness of proposed filters against unmodeled bias and uncertainty this test problem is used. Generally the values of H and B are taken as unity. Truth is initialized with the random number generated from the normal distribution of zero mean and given covariance. Filter state is initialized with zero.

3.3

Second Order Linear (L2)

(i) A simple second order linear problem has been chosen as a test problem from earlier publication [Xie 1994]. The system structure is same as described in equations (3.1), (3.2) and 0 − 0.5 . The measurement matrix and 1

(3.3). The state transition matrix has been chosen as F = 1 other parameters are chosen as follows.

− 6 0 0 B = H = [−100 10] wk ~ N (0,1) v k ~ N (0,1) ∆F = , | δ |≤ 0.3 1 0 δ

(ii) A rather contrived two-dimensional model has been chosen from earlier publications [Boel 2002], for testing and validation of different improved filters discussed later. The system had nearly decoupled state variables but with nearly identical eigenvalues, which shows poor

Chapter3

controllability and observability. The system matrix, uncertainty matrix, noises and measurement matrix

have

been

chosen

as

0.99 − 0.01 F = , and 0.99 0

0 δ ∆F = , | δ |≤ 4 . 0 0

H = [1 −1] .

wk ~ N (0,1) v k ~ N (0,1) ; B is chosen as unity matrix.

For both the cases truth is initialised with a random number generated from the zero mean and 1

0

covariance and filter states are initialised with zero. 0 25

3.4

First Order Nonlinear Problem (N1-1)

We consider the following single dimensional process and measurement sequence as a test problem. The problem was appeared in [Ito 2000] then it is revisited with a different measurement equation in [Sadhu 2007]. xk +1 = xk + ∆tf ( xk ) + wk

(3.4)

yk = ∆txk (1 + 0.5 xk ) + vk

(3.5)

Where f ( x) = 5 x(1 − x 2 ) and wk ~ N (0, b 2 ∆t )

v k ~ N (0, d 2 ∆t )

The problem is very interesting in nature. It should be noted that the process equation has three equilibrium points, 0, -1 and +1. Among them equilibrium at 0 is unstable and other two equilibriums are stable. So the truth state will stabilize either around –1 point or on around +1 point. We use the time elapsed ∆t = 0.01 second, the initial condition of the truth x0 = −0.2 , initial condition of the filter xˆ 0|0 = 0.8, b=0.5 and d=0.1. We shall consider the time from 0 to 0.8 second. The value of initial error covariance is taken as 2 ( P0|0 = 2 ). Figure 3.1 and 3.2 show the evolution of states and corresponding measurement values for the case when state variable stabilizes at –1 equilibrium point. Similarly the figure 3.3 and 3.4 show the evolution of state variable, which stabilized near about +1 equilibrium point, as well as measurement. It is observed that state variable shows more tendencies to stabilize around –1 point rather than to stabilize around +1 equilibrium point.

50

Chapter3

0 0

50

100

150

Truth value of state

-0.2

200

Truth value of state

-0.4 -0.6 -0.8 -1 -1.2 -1.4 step

Fig 3.1 Truth value for state stabilizes around the equilibrium point -1 0.02

0.01

Measurement

0

-0.01

-0.02

-0.03

-0.04 -1.4

-1.2

-1

-0.8 -0.6 State variable

-0.4

-0.2

0

Fig 3.2: Measurement for state when stabilizes around –1 equilibrium point

3.5

Another First Order Nonlinear System (N1-2)

Here we consider another first order system with nonlinear process and linear measurement. xk +1 = f ( xk ) + wk where wk ~ N (0, Q k )

(3.6)

y k = Hxk + v k , where vk ~ N (0, Rk ) .

(3.7)

The process and measurement noise covariance are taken as unity, i.e., Qk = 1, Rk = 1 . The process nonlinear equation has been taken as f ( x k ) = 2 − 0.001x k sgn ( x k ) , H=1, x0 = 0

51

Chapter3

1.6 Truth value of state Truth value of state

1.2

0.8

0.4

0 0

50

100

150

200

-0.4 Step

Fig 3.3 Truth values for state stabilizes around the equilibrium point +1 0.05 0.04

0.03

Measurement

0.02

0.01 0

-0.01

-0.02

-0.03 -0.2

0

0.2

0.4 0.6 S tate variable

0.8

1

1.2

Fig 3.4: Measurement for state stabilizes around +1 equilibrium point

3.6

2nd Order Bearing Only Tracking (BOT) Problem (non-linear) Bearing only tracking (BOT) is an important class of target tracking problems, which has

many civil and military applications [Bar Shalom 1993]. The problem has been dealt by a number of workers [Sadhu 2004a, Lin 2002] using filtering techniques ranging from extended Kalman filter (EKF) the unscented Kalman Filter (UKF), to the recently introduced particle filters (PF). Availability of substantial computing power in inexpensive and physically small VLSI components has encouraged exploration of simulation based techniques such as particle filtering. Bearing-only sensors are generally passive, comparatively inexpensive, consume less power, and more robust compared to active sensors. The problem is to estimate the coordinates of

52

Chapter3

the target using the bearing measurement only from a moving platform whose coordinates at each time step are (at least approximately) known. The problem is very important for test purpose due to its poor observability and high nonlinearity and more over less dimensionality. Keeping these in mind this problem was also considered as test problem and it is described in more detail in the next section.

3.6.1

BOT Problem Formulation The earliest version of this particular BOT problem occurs in Bar-Shalom [Bar Shalom

1993] and is later re-formulated by Lin et al [Lin 2002]. A brief overview of the problem formulation is being presented in this section. Bearing only tracking requires either (i) multiple tracking stations with known coordinates (where it is a generalised triangulation problem) or (ii) a moving platform with known velocities, on which the tracker is mounted. The non-linear bearing only target-tracking problem used in [Lin 2002] is of the second type. The problem may be visualised by a situation, where a low flying aircraft is tracking an ammunition train or an enemy ship. Though the numerical data provided in [Lin 2002] is possibly more suitable for an under water scenario, we continue to use the low flying aircraft vocabulary for appreciation of the problem. The tracking is done using angle of depression measurement from the airborne platform moving at a nearly fixed altitude. The target is assumed to move in a straight line (assumed to be the X-axis) in the horizontal plane (fig-3.5), with a near constant velocity (perturbed by the process noises in position and velocity). The platform, in order to track the target, attempts to move in a course parallel to that of the target with a near constant velocity in the same vertical plane. Imperfections of platform motion are modelled by noises in X and Y directions. The average (neglecting the noisy imperfections) motions of the platform are known with negligible errors. The bearing measurement for the target, however, is noisy. The measurement is processed to estimate the velocity and position of the target in an earth fixed coordinate system. The observer model is non-linear and both process and measurement noises are Gaussian. Note that the platform motion noise would appear as a measurement noise. The BOT problem considered has two components, namely, the target kinematics and the tracking platform kinematics as shown in fig 3.5. The target is moving in X direction with the following discrete state space relations X k +1 = Fk X k + G K w

(3.8)

53

Chapter3

Fig 3.5 Tracking platform kinematics.

x1, k T 2 / 2 1 T Fk = where x1,k is the position along X-axis in meters, and G k = 0 1 T x2, k

With X k =

x 2,k is the velocity in m/sec,

wk is independent zero mean Gaussian white acceleration noise

sequence with variance q. The sampling time is denoted as T, which has a nominal value of 1 sec. The (unknown) true initial condition and the known noise variance are

80 X 0 = q = 0.01 m 2 / s 4 1

The target motion noise covariance matrix may be computed from above as T 2 / 2 2 Q k = G k G Tk q = T /2 T q T

[

]

T 4 / 4 T 3 / 2 = 3 q 2 T / 2 T

(3.9)

In figure 3.6 and 3.7 truth position and velocity of the tracking object have been plotted for a representative run.

3.6.2

Measurement Model I

In this measurement model, bearing angle is the only measurement. The tracking platform motion may be described by the following discrete time equations: x p , k = x p , k + ∆x p , k y p , k = y p , k + ∆y p , k

k=0,1,…,nstep

(3.10)

k=0,1,…,nstep

(3.11)

54

Chapter3

where x p ,k and y p ,k are the (known) average platform position co-ordinates and ∆x p ,k and ∆y p ,k are the mutually independent zero mean Gaussian white noise sequences with variances rx =1 m2 and ry =1 m2, respectively. The mean positions of the platform are: x p , k = 4kT and y p ,k =20. The measurement equation (in bearing coordinate) is given as z m,k = z k + v s , k

(3.12)

zk = h[ x p , k , y p, k , x1, k ] = tan −1

y p,k

(3.13)

x1, k − x p , k

z m, k is the bearing angle between the x axis and the line of sight (LOS) from the sensor to the

target and v s , k is the zero mean Gaussian white measurement noise sequence with variance ( rs = (3 0 ) 2 ), assumed to be independent of the sensor platform perturbations and sampling interval. The random component of platform motion is thus seen to induce additional measurement error, which is non-additive and already embedded. The above effect can be approximated as additive noise by expanding the non-linear measurement equations as zm , k = h[ x p , k , y p , k , x1, k ] + vs , k ≅ h[ x p , k , y p , k , x1, k ] + v k

(3.14)

Where v k is the equivalent additive measurement noise (with variance R k ) given approximately by small perturbation theory as vk ≈

[ y p ∆x p + {x1, k − x p , k }∆y p ] [ x1, k − x p , k ]2 + y p , k

2

(3.15)

+ vs , k

R k is calculated considering ∆x p , k , ∆y p , k and v k are mutually independent

[ ]

2 2 R k = E vk 2 = y p , k rx + [ x1, k − x p , k ]2 ry + rs 2 2

{[ x1, k − x p , k ] + y p , k }

In fig 3.8 the measurement has been plotted for each time instance with the true position. It can be seen that as the time precedes the tracking platform approaches the tracking object the bearing angle increases.

55

Chapter3

Truth value of position (m)

96 92 Truth 88 84 80 76 0

5

10

15

20

Time(sec)

Fig 3.6 Truth value of position for a particular run

Truth value of velocity(m/s)

1.2 Truth

0.8

0.4

0 0

5

10

15

20

Time(sec)

Fig 3.7 Truth value of velocity for a particular run

3.6.3

Measurement Model II

In this model, the LOS rate measurement is also available along with the LOS. Though in reality, the LOS and the LOS rate measurement noises may be strongly correlated, we assume a diagonal

R matrix with the element R2,2 being made a variable for a systematic study.

56

Chapter3

Fig 3.8 Measurement versus position versus time

3.6.4

Measurement Model III

In this model, it is assumed that the target is being tracked using a high PRF RF seeker. Typically that type of seeker gives range and range rate signals as the output. Also the effect of eclipsing is present. In this model range and range rate signals are not considered but the effect of eclipsing [Zrnic 2001] has been taken as shown qualitatively in fig 3.9. The continuous line in that figure shows the S/N ratio for eclipsed signal where as the dotted line shows S/N ratio for the signal without eclipsing. The consequent R k is shown in fig 3.10.

50 45 40 35 SNR

30 25 20 15 10 5 0 0

5

10

Time (sec)

15

Fig 3.9 Signal to noise ratio (with and without eclipsing)

57

20

Chapter3

1000 100

log (R)

10 1 0.1

0

5

10

15

20

0.01 0.001 0.0001

Time (sec)

Fig 3.10 Measurement error covariance with eclipsing

3.6.5

Filter Initialisation

Traditionally, tracking filters are initialised from first two measurements. The latest bearing measurement defines the initial position estimate and the difference of two bearing measurements provides the estimation of initial velocity. The initial position estimate thus obtained may be shown to have a covariance of P11, 0 = rx +

ry 2

tan z

+

yp

2

4

sin z

(3.16)

rs

The traditional way of initialising velocity estimation would create unduly large variance due to the large measurement error covariance in the present problem. Lin et al [Lin 2002] attempted to reduce such large errors by using prior knowledge about the target motion. As per [Lin 2002] the initial velocity estimation is selected as xˆ2,0 = 0 and associated variance as terms P12, 0 and P21,0 are taken as zero.

58

P22 , 0 = 1 .

The off diagonal

4 Risk Sensitive Estimator 4.1

Introduction Traditionally Kalman filtering technique relies on the assumption that the system is

accurately known. Performance of Kalman filter degrades if there is uncertainty in system model. To cope up with this disadvantage different robust techniques such as H ∞ filtering, risk sensitive filtering, have been proposed in literature. In this thesis risk sensitive filtering has been chosen as the subject of study and enhancement. Initially risk sensitive estimator originates from risk sensitive control law. The concept of risk sensitive control had been applied to estimation problem to obtain the robust risk sensitive estimator. The risk sensitive estimator (RSE) derived earlier used exponential square error type cost function. Later a more general form of cost function [Boel 2002] has been used compare to the earlier publications [Dey 1997, Collings 1994, Moore 1997, Lo 2002]. The RSE is expected to have enhanced robustness and is closely linked with H ∞ type of robust estimators [Boel 2002]. While the properties and motivations for risk sensitive control law are fairly well known, despite a number of worthy publications, the same cannot be said about risk sensitive estimators. From the dearth of publication (with the notable exception of [Jayakumar 1998]) it may arguably be said that the properties of RSE are not appreciated to the extent of extracting design pragmatics for practical problems. Objective of the present chapter is to attempt a mitigation of the above shortcoming. In this chapter robustness properties of RSE have been studied and some unknown properties have been reported. In this study it has been revealed that the risk sensitive filter performs well compare to its risk neutral counterpart in presence of unmodelled bias and inaccurate knowledge of process noise. The new properties have been illustrated with the help of simple example. More over available literature on RSE tend to use different conventions and symbols, which could be confusing to a beginner. To add to the problem, there are some printing mistakes in some important publications [Boel 2002, Speyer 1992]. In this chapter all possible forms of risk sensitive Kalman filter available in the literature have been categorized. Errors appeared in earlier publication have been corrected. An alternative linear matrix inequality (LMI) based formulation of RSE has been proposed. The concept of risk sensitive estimation (RSE) is applicable to linear as well as nonlinear problems. It is well known that the closed form solution of nonlinear recursive risk sensitive estimation formula can only be obtained for linear Gaussian cases. For nonlinear system an EKF

Chapter 4

like framework (which was called as extended risk sensitive filter) has been given in [Jaykumar 1998]. However all the well known limitations of EKF including smoothness requirement and noise Gaussianity are applicable to extended risk sensitive filter (ERSF). To mitigate those limitations several methods will be proposed in next three upcoming chapters. Toward this goal recursive solution for nonlinear risk sensitive estimation is necessary to know as a starting point. For prior estimation, nonlinear recursive solution provided in [Boel 2002] has been adopted throughout the thesis. For posterior estimation a recursive solution of risk sensitive estimation has been derived in this chapter in the form of a theorem. The proof of the theorem has been done using inductive method. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem [Boel 2002, Dey 1997, 2001].

4.2 Formulation of Risk Sensitive Estimation Problem Consider a general (non-linear) signal model consisting of equations for the state ( x k ∈ R n ) and measurement y k ∈ R p with additive, uncorrelated noise wk ∈ R n , v k ∈ R p of known statistics at the instance k ={0,1,2,3…}. x k +1 = f ( x k ) + w k (4.1) yk = g (xk ) + vk (4.2) The vectors f ( x k ) and g ( x k ) are general (without any assumption of smoothness) nonlinear function of x k and k . The generalized cost function for risk-sensitive estimator at any instance k can be defined as [Boel 2002]. k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp(µ 1 ∑ ρ 1 (φ ( x i ) − Φ 2 2 i k i =0

(4.3)

The constant parameters µ1 and µ 2 are called risk-sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted

ˆ , and ς is a parameter. Functions ρ (.) and ρ (.) are both strictly convex, continuous and by Φ i 1 2 bounded from below, attaining global minima at 0. Here, the variables to be estimated are the state variables themselves ( Φ ( x) = xˆ ), and both the convex functions ρ1(.) and ρ 2(.) are known quadratic functions of vectors i.e., ρ j (α ) = α T α for j=1,2… We denote the optimal estimate as xˆi and accordingly we change the notation for the information state.

60

Chapter 4

k −1

J RS (ς , k ) = E[exp(µ1 ∑ ( x i − xˆ i ) T ( xi − xˆ i ) + µ 2 ( x k − ς ) T ( x k − ς ))] i =0

(4.4)

The initial state x0 is independent of the processes noise mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk-sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by

µ 2 . The current optimum estimate xˆ k is obtained by finding the optimum value of ς , which minimises J RS (ς , k ) , i.e. xˆ k = arg min J RS ( ς , k )

(4.5)

ς

The constant parameters µ1 and µ 2 are called risk sensitive parameters.

4.3 Recursive Solution of Risk Sensitive Estimation 4.3.1

Posterior Solution The recursive solution of RSE may be realized generally in a two-step process. First, a

recursive relation of an information state [Boel 2002, Dey 1997] is formulated, which can be updated in each time step. In each time step, the optimal estimate is then obtained by extremizing the cost of another function involving the information state.

Theorem 4.1 The posterior solution of the RSE problem may be obtained from the following recursive relations: +∞

xˆ k +1|k +1 = arg min ∫ exp[µ 2 ( xk +1 − ς ) T ( xk +1 − ς )]α k +1 dxk +1 ς

(4.6)

−∞

Where +∞

α k +1 = p( yk +1 | xk +1 ) ∫ exp[µ1 (xk − xˆ k )T (xk − xˆk )]α k p(xk +1 | xk )dxk

(4.7)

−∞

Proof of Theorem 4.1 Starting from the chain rule, of conditional density [Ristics 2004 a], it is straightforward to show that p( x0 , x1 , x 2 ,...x n | y 0 , y1 , y 2 ,... y n ) = p ( y n | x n ) p ( x n | x n −1 ) p( x0 , x1 , x 2 ,...x n −1 | y 0 , y1 , y 2 ,... y n −1 ) We derive the relations for the posterior estimation case first. The first posterior estimate xˆ1 is obtained as follows: from (4.6)

Define α 0|0 = p ( x 0|0 ) = p ( x 0 | y 0 ),

xˆ 0 = mean( p ( x 0|0 ))

xˆ1|1 = argminJ RS (ς ,1) = argminE[exp(µ1(x0 − xˆ0 )T (x0 − xˆ0 ) + µ2 (x1 − ς )T (x1 − ς )]p( y1 | x1)

ς

ς

61

Chapter 4

+∞ +∞

= argmin ∫ ∫ exp[µ1(x0 − xˆ0 )T (x0 − xˆ0 )]exp[µ2 (x1 − ς )T (x1 − ς )]p( y1 | x1) p(x1 | x0 ) p(x0 | y0 )dx0dx1

ς

−∞ −∞

by chain rule. Now let +∞

α 1 = p ( y 1 | x 1 ) ∫ exp[ µ 1 ( x 0 − xˆ 0 ) T ( x 0 − xˆ 0 )]α 0 p ( x 1 | x 0 )dx 0 −∞

+∞ then, xˆ1|1 = arg min ∫ exp[ µ 2 ( x1 − ς ) T ( x1 − ς )]α 1 dx 1 ς −∞

similarly xˆ2 can be obtained as +∞

xˆ 2|2 = arg min ∫ exp[ µ 2 ( x 2 − ς ) T ( x 2 − ς )]α 2 dx 2 ς

−∞

where +∞

α 2 = p( y 2 | x 2 ) ∫ exp[ µ 1 ( x1 − xˆ1 ) T ( x1 − xˆ1 )]α 1 p( x 2 | x1 )dx1 −∞

Proceeding in this manner, by the method of induction one can conclude that +∞

xˆ k +1|k +1 = arg ςmin ∫ exp[ µ 2 ( x k +1 − ς ) T ( x k +1 − ς )]α k +1 dx k +1 −∞

Notes: 1) Let the probability distributions of wk , v k be given respectively by p w (.), p v (.) . The two probability density terms in equation (4.7) can then be expressed as p ( y k +1 | x k +1 ) = p v ( y k +1 − g ( x k +1 )) p ( x k +1 | x k ) = p w ( x k +1 − f ( x k ))

2) Here we provide an inductive method for deriving the expression for the information state. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem [Boel 2002, Dey 1997, 2001].

3) The estimation equation is specifically applicable for posterior estimation where the estimation is performed after receiving the measurement. Similar relations can be obtained for prior estimates, also known as delayed measurement [Banavar 1998] as is used in [Boel 2002].

4.3.2

Prior Solution

The solution to the risk-sensitive estimator (RSE) problem for a priori estimation may be obtained from the following recursive relation [Boel 2002] using an information state α k (x k ) and probability density functions p( xk +1 | xk ) and p( y k | xk ) . α k (xk ) = ∫ p( xk | xk −1 ) p ( y k −1 | xk −1 ) exp( µ1 ( xk −1 − xˆ k −1|k −2 )T ( xk −1 − xˆ k −1|k − 2 ))α k −1 (xk −1 )dxk −1

Optimal estimate can be obtained from

62

(4.8)

Chapter 4

+∞

xˆ k|k −1 = arg min ∫ exp( µ 2 ( xk − ς )T ( xk − ς ))α k ( xk )dxk ς ∈R

−∞

(4.9)

Both the posterior and prior recursive solution of risk sensitive estimation has been used in the later chapters to develop risk sensitive particle filter and adaptive grid risk sensitive filter.

4.4

Risk Sensitive Estimation for Linear Gaussian Problem

The linear Gaussian system model can be described as: x k +1 = Fx k + Bwk ,

wk ~ N (0,Q)

y k = Hx k + v k ,

vk ~ N (0,R)

For the linear Gaussian risk sensitive estimation closed recursive Kalman filter like form is available. It can easily be derived. For example substituting the Gaussian probability density function in equation (4.6) and integrating, the risk sensitive Kalman filter formula for posterior estimation can readily be achieved. We call such filter as risk sensitive Kalman filter (RSKF). The multiplicity of forms in earlier literature [Boel 2002, Dey 1997, Spyere 1992, and Collings 1994] arises because one may choose (a) the posterior or prior estimation expressions and derive them using (b) posterior or prior covariance to obtain the estimates. Thus there are, at least, four possible forms. Other subtle differences like using information filter (inverse of covariance) [Collings 1994] are also possible. It is to be noted that as the exact solution is available for linear gaussian case the term µ 2 will not appear in the solution.

4.4.1

Posterior Risk Sensitive Kalman Filter

The prior error covariance Pk +1 k is given as Pk +1 k = Fk [ Pk−k1 −1 + H kT Rk−1H k − µ1I ]−1 FkT + BQk BT

(4.10)

The posterior state estimate at ( k + 1 )th step is given as xˆk +1 k +1 = Fk xˆ k k + ( Pk−+11 k + H kT+1 Rk−+11 H k +1 ) −1 H kT+1 Rk−+11 ( y k +1 − H k +1 Fk xˆk k ) = Fk xˆ k k + K k +1 ( yk +1 − H k +1 Fk xˆ k k )

(4.11)

Where the filter gain is defined as K k +1 = [ Pk−+11 k + H kT+1 Rk−+11 H k +1 ]−1 H kT+1 Rk−+11 The filter exists if and only if [ Pk−k1−1 + H kT Rk−1 H k − µ1 I ] > 0 The above expressions are equivalent (except the choice of symbols) to those used by earlier workers [Banavar 1998]. It should be noted that here posterior estimation formula uses prior covariance. Also formula for posterior risk sensitive state estimation form is available in [Dey 2001] where posterior error covariance has been used.

63

Chapter 4

xˆk +1 k +1 = Fk xˆk k + Pk +1 k +1 H k +1 Rk−+11 ( yk +1 − H k +1 Fk xˆk k )

(4.12)

where Pk−+11 k +1 = [ Fk ( Pk−k1 − µ1 I ) −1 FkT + BQk BT ]−1 + H kT Rk−1 H k

(4.13)

The state estimate equation (4.12) can be re-written in the following form xˆk +1 k +1 = Fk xˆ k k + K k +1 ( yk +1 − H k Fk xˆ k k )

(4.14)

where K k +1 = Pk +1 k +1 H k +1Rk−+11

(4.15)

is the filter gain. Note that again the relations are valid only for sufficiently small risk sensitivity factor satisfying Pk−k1 − µ1 I > 0 . In the case where µ1 = 0 , it can be seen that the equations (4.12) and (4.13) are

exactly same with the more popular form of linear Kalman filter. Matrix inversion lemma may be used to obtain the expression in desired form.

4.4.2

Prior Risk Sensitive Kalman Filter

The prior estimate is given as xˆk +1 k = Fk xˆk k −1 + Fk K k ( yk − Hxˆk k −1 )

(4.16)

With filter gain defined as K k = [ Pk−k1−1 − µ1 I ]−1 H kT ( Rk + H k [ Pk−k1−1 − µ1 I ]−1 ) −1 Prior error covariance is given by Pk +1 k = Fk [ Pk−k1−1 + H kT Rk−1 H k − µ1 I ]−1 FkT + BQk BT

(4.17)

The above relations exist provided Pk−k1−1 − µ1I > 0 ⇒ µ1 I < Pk k −1 The above expressions are simplified forms and equivalent to those used by earlier workers [Norgaard 2000]. Matrix inversion lemmas can be applied to get the desire form. It should also be noted that for risk neutral case the risk parameter becomes zero and the RSKF reduces to well known Kalman filter algorithm. The choice of risk parameter is bounded so that it satisfies the above inequality. In [Boel 2002] another form of prior to prior state update formula for RSKF has been provided in page 454. The formula given there is for unity variance process and measurement noise sequence. The prior state estimation formula according to Boel is (using Boel’s notation) Σ k +1 = A(C T C + Σ −k 1 − 2µ1 I ) −1 AT + I

(4.18)

xˆk +1 = A(C T C + Σ k−1 − 2µ1 I ) −1 (C T yk +1 + Σ k−1 xˆ k )

(4.19)

64

Chapter 4

The A matrix is state transition matrix which is equivalent to our F matrix. C is the observation matrix which is equivalent to our H matrix. The term Σ is error covariance that is equivalent to P matrix in our notation. The recursive equation for error covariance is correct. If unity covariance process noise is not considered the unity matrix will be replaced by corresponding covariance matrix. But the prior state estimation formula stated in Boel (4.19) is not correct. The correct form in Boel’s notation will be xˆk +1|k = A(C T C + Σ k−1+1|k − 2 µ1I ) −1 (C T yk +1 + Σ k−1+1|k xˆk |k −1 − 2 µ1 xˆk |k −1 )

(4.20)

The above formula is for unity measurement error covariance. For any measurement noise covariance ( Rk ) the formula will be (in conventional notation) xˆ k +1|k = F ( H T R −1 H + Pk−+11|k − µ 1 I ) −1 ( H T y k +1 + Pk−+11|k xˆ k |k −1 − µ 1 xˆ k |k −1 )

(4.21)

4.5 LMI Based Formulation of RSKF A wide variety of problems arising in system theory can be reduced to a few convex optimization problems involving linear matrix inequality (LMI). These problems can be numerically solved very efficiently using interior point method. Also softwares are available which can readily solve the LMI problem. So there is increase tendency to express a problem in LMI form and solve it. The monograph [Boyd 1994] may be referred as a good starting point to learn LMI for beginner. In the field of robust estimation there are two approaches to solve the problem. The first approach is Riccati equation based approach [Xie 1994, Nagpal 1991] and second is LMI based approach [Li 1997, Geromel 2000, Shaked 2001, Xie 2003]. The later approach has gained popularity since the development of interior point algorithm by Karmarkar in 1984. The LMI based approach has been applied to solve the robust H2 [Geromel 2000, Shaked 1997] and H ∞ [Geromel 2000] problem. But the method is not available in literature where risk sensitive filter has been solved using LMI. Shaked [Shaked 2001] formulated LMI based Kalman filtering for unity variance Gaussian process and measurement noise sequence. In this section LMI based Kalman filter will be reformulated for arbitrary process and measurement noise covariance. An alternative Linear Matrix Inequality (LMI) based risk sensitive Kalman filter will also be formulated for both posterior and prior type of estimation. The alternative formulation will provide a platform for testing the validity of available filtering algorithm.

4.5.1

LMI Based Kalman Filter Formulation

LMI based Kalman filter is first formulated by Shaked et al [Shaked 2001]. That formulation [Shaked 2001] was for unity process and measurement noise covariance and the formula contains obvious errors. In that work [Shaked 2001] Fk K k has been called as the Kalman gain, which is

65

Chapter 4

not conventional. Here LMI based Kalman filter has been reformulated with arbitrary process and measurement noise covariance. In two step Kalman filter algorithm the recursive equation of the error covariance is given by Pk +1|k = ( Fk − Fk K k H k ) Pk|k −1 ( Fk − Fk K k H k )T + Fk K k Rk ( Fk K k )T + BQk BT

(4.22)

and the posteriori estimate is given by xˆk|k = Fk xˆ k −1|k −1 + K k ( yk − H k Fk xˆk −1|k −1 )

(4.23)

The Kalman filter minimizes error covariance. That is equivalent to minimization of trace of the error covariance matrix at each instance. So this can alternatively be stated using semi definite programming (SDP) as follows. Pk +1|k and K k are the solution of the following SDP

min

trace( Pk +1|k )

Pk +1|k , K k

Subjected to − Pk +1|k ( F − F K H ) T k k k k BT ( Fk K k )T

Fk − Fk K k H k −1 k |k −1

B

−P

0

0

− Qk

0

0

−1

Fk K k 0 ≤0 0 −1 − Rk

(4.24)

Pk +1|k > 0

(4.25)

Pk +1|k − PkT+1|k = 0

(4.26)

Solving the above three inequalities with the cost criteria, which minimizes the trace of error covariance matrix, Kalman gain (Kk) and error covariance matrix can be obtained. The inequality describe in (4.24) and can easily be reduced in the form of (4.22) by using Schur complements. Softwares are readily available to solve the inequality. In Matlab LMI toolbox is available. In Scilab also similar toolbox is available. Using Kk obtained from the solutions of above LMI, posterior estimation can be done using equation (4.23). Prior estimate can also easily be obtained from the relation xˆk +1|k = Fk xˆ k |k . 4.5.2

LMI Based Risk Sensitive Kalman Filter Formulation

In the previous section it has been discussed that risk sensitive estimator minimizes exponential squared error type cost function. So risk sensitive cost function mentioned in the equation (4.4) can be stated as min exp[ µ1trace( Pk +1|k )] . For any other convex form of Kernel similar cost function can also be realized. Using Schur’s inequality it can readily be shown that the solution of equation (4.10) is equivalent to the solution of the following SDP for Pk +1|k .

66

Chapter 4

exp[ µ1trace( Pk +1|k )]

min Pk +1|k

Subjected to − Pk +1|k T Fk BT

B + H k Rk H k − µ1I ] 0 ≤0 0 − Qk−1 Fk

−1 k |k −1

− [P

T

−1

(4.27)

Pk +1|k > 0

(4.28)

Pk +1|k − PkT+1|k = 0

(4.29)

From the above LMI formulation, error covariance Pk +1|k can be obtained for each step. From Pk +1|k both posterior and prior risk sensitive Kalman filter can be realized using equation (4.11)

and (4.16). It should be noted that for RSKF the condition for filter to exist should be checked at each step as discussed in previous section. The SDP problem stated above is solved using both Scilab and Matlab software and it is verified that the obtained solution i.e. Pk +1|k is numerically same as obtained from the solution risk sensitive Riccati equation. In non-linear cases the method can be extended to obtain LMI based solution of extended risk sensitive filter (ERSF). For that measurement matrix (Hk) and state transition matrix (Fk) can be obtained after linearising the state and measurement equation. The values can be used in equation (4.24) and (4.27) to obtain error covariance. Also in that case equation (4.11) and (4.16) need to be changed accordingly to obtain the estimated value of state variable.

4.6 Robustness Properties of RSKF Essential general properties of RSE have been well compiled in [Banavar 1998]. We shall concentrate only on two distinct but interconnected aspects of the properties, namely, robustness and the pragmatics for choosing risk sensitive parameters. We draw upon the already known properties, examine and reinterpret them as also attempt to extend them. At this moment it is near impossible to comment on properties for non-linear and non Gaussian signal models. However, in the tradition of the predecessors, we use the properties of Linear Gaussian problems to gain insight and conjecture that some of the properties would also be applicable to ‘mildly’ non-linear non-Gaussian cases. The extended KF like linearisation, used in [Jaykumar 1998] also indicate that some properties in linear cases may be extended to non-linear cases.

67

Chapter 4

4.6.1

Robustness Properties

The connection between H ∞ filter and the RSE is well known [Boel 2002]. In H ∞ filter the objective is to minimize the cost ( J H ∞ ) such that J H ∞ < γ 2 . In case of Kalman filter γ → ∞ . Whereas in case of RSE the risk sensitive parameter functions as µ1 is proportional to 1 / γ 2 as stated in [Boel 2002], with say, µ 2 = 1 . This is the primary reason why the RSE is considered to be robust. However, the different aspects of the robustness, namely, with respect to the presence of un-modeled bias, error/inaccuracy in state matrix and the same for process would be carefully examined. Also of importance is to find the temporal aspects that is how long does it take for the RSE to recognize the modeling error and to compensate for them. The properties are enumerated as given below: (i) As the risk sensitive cost function memorizes the past errors incurred by the estimator, intuitively it is expected that the RSE would learn from past mistakes and perform better. At the same time, learning requires time and the effect of learning can only be seen over time and not instantaneously. One may therefore expect that the performance of RSE to improve over time as compared to risk neutral filter. (ii) The above also means that RSE would be more effective against sustained perturbations rather than quickly varying disturbances in the model. Thus, in the presence of an un-modelled bias, the RSE is expected to track with lesser errors than a risk-neutral filter. That it is indeed so, is exemplified in the next section. As a corollary, one would not expect the RSE to effectively reject un-modelled, quickly varying, disturbances. (iii) It should be pointed out that the RS cost function assigns cost for each point in state space and therefore penalizes the regions in state space, which, in the past, more often resulted in large error covariance. (iv) The RS cost function computes the cost associated with points in state space based on its knowledge of the nominal model and also on the particular realization of the measurement sequence. As such, RS cost function cannot directly infer perturbations and changes in the signal model; it has to depend on measurement. For a signal model with lower measurement noise (covariance), the RSE is expected to perform better. (v) Inspection of equation (4.8) reveals that the effect of increasing µ1 tantamount to increasing the process noise covariance as well as the estimation error covariance. This enhances confidence in measurement and reduces the same in the assumed dynamics of the process leading to higher feed back gain of the estimator- effectively increasing the filtering bandwidth.

68

Chapter 4

(vi) In practical application of KF, it is customary to use a larger value of process noise covariance (Q) than the nominal to avoid the performance penalty if the true Q is larger than the nominal or to take care of modelling errors in plant dynamics. The above property shows why such artificial measures are unnecessary for RSKF as the risk sensitive parameter automatically does that. Further, increase in Q, generally makes the estimators faster, however, RS estimators, in general, takes a longer time to settle. The effect of increasing Q and increasing the risk factor therefore does not have one to one correspondence. 4.6.2

Pragmatics Related to RS Parameter

From the discussion of previous paragraphs it may be easily inferred that enhancing the value of µ1 increases robustness and bandwidth of the filter and therefore is desirable. However, there are

limits beyond which one cannot increase the risk sensitive parameter. The estimation in risk sensitive cases is possible over a horizon of length N where 0 ≤ k ≤ N if

and only if the following conditions are satisfied: ( Pk−|k1 − 2 µ1 I ) > 0 for 0 ≤ k ≤ N . This is

discussed in [Boel 2002, Banavar 1998] and is also evident from inspecting equation (4.7) and equation (4.8). In general there is no guarantee beforehand that these conditions will be satisfied for each step. Apart from the above, larger values of risk sensitive parameters often gives rise to overflow or reduced accuracy. The present worker found that the following simple heuristic is useful to assign the first guess value for the risk sensitive parameter. This can be intuitively justified from the convergence criteria as above.

µ 1 ≤ 1 /( 2 || P ||) where ||P|| is the largest eigenvalue of P, taken over all the values of k. The later can be estimated from knowledge of the physics of the problem or from a run of KF or EKF.

4.7 Numerical Experiment and Illustration for RSKF 4.7.1

Sensitivity to State Model Error The figure 4.1 and 4.2 are the root mean square error (RMSE) of the state variables

obtained from ordinary KF and risk sensitive KF (RSKF) for 100 Monte Carlo run for the problem ‘2nd order linear (L2)’ discussed in section 3.3, case (i). The values of state transition matrix, measurement matrix, noise characteristics and input are mentioned in section 3.3, case (i). The perturbation term is taken as δ = 0.07 . Risk sensitive parameter has been chosen as

µ 1 =0.004. It should be noted from the figure that accuracy in estimation obtained from RSKF is better compared to KF in presence of perturbation. It should also be noted that RSKF estimation is much better than KF in the later time steps compared to the first few time steps. This is as

69

Chapter 4

expected according to our previous discussion. It is seen from the figures that even for such a small δ the RMSE calculated for the state variable from KF algorithm diverges but the RSKF does not. The figure 4.3 illustrates the variation of the RMSE of first state variable at 100th step for both KF and RSKF as the uncertainty parameter δ ranges in value from –2 to 2 for a fixed value of

µ 1 =0.004. The graph shows that for very low value of δ , the Kalman filter achieves a lower terminal RMSE value than RSKF. For the larger value of δ the RSKF has the lower terminal RMSE value than KF. It is also seen from the graph that in this particular case the variation with

δ is not symmetric in the positive and negative regions. The nature of variation of terminal RMSE with δ for second state variable shows similar trend and not shown here. 4.7.2

Sensitivity to Unmodelled Bias If any unmodelled bias is present in the system, the integrator like effect is observed on the

truth value of the state variable. It is illustrated in figure 4.4 using simple first order linear system (L1) discussed in section 3.2. For that problem the state transition matrix is taken as F=0.99, ∆F is zero and bias is taken as 0.2. The standard deviation of the process and the measurement noise is 0.1 and 2.5 respectively. µ1 has been taken as 0.085. From the figure 4.4 it can be seen that ordinary Kalman filter fails to track the true value of the states whereas RS filter acts comparatively better.

RMSE of 1st state variable

1.2 KF RSKF

1

RMSE

0.8 0.6 0.4 0.2 0

0

20

40

60

80

Step

Fig 4.1

RMSE of first state variable for KF and RSKF

70

100

Chapter 4

RMSE of 2nd state variable

6 KF RSKF

RMSE

5.5 5

4.5 4 3.5 0

20

40

step

60

80

100

Fig 4.2 RMSE of 2nd state variable for KF and RSKF Terminal value of RMSE

2.5 RSKF KF RMSE of 1st state

2 1.5 1 0.5 0 -0.2

-0.1

0

0.1

delta

0.2

Fig 4.3 Terminal RMSE of 1st state variable for KF and RSKF for different δ

This property seems to be very important for risk sensitive filtering application point of view. To examine whether this property exists also for multidimensional case an experiment has been done with a two dimensional linear model in the form of x k +1 = Fx k + wk + bias y k = Hx k + v k 0 − 0.5 , H = [1 − 0.5] , wk ~ N (0,1) vk ~ N (0,1). The bias vector is taken as 1 1

Where F =

bias = [0.2 0.2]T . Truth states are initialised with a random number generated from the zero mean

and unity covariance and filter is initialised with zero values for the both states. In figure 4.5 truth

71

Chapter 4

value and estimated value of first state variable obtained from RSKF and KF have been plotted. The figure shows that the estimated values obtained from the RSKF are comparatively close to truth than the estimated values obtained from Kalman filter. The same nature of the estimators is reflected in RMSE plot shown in figure 4.6 for 100 MC run. From that figure it can be seen that the RMSE plot for RSKF is lower than KF. It has been discussed in section 4.6.1 that the RSKF learns from past history and the effect of learning is evident after some time has elapsed. Figure 4.6 supports the above argument as RMSE of RSKF becomes significantly lower than ordinary KF after some times not from the beginning of estimation. Estimated and truth value of state

Truth and estimated value

8

TRUE KF RSKF

6

4

2

0 0

10

20

30

40

50

-2 step

Fig 4.4: The truth and estimated value of the state with unknown bias Truth and es tim ated value of 1s t s tate 30 Truth KF RS K F

25

20

state

15

10

5

0

-5

0

10

20

30

40

50 s tep

60

70

80

90

100

Fig 4.5 Truth and estimated value of 1st state variable with unknown bias for 2D case

72

Chapter 4

RM S E for K F and RS K F 9 RM S E of RS K F RM S E of K F

8 7

RMSE

6 5 4 3 2 1

0

10

20

30

40

50 s tep

60

70

80

90

100

Fig 4.6 RMSE for 1st state with unknown bias 2D case

4.7.3

Sensitivity to Noise Modeling Error

From the closed form equation of the risk sensitive filter for linear case it is clear that incorporating the risk sensitive parameter increases the effective prior covariance of the filter. So if the system is modelled wrongly by using a smaller process noise covariance, the risk sensitive filter estimates better than the risk neutral filter. For this experiment we have taken a first order linear problem (L1) discussed in section 3.2 with F=0.99. Filter’s process noise covariance and measurement noise covariance are taken as 0.01 and unity respectively whereas the true process noise covariance is taken as 4 times that assumed for filter. The RMSE obtained from the RSKF with µ1 =0.3, and KF are shown in the figure 4.7. The simulated result supports the concept discussed above. RMSE for Q mismatch 1.2 KF

1

RSKF RMSE

0.8 0.6 0.4 0.2 0 0

20

40

step 60

80

Fig 4.7:RMSE for RSKF and KF for Q mismatch

73

100

Chapter 4

4.8

Discussion and Conclusion In this chapter robustness properties and model sensitivity of RSE have been studied.

Some unknown properties of the risk sensitive estimator such as sensitivity to unknown bias, sensitivity to noise modeling error have been illustrated with the help of simple linear example. Different forms of risk sensitive filtering appeared in the literature have been categorized using conventional notation. Errors appeared in earlier publications have been corrected here. The role of risk sensitive parameter and simple guideline to choose such parameter have been provided. An alternative linear matrix inequality (LMI) based formulation of RSE has been proposed. A recursive posterior solution of nonlinear risk sensitive filtering has been derived in the form of a theorem. The proof of the theorem has been done using inductive method. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem. In the upcoming chapter various methods will be developed for nonlinear risk sensitive estimation problem. Among them posterior risk sensitive particle filter and adaptive grid risk sensitive filter will be developed based on the proposed theorem.

74

5 5.1

Risk Sensitive LRKF

Introduction

In the previous chapter basic background of risk sensitive estimation, different formulation and robustness properties have been discussed. In this chapter two different type of risk sensitive filtering for nonlinear system based on linear regression technique are proposed. Basically based on central difference filtering and unscented Kalman filtering technique the risk sensitive version of both (named as central difference risk sensitive filter and risk sensitive unscented Kalman filter respectively) are proposed in this chapter. The primary motivation in formulation of the sigma point based risk sensitive filtering was the numerical efficiency and better approximation of nonlinear function than extended risk sensitive filter (ERSF). It may be noted that central difference filter (CDF) and unscented Kalman filter (UKF) are both the member of the family called sigma point filtering [Merwe 2001a]. Sigma points are generated for both the filters based on cholesky decomposition of error covariance with different tuning parameter. In CDF [Ito 2000] central difference approximation of first and second order increment has been calculated from which mean and covariance of the distribution are approximated. On the other hand in UKF weights are assigned against each sigma points. From sigma points and corresponding weights the mean and covariance of the distribution are approximated. It should be pointed out here that before this work of present author and coworker [Bhaumik 2008, Sadhu 2007, Srinivasan 2007], the only technique available for nonlinear risk sensitive filtering was the extended risk sensitive filter (ERSF) [Jayakumar 1998]. The method is numerically efficient but employs the small signal linearization technique of the extended Kalman filter (EKF) [Brown 1996]. The method captures the essence of risk sensitive filtering for plants with mild nonlinearities. However all shortcomings of the EKF [Brown 1996], including smoothness requirement for the functions and noise Gaussianity restriction are inherited in the ERSF. Hence, the ERSF fails to provide a good estimate and even diverges severely in nonlinear situations. In this chapter the following two different methods are proposed for risk sensitive filtering: (i)

Risk sensitive filter based on central difference approximation called central difference risk sensitive filtering (CDRSF).

(ii)

Risk sensitive filtering based on unscented transformation [Julier 1997, 2000] called risk sensitive unscented Kalman filter (RSUKF).

Chapter 5

The CDRSF has been developed by the present author jointly with Srinivasan under the guidance of his supervisors. The work has been published in journal [sadhu 2007] and also appears in the thesis submitted by Srinivasan [Srinivasan 2007]. Here the CDRSF has been presented very briefly for the sake of completeness of the dissertation. For detail please Srinvasan’s thesis [Srinivasan 2007] is referred. The work RSUKF has not appeared before in anywhere. The formulation has been communicated to “IET Control theory and application” [Bhaumik 2008] by the present author and it has been accepted for publication. The detail formulation of RSUKF, implementation and scope for improvement have been discussed in this chapter. The formulation has been applied to test problems and simulation results are compared with ERSF. 5.2

Risk Sensitive Filter

Consider the nonlinear plant described by the state and measurement equations as follows: x k +1 = φ ( x k ) + wk

(5.1)

yk = γ ( xk ) + vk

(5.2)

where x k ∈ R n denotes the state of the system, y k ∈ R p is the measurement at the instance k where k={0,1,2,3,…,N}, φ ( x k ) and γ ( x k ) are known nonlinear functions of xk and k. The process noise wk ∈ R n and measurement noise v k ∈ R p are assumed to be uncorrelated and normally distributed with covariance Q and R respectively. The following notations have been used for the probability density functions: f ( x k +1 | x k ) ∆

p X k +1 | X k (. | x k ) and g ( y k | x k ) ∆

pYk | X k (. | x k ) .

The objective is to estimate a known function Φ(x) of the state variables. The estimate is designated as Φˆ ( x) and its optimal value in the risk sensitive sense is denoted as Φˆ * ( x) which minimizes the cost function

(

)

(

) (

(

))

k −1 ˆ ,..., Φ ˆ = E exp µ ∑ ρ Φ(x ) − Φ ˆ * + µ ρ Φ(x ) − Φ ˆ* CΦ 1 k 1 i i 2 2 k k i =1 1

where µ1 ≥ 0 and µ 2 > 0 are two risk parameters. Functions ρ1 (.) and ρ 2 (.) are both strictly convex, continuous and bounded from below, attaining global minima at 0. Conventionally they represent squared deviation for the mean squared measure. In particular, the minimum risk sensitive Estimate (MRSE) is defined by

(

)

ˆ * = arg min C Φ ˆ * ,..., Φ ˆ * ,Φ ˆ .. Φ k 1 k −1 k

(5.3)

It can be shown that the MRSE satisfies the following recursion

(

(

))

σ k +1 (x k +1 ) = ∫ f (x k +1 | x k ) g ( y k | x k )exp µ 1 ρ 1 Φ (x k ) − Φˆ *k σ k (x k ) dx k

76

(5.4)

Chapter 5 ˆ * = arg min ∫ exp(µ ρ (Φ(x ) − α )) σ (x ) dx Φ k k k k k 2 2

(5.5)

α ∈R

where σ k (x k ) represents an information state [Boel 2002] and may be normalized and α is a parameter. Here, the variables to be estimated are the state variables themselves ( Φ( x) = x ), and both the convex functions ρ1(.) and ρ 2 (.) are known quadratic functions of vectors, of the form, ρ j (α ) = α T α for j=1, 2. In particular, for prior estimation, we denote the optimal estimate as xˆ k +1|k and accordingly we change the notation for the information state.

Hence, eqns (5.4) and (5.5) can be re-written as,

(

)

σ k +1|k (x k +1 ) = ∫ f (x k +1 | x k ) g ( y k | x k ) exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 ) σ k |k −1 (x k ) dx k T

(5.6)

where

(

)

T xˆk|k −1 = arg min ∫ exp µ 2 (xk − α ) (xk − α ) σ k|k −1 (xk ) dxk .

α∈R

5.3

CDRSF for Prior Estimation

5.3.1

Relations for Prior Estimation

(5.7)

The integrands (5.6) and (5.7) are rewritten as follows: σ k +1|k (x k +1 ) = ∫ f (x k +1 | x k ) σ k |k (x k ) dx k

(5.8)

where σ k |k (x k ) is the a posteriori estimate obtainable from the previous (kth) step using the following relation:

(

(

σ k +1|k +1 ( x k +1 ) = g ( y k +1 | x k +1 ) exp µ 1 x k +1 − xˆ k +1|k

) (x T

k +1

))

− xˆ k +1| k σ k +1|k ( x k +1 ) .

(5.9)

The optimal estimation is given by the expression

(

)

T xˆ k +1|k = arg min ∫ exp µ 2 (x k +1 − α ) ( x k +1 − α ) σ k +1|k (x k +1 ) dx k +1 .

α ∈R

(5.10)

Similar to CDF [Ito 2000], Gaussian approximations have been used in the proposed CDRSF to replace σ k (.) , which makes the integrations (5.8) and (5.9) tractable. Deterministic sigmapoints have been used to evaluate relation (5.8) as a Gaussian quadrature, and another set of sigma-points have been used to evaluate the convolution (5.9). Further, a Gaussian approximation would allow representation of the concerned distributions simply by the respective mean and covariance. To facilitate such approximation, σ k (.) must be normalized to qualify as a probability density function. When Gaussian approximation is used for σ k +1|k (.) , the optimal estimate is simply the mean value of the distribution [Bhaumik 2005b].

77

Chapter 5

5.3.2

Implementation of CDRSF for Prior Estimation

(i) Filter initialization: Initialise the filter with appropriate values of xˆ 0|0 and P0|0 . These are substituted as the initial value and covariance of the information state. (ii) Predictor step: the predicted state estimate and predicted error covariance are evaluated by approximating the nonlinear system dynamics using the central difference approximation approach and it is described as follows: •

Compute the factorization Pk |k = S T S using Cholesky decomposition as in [Ito 2000].

•

Choose a suitable scalar h and compute the central difference approximations of the first and second increments, respectively as a i = H i ,i =

φ (xˆ k |k + hS T ei ) − 2φ (xˆ k |k ) + φ (xˆ k |k − hS T ei ) h

2

φ (xˆ k |k + hS T ei ) − φ (xˆ k |k − hS T ei ) 2h

, and

, where ei is the unit vector in the ith

direction and 1 ≤ i ≤ n . •

Compute the mean and covariance as 1 i =1 2 n

1 i =1 2

n

n

σ k +1|k = xˆ k |k + ∑ H i ,i and Pk +1|k = Q + ∑ a i a i T + ∑ H i ,i H i ,i T , i =1

where Q is the process noise covariance. (iii) Optimization step: With Gaussian approximation, the optimal estimate is xˆ k +1|k = σ k +1|k . The mean of the distribution σ k +1|k (x k +1 ) are denoted by σ k +1|k (iv) Corrector step: From the available predicted state estimation and predicted error covariance the updated state estimate and updated estimate error covariance are evaluated using the central difference approximation of nonlinear measurement function and it is described as follows: The last two terms in equation (5.9) may be combined into an equivalent Gaussian distribution if we take the Gaussian approximation σ~ k +1|k (x k +1 ) in place of σ k +1|k (x k +1 ) . The combined term may

(

be expressed as N(mean, cov) where mean= xˆ k +1|k and cov= Pk++1|k = Pk +1|k −1 − 2µ1 I

)

−1

. Hence, the

corrector step would proceed as follows:

(

−1

)

−1

•

+ Compute Pk +1|k = Pk +1|k

•

Compute the factorization Pk++1|k = S T S using Cholesky decomposition as in [Ito 2000].

− 2 µ1 I

.

~ ~

78

Chapter 5

•

Compute the central difference increments along partial derivative approximations as ~ ~ γ xˆ k +1|k + hS T ei − γ xˆ k +1|k − hS T ei bi = ,and 2h

(

G i ,i =

•

) (

(

)

)

(

γ xˆ k +1|k + hS T ei − 2γ (xˆ k +1|k ) + γ xˆ k +1|k − hS T ei ~

h

~

2

) , where 1 ≤ i ≤ n .

T + Compute xˆ k +1|k +1 = xˆ k +1|k + Lk +1 ( y k +1 − z k +1 ) and Pk +1|k +1 = Pk++1|k − L+k +1 Pxz ,

~ n n 1 T where Pxz = S T (b1 ,..., bn ) , Pzz = ∑ bi bi T + ∑ G i,i Gi,i T , i =1

i =1

2

L+k +1 = Pxz (R + Pzz ) , and z k +1 = γ (xˆ k +1|k ) + ∑ Gi ,i . 1 i =1 2 n

−1

Recursion: Estimates for the subsequent steps may be computed by repeating the predictor, optimization and corrector steps as above for the required number of time steps.

Note (i)

CDRSF and CDF have the same computational complexity as is evident from the algorithms.

(ii)

The posterior form of the CDRSF has been illustrated in [Srinivasan 2007]. It is to be noted that unlike ERSF, there is no need for calculating Jacobian in the process of implementing CDRSF.

(iii)

The variable h signifies the span of the sigma-points and may be chosen appropriately. For Gaussian distribution the recommended value of h = 3

[Ito2000]. For non Gaussian distribution no standard value can be assigned. In that situation the favorable value of h needs to be determined using trial. In this respect idea over the spread of distribution may help the designer. So for nonlinear situation the CDRSF has another more tuning parameter h apart from the risk sensitive factor. (iv)

The condition 2 µ1 Pk +1|k < I should be satisfied for Pk++1|k to be computable in the corrector step. Hence, the risk-sensitive factor µ1 should be assumed to be low enough to ensure convergence of the CDRSF. It is to be noted, however, that the ERSF too suffers from a similar drawback.

5.4

Formulation of Risk Sensitive Unscented Kalman Filter

A general, closed form evaluation of the integral in equation (5.6) is intractable and hence, the well-known unscented transformation (UT) based approach has been used to obtain the approximate solution of the said integral.

79

Chapter 5

However, the unscented transform is not directly applicable and additional steps and assumptions are needed. These are required for (i) obtaining the optimal RS estimate from eqn. (5.7), (ii) reformulating the RS recursive eqns. (5.6) and (5.7) and, (iii) modifying the covariance to take care of the risk factor. Firstly, the information state σ k (xk ) is assumed to be normalized so that it may be interpreted as a probability density distribution. This allows a probabilistic interpretation of the concerned equations and makes the treatment amenable to unscented transform. The algorithm, however, is so arranged that explicit normalization is avoided. In risk-neutral estimation, the mean squared error is to be minimized and the mean of the underlying probability distribution is the solution. The definition of the optimal RS estimate given in (5.7) does not automatically provide the mean as the optimal estimate. However, the mean is the optimal estimate under assumption of Gaussianity. We can further state that even when σ k (xk ) is non Gaussian but may be represented by a mixture of Gaussian densities of the form σ k (x k ) = ∑ wi Ν (mi , Pi ) , (where wi is a weight, mi and Pi are respectively the mean and n

i =1

covariance of the component densities and n is the number of such component densities) the expected value of the distribution σ k (xk ) is the optimal RS estimate, for at least the following cases: (i)

when the component densities have the same mean (but different covariances),

(ii)

when the distribution is composed of identical pairs of mixtures, where each of such identical pair may have components described in (i) above.

Note that the above two situations cover a wide variety of symmetric distributions, including multi-modal distributions. The unscented Kalman filter algorithm makes an implicit Gaussian random variable (GRV) assumption [Ito 2000] while maintaining the mean and covariance through nonlinear transformations. The particular method by which the risk-sensitivity is accommodated within the UKF formalism is described next.

The Prior Form of RSUKF

5.4.1

In order that the UT may be applied, the integrand (5.6) is first decomposed into the following expressions. The integrand (5.6) is rewritten as: σ

k + 1| k

(x k +1 ) =

∫ f ( x k + 1 | x k )σ

k |k

(x k )dx k

(5.11) 80

Chapter 5

(

)

Where σ k |k (x k ) = g ( y k | x k ) exp µ 1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) T

(5.12)

Let us define σ + k |k −1 ( x k ) as

(

)

σ k+|k −1 (x k ) = exp µ 1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) T

(5.13)

Then (5.12) can be re-written as σ k |k (x k ) = g ( y k | x k )σ k+|k −1 (x k )

(5.14)

The expression (5.11) provides the basis for time-update or predictor step equation. The optimal estimate is then obtained by using eqn. (5.7). The expression (5.13) defines the “Riskupdate” step. It is specific to risk-sensitive filtering and there is no analogue in the risk-neutral case. Expression (5.14) provides the modified measurement update or corrector step equation. The computing steps are broadly as follows: (i) Initialization of filter with σ 0|0 and P0|0 and calculation of the weights of the sigma points. (ii) The time update step or predictor step starts with the knowledge of σ k|k and Pk |k from the previous cycle (or from the initial conditions, for the first cycle) and computation of the corresponding sigma points. The step is identical to UKF as the governing relations are also the same, which involves transforming the sigma points and adding the effect of process noise. From the transformed sigma points the updated information state σ k +1|k (x k +1 ) specified by its mean and covariance σ k +1|k and Pk +1|k may be computed using the weighted sum technique of UT. (iii) The optimal estimate may be replaced by the mean of σ k +1|k (x k +1 ) under Gaussianity assumption, as discussed in the previous subsection.

(

)

(iv) In “Risk-update” step, the term exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) is computed by the T

Gaussian distribution assumption, whereby, only the covariance term is affected (the mean of both terms being the same and the resultant covariance is expressed as P + k +1| k = ( P −1k +1|k − 2 µI ) −1 . (v) As the mean and covariance undergo changes due to time update and risk update, it is desirable that the sigma points be recalculated on the basis of σ k +1|k (x k +1 ) and P + k +1|k . If the changes are not significant, this step may be skipped to save some computation. (vi) The measurement update step is identical in form to the standard UKF, yielding σ k|k and Pk |k

using the weighted sum technique of UT.

The specifics of the proposed algorithm are described in the section 5.4.3. 5.4.2

The Posterior Form of RSUKF

The posterior form of the risk sensitive estimate is given by

81

Chapter 5

(

)

T xˆ k |k = arg min ∫ exp µ 2 (x k − α ) ( x k − α ) σ k |k (x k ) dx k .

ζ ∈R n

where

(

(

σ k |k (x k ) = ∫ f ( x k | x k −1 ) g ( y k | x k ). exp µ1 x k −1 − xˆ k −1|k −1

(5.15)

) (x T

k −1

))

− xˆ k −1|k −1 σ k −1|k −1 (x k −1 ) dx k −1

(5.16)

In a manner analogous to the prior RS estimate, we introduce the distribution

(

(

σ k+−1|k −1 = exp µ 1 x k −1 − xˆ k −1|k −1

) (x T

k −1

))

− xˆ k −1|k −1 σ k −1|k −1 (x k −1 )

(5.17)

Hence, σ k | k −1 ( x k −1 ) = ∫ f ( x k | x k −1 ).σ k+−1|k −1 ( x k −1 )dx k −1

(5.18)

where, σ k |k (x k ) = g ( y k | x k ) σ k |k −1 (x k −1 ) .

(5.19)

Eqn (5.17) is defined as the risk sensitive update step, eqn. (5.18) corresponds to the predictor step, and eqn (5.19) corresponds to the corrector (measurement update) step. The filter algorithm for posterior estimation is not being included in this chapter but the steps for the posterior RSUKF proceed in a manner analogous to those for the prior form. However, in the posterior form, it is convenient to consider the risk-sensitive term in the prediction step, and consequently, appropriate changes must be incorporated in the filter algorithm in accordance with eqns. (5.17)-(5.19). 5.4.3

Risk-Sensitive Unscented Kalman Filter Algorithm for Prior Estimation

5.4.3.1 General RSUKF Algorithm for Nonlinear Systems

Step (i) Filter initialization: •

Initialize the filter with σ 0|0 = xˆ 0|0 and P0|0

•

Calculate weights

(5.20)

W0 = κ /(n + κ ) , Wi = 1 / 2(n + κ ) , Wi + n = 1 / 2(n + κ )

(5.21)

Step (ii) Predictor step: •

Assign sigma points with σ k |k and Pk |k as χ k = [σ k |k σ k |k + ( (n + κ ) Pk |k ) i σ k |k − ( (n + κ ) Pk |k ) i ]

(5.22)

where ( (n + κ ) Pk |k ) i is the ith column of the matrix square root of (n + κ ) Pxx , where the matrix square root A of P is of the form P=AAT, [Julier 2000]. •

Update sigma points using process dynamics

χ i ,k +1|k = φ ( χ i ,k ) 2n

•

Compute time updated mean and covariance σ k +1|k = ∑ Wi χ i ,k +1|k

•

Pk +1|k = ∑ Wi [ χ i, k +1|k − σ k +1|k ][χ i, k +1|k − σ k +1|k ]T + Q

i =0

2n

i =0

82

(5.23) (5.24) (5.25)

Chapter 5

Step (iii) Optimal RS estimate: •

With Gaussian approximation, the optimal estimate is xˆ k +1|k = σ k +1|k

(5.26)

Step (iv) Risk sensitive update •

The mean remains the same and the covariance is updated with

•

Pk++1|k = ( Pk−+11|k − 2µ1 I ) −1

(5.27)

Step (v) Re-compute Sigma points •

Sigma points are recomputed with mean as σ k +1|k (x k +1 ) and covariance as P + k +1|k .

Step (vi) Corrector step •

Projected measurement at each sigma point Υi ,k +1|k = γ ( χ i , k +1|k ) .

•

Mean of measurement yˆ k−+1 = ∑ Wi Υi ,k +1|k

•

Calculation of covariances

•

Pyk +1 y k +1 = ∑ Wi [Υi , k +1|k − yˆ k−+1 ][Υi , k +1|k − yˆ k−+1 ]T + R

•

Pxk +1 yk +1 = ∑ Wi [ χ i , k +1|k − xˆ k +1|k ][Υi , k +1|k − yˆ k−+1 ]T

(5.31)

•

Calculation of Kalman gain K k +1 = Pxk +1k yk +1 Py−k1+1k y k +1

(5.32)

•

Posterior state values x k +1|k +1 = xˆ k +1|k + K k +1 ( y k +1 − yˆ k−+1 )

(5.33)

•

Posterior error covariance matrix is given by Pk +1|k +1 = Pk++1|k − K k +1 Pyk +1 y k +1 K k +1T .

(5.34)

2n

(5.28) (5.29)

i=0

2n

i =0 2n

i =0

(5.30)

5.4.3.2 Special Cases

For a nonlinear signal model with either a linear plant or linear measurements, the RSUKF algorithm can be simplified, leading to significant reduction of computation time, especially for systems with high dimensions. This can be done by using the RSUKF algorithm for the step corresponding to the nonlinear equation and the Kalman filter algorithm for the step corresponding to the linear equation. However, there are specific considerations which must be taken into account in each case. Hence, the modifications to the general RSUKF algorithm (in section 5.4.3.1) in each of these two special cases are being stated below.

Nonlinear System and Linear Measurement In this special case, the algorithm may be modified as follows:

Steps (i) - (iv) would be the same as the general RSUKF algorithm stated in section 5.4.3.1. Step (v) is skipped. Step (vi) The corrector step would be the same as the corrector step of the Kalman filter.

83

Chapter 5

Linear System and Nonlinear Measurement For linear process equation and nonlinear measurement equation, the predictor step would be computed by using the Kalman filter algorithm and the remaining steps are the same as that in the general RSUKF algorithm stated in section 5.4.3.1. In particular,

Step (i) Filter initialization is done in the same way as the general RSUKF algorithm. Step (ii) The predictor step is computed in a manner similar to the predictor step of a Kalman filter.

Steps (iii) - (vi) are the same as the general RSUKF algorithm in section 5.4.3.1. 5.4.3.3 Scaled Risk Sensitive Unscented Kalman Filter (Scaled RSUKF) The unscented transformation (UT) may be done in “unscaled” way or in scaled way. In scaled UT the sigma points are drawn with scaled parameters. These sigma points capture the mean and covariance of a Gaussian random variable and when propagated through the true nonlinear system it captures the posterior mean and covariance accurately to the second (extendable to third) order Taylor series expansion for arbitrary nonlinearity. Based on “unscaled” transformation the risk sensitive unscented Kalman filter has been formulated and described in earlier section. Using scaled unscented transformation RSUKF can also be developed. Similar to scaled UKF, in scaled RSUKF some degrees of freedom are available in the choice of sigma point location and it can rectify the numerical problem associated with ordinary RSUKF. The algorithm of scaled RSUKF is as follows:

Step (i) Filter initialization: •

Initialize the filter with σ 0|0 = xˆ 0|0 and P0|0

•

Calculate weights W0( m) =

λ n+λ

W0(c ) =

λ n+λ

+ (1 − α 1 + β ) Wi ( m ) = Wi c =

(5.35)

λ

2

2( n + λ )

; i = 1,2,...,2n

(5.36)

Step (ii) – (vi) would be same as described in section 5.4.3.1 Notes (i)

In risk sensitive filtering, the risk-sensitive parameter provides a tool for design trade off between nominal and robust filtering performance.

(ii)

The algorithms presented here use a basic form of UKF proposed by Julier and Uhlmann [Julier 2000] and also scaled version of UKF. In some rare cases, RSUKF steps may yield a singular covariance matrix. The scaled version of unscented

84

Chapter 5

transform is recommended for that case. Another improved version of UKF called as square root UKF [Merwe 2001a] exists in literature. Risk sensitive formulation of that can also be done in a similar manner. In square root UKF square root of error covariance is propagated directly [Merwe 2001a] to avoid computational cost associated with Cholesky factorization. (iii)

The condition ( Pk−+11|k − 2µ1 I ) > 0 should be satisfied for positive covariance matrix. Hence, the risk sensitive factor µ1 should be assumed to be low enough to ensure convergence of the RSUKF. It is to be noted, however, that the ERSF and CDRSF too suffer from similar kind of restriction.

(iv)

Numerical values of n and κ have been chosen as discussed by Julier and Uhlmann [Julier 2000] where heuristically for Gaussian noise value for (n + κ ) was taken as 3.

(v)

The parameter (n + κ ) in RSUKF can be considered equivalent to parameter h in CDRSF. It can be noticed that in CDRSF the value of h for Gaussian distribution is

3 and for RSUKF the value of

(n + κ ) is also

3 for the same kind of

distribution. (vi)

The main motivation of introducing the scaled parameter is to move the sigma points far away or close to its mean positions. Here λ = α 1 2 (n + κ ) − n is scaling parameter where α 1 is constant to control spread of sigma point around the mean value of random variable which is to be estimated, κ is a secondary scaling parameter and β is a parameter used to incorporate the prior knowledge of the distribution of random vector x . Normally the value of α 1 is small positive value in the range of 1 ≤ α 1 ≤ 10 −4 and κ = 0 . For Gaussian distribution, β = 2 is optimal value. In the

expressions of weight computation, the superscript ‘ m ’ denotes mean and ‘ c ’ denotes covariance. (vii)

Though the risk sensitive particle filter (RSPF) and adaptive grid risk sensitive filter (AGRSF), described in nest two chapters, have better performance than the CDRSF and RSUKF, the computational load is very high for these two filters and these filters may not be suitable in many real time applications, where RSUKF will be a better candidate, especially for higher order systems.

(viii)

For getting more accurate result with a given number of particles, RSPF requires a good proposal density. So far use of ERSF was proposed for this role [Sadhu 2008]. Clearly RSUKF will be much more suitable.

85

Chapter 5

(ix)

Though we used the result that optimal RS estimate is the mean with a Gaussian distribution assumption, it may be shown that this is true for many other symmetrical distributions using the Gaussian mixture models.

5.5

Case Study

Srinivasan in his thesis studied central difference risk sensitive (CDRSF) for both first order and second order nonlinear system including bearing only tracking problem. So in this section the emphasis is given to study RSUKF for different test problems.

Case I Linear Gaussian System In this case one example has been taken where RSUKF has been applied to a single dimensional linear Gaussian model. This example has been considered only to validate the formulation of RSUKF with RSKF in linear Gaussian condition. The system here considered as discussed in section 3.2 (L1). The system matrix (F) and measurement matrix (H) are taken as 0.99 and unity respectively. The uncertainty factor ∆F = 0 and bias is taken as zero. Process and measurement noises are considered as Gaussian with zero mean unity variance. The true state has been initialized with zero whereas the filter state and error covariance have been initialized with unity. The risk factor ( µ1 ) has been taken as 0.0756 for this problem. Fig 5.1 shows that both the RSKF and RSUKF state estimation have same value for this linear problem as expected from theory. This validates the formulation of RSUKF.

Case II Nonlinear Single Dimensional Case We consider the single dimensional process and measurement sequence (N1-1) described in section 3.4. The problem first appeared in [Ito 2000] then it is revisited with a different measurement equation in [Sadhu 2007]. The state of the system has been estimated using the ERSF and RSUKF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. Tracking performance for extended risk sensitive filter (ERSF) and proposed risk sensitive unscented Kalman filter (RSUKF) have been shown for a typical run in figure 5.2. For this representative run, it has been observed that the ERSF fails to track the truth whereas the performance of the RSUKF is satisfactory. The performance of the two filters is compared first in terms of percentage of fail count, which has been defined as the number of cases out of 100 (in a population of 1000 Monte Carlo run) where a filter fails to track the truth i.e. the filter settles at the wrong equilibrium point. For ERSF the percentage fail count is about 26% whereas for RSUKF it is about 1%. The numbers indicate the improvement of estimation accuracy with the proposed filter in comparison to the traditional ERSF.

86

Chapter 5

Comparison of filter performance has also been carried out with Root Mean Square Error (RMSE). The RMSE of RSUKF and ERSF for 1000 Monte Carlo runs are compared in figure 5.3. The figure shows that RMSE of ERSF is high (due to a large population of track loss cases), whereas the RMSE of RSUKF settles at 0.2 units. Also the computational costs of CDRSF and RSUKF are comparable to that of ERSF.

4

RSKF RSUKF

Estimated value

3 2 1 0 0

-1

5

10

15

20

25

-2 -3 Step

Fig 5.1 Identical nature of RSUKF and RSKF for linear Gaussian system

Truth & estimated value

1.5

Truth RSUKF ERSF

1 0.5 0 0

20

40

60

80

-0.5 -1

-1.5

step

Fig 5.2 Performance of ERSF and RSUKF for a representative run

87

Chapter 5

1.2 1 RSUKF 0.8 RMSE

ERSF

0.6 0.4 0.2 0 0

50

100 Step

150

200

Fig 5.3 RMSE plot of ERSF, CDRSF and RSUKF for 1000 runs

Case III

BOT Problem

To demonstrate the multidimensional nonlinear estimation using unscented risk sensitive filtering (RSUKF), the proposed filter has been applied to bearing only tracking (BOT) problem formulated in section 3.6 with the measurement model I described in section 3.6.2. The CDRSF has already been applied to BOT problem and results are available in [Srinivasan 2007]. The RMSE of position and velocity obtained from RSUKF and UKF has been plotted in figure 5.4 and 5.5 respectively. It has been verified that for zero risk sensitive factor the RSUKF exactly matches with ordinary UKF. From simulation it is observed that the in our BOT problem the state estimation is very sensitive to risk sensitive parameter. Only for very small value of risk sensitive parameter the RSUKF exists. It is observed that if the risk sensitive parameter is taken more than 0.00007, the RSUKF does not exist because error covariance loses its positive definite nature in intermediate step. In our simulation the value of risk sensitive parameter is taken as 0.0000605. The superiority of RSUKF in comparison to UKF cannot be established from this problem. Also comparison of RSUKF and ERSF has been made for large initial error condition. The results have been shown in figure 5.6 and 5.7. The improvement of performance by using RSUKF over ERSF is evident from the figure 5.6. The RSUKF is much more robust with respect to large initial errors. In fact, a 1000 cycle MC run shows that there are over 25 cases of track loss in ERSF. The track loss with RSUKF for the particular value of the risk factor is about a seventh of this number of cases.

88

Chapter 5

RMSE of position for UKF & RSUKF 25 RMSE_UKF

20 RMSE (position)

RMSE_RSUKF

15

10

5

0 0

5

10 15 Time (sec)

20

25

Fig 5.4 RMSE of position obtained from UKF & RSUKF

5.6

Scope for Future Work (i) In this chapter RSUKF is realized based on UKF proposed by Julier and Uhlmann in [Julier 1997, 2000]. Scaled version of RSUKF has also been formulated. It is well known that the most computationally expensive operation in UKF corresponds to calculate the sigma points in each instant of time. For that cholesky factorization need to be calculated in each step. To avoid this, square root UKF [Merwe 2001a] has been proposed. In this method the matrix obtained after cholesky factorization will be propagated recursively. Similar formulation for risk sensitive estimation can be possible. This will minimize the computation cost. (ii)

Ito [Ito 2000] proposed Gauss Hermite filter (GHF) in 2000. Like central

difference filter, GHF is also based on quadrature rules. Development of risk sensitive version of the GHF remains under the scope of future work.

89

Chapter 5

RMSE of velocity for UKF & RSUKF 1.2 RMSE_UKF RMSE_RSUKF

RMSE(velocity)

1 0.8 0.6 0.4 0.2 0 0

5

10 Time(sec) 15

20

25

Fig 5.5 RMSE of velocity obtained from UKF & RSUKF Comparison of ERSF and RSUKF

Estimated and truth value of position

250

Truth RSUKF ERSF

200 150 100 50 0

0

5

10

15

20

Time (second)

Fig 5.6 Position estimation obtained from ERSF and RSUKF for large initial position error Comparison of RSEKF and RSUKF

Estimated and truth value of velocity

2

Truth RSUKF RSEKF

1

0 0

5

10

15

20

-1

-2

-3 Step(second)

Fig 5.7 Velocity estimation obtained from ERSF and RSUKF for large initial position error

90

Chapter 5

5.7

Discussion and Conclusion

In this chapter two risk sensitive filters, named as central difference risk sensitive filter, and unscented risk sensitive filter, based on sigma point approximation have been formulated. The CDRSF has been developed jointly with Srinivasan under the guidance of supervisors [Sadhu 2007] and it has been appeared in the thesis submitted by Srinivasan [Srinivsan 2007]. So in this chapter main emphasis has been given over RSUKF. For detail formulation of CDRSF reader may read Srinivasan’s thesis [Srinivasan 2007]. ERSF inherits the shortcomings of EKF where as RSUKF retains the advantage of UKF in handling nonlinear models. This may be attributed to the fact that the UKF considers higher order terms in Taylor series where as a simple EKF makes only a first order approximation. The developed RSUKF technique has been applied to a linear Gaussian system. The result exactly matches with RSKF. This validates the algorithm. More over the RSUKF has been applied to single dimensional nonlinear test problem. The result has been compared with ERSF. The results suggest the superiority of RSUKF over ERSF in terms of estimation accuracy. The computation time necessary for RSUKF is comparable to ERSF. More over RSUKF has been applied to BOT problem. Superiority of RSUKF over ERSF is obtained in terms of immunity to failure.

91

6 Adaptive Grid Risk Sensitive Filter 6.1

Introduction For state estimation in nonlinear risk sensitive problem, prior to this work, only extended

risk sensitive filter (ERSF) is available in literature. In the earlier chapters of the thesis other closed form solutions for nonlinear risk sensitive estimation such as central difference risk sensitive filter [Sadhu 2007] (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) [Bhaumik 2008] have been proposed. The above-mentioned estimators are computationally efficient but suffer from the limitation arises due to approximation made in linearization process. Due to computational efficiency these filters may be useful for onboard application in aerospace problem in near future. But it is necessary to compare the performance of those approximate filters with a benchmarking algorithm to quantify the compromises made for not providing high computational power on board and also for tuning purpose. In non-robust estimation scenario Cramer Rao bound is being used for such benchmarking process. In robust estimation, nonexistence of such lower bound makes it difficult to compare the filtering performance with respect to achievable performance. This situation motivates to develop algorithm for risk sensitive filtering, which is not limited by nonlinearity or non gaussianity of the problem. Towards this goal two types of nonlinear risk sensitive filters based on grid (or point mass) adaptation and particle filtering method have been proposed in this thesis. Among them in this chapter the method based on grid adaptation technique has been proposed and exemplified. The method is named as adaptive grid risk sensitive filtering (AGRSF) [Bhaumik 2005, Bhaumik 2006a, Bhaumik 2007]. The main disadvantage of the method is its high computational cost especially for higher dimensional system. Computational load increases in power with dimension of the system. The drawback is well known as curse of dimensionality. But in the absence of any computing resource and time constraints, the method can estimate as accurate as desired. So the proposed method can be used off line and the estimated value obtained from this filter can be used to compare the performance of linear regression based risk sensitive estimators. The ordinary grid based or point mass technique was first reported in early seventies (Bucy 1971, Sorenson 1974). Fundamentally the method approximates continuous state space by grid of isolated points. Values of conditional probabilities are calculated only at the grid points. The method suffers from many disadvantages like poor numerical efficiency and finite resolution. Non-availability of computer makes the method as non-implementable at that time. Later Bergman [Bergman1997, Bergman 1999 b] modified and reformulated the algorithm and

Chapter 6

implemented for terrain-aided navigation. In that case uniform grid has been used although some preliminary adaptation methods have been implemented. The concept of grid adaptation had also been tried in [Simandl2002, Sun2005]. A short tutorial on point mass filtering has been made by Chen [Chen2003] and Arulampalam et al [Arulampalam 2002]. Simandl in his report [Simandl 2006] discussed about a technique similar to grid adaptation in the name of floating grid and anticipative grid. He also discussed grid design technique for single dimensional as well as multidimensional system. In [Srinivasan 2007] methodologies for efficient determination of the point mass in state space and various effective ideas towards the grid adaptation (such as use of posterior proposal, axes transformation, efficient grid interval) have been reported. It has been realised that among the various grid adaptation methods available in the literature, techniques proposed in [Srinivasan 2007] is theoretically much superior, and easier to implement. Hence in the process of formulating AGRSF all the grid adaptation techniques proposed by Srinivasan [Srinivasan 2007] have been adopted for efficient utilisation of point masses. As discussed above risk sensitive estimation based on grid adaptation technique will be proposed in this chapter. The risk sensitive filtering has been developed from fundamental principles so that it becomes easy to understand debug and implement. It should also be noted that this is the first attempt to solve nonlinear risk sensitive estimation problem numerically using grid-based method. The sections in this chapter have been organised as follows. In the next section ordinary grid based solution of the risk sensitive estimator problem has been formulated. Immediately the weakness of ordinary grid based risk sensitive filter have been realised. So in the following section various grid adaptation techniques adopted in this work have been discussed in detail. After that search algorithm necessary for AGRSF has been discussed. The developed AGRSF has been applied to a single and two dimensional linear problem to validate the formulation of proposed filtering technique. Next the developed filter has been applied to single dimensional nonlinear problem and 2D bearing only tracking problem. Finally the chapter ends with conclusion where various advantages, disadvantages and scope for future work have been discussed.

93

Chapter 6

6.2 6.2.1

Grid Based Solution of Risk Sensitive Estimation Problem Risk Sensitive Estimation Problem Consider a general (nonlinear) signal model consisting of equations for the state ( xk ∈ R n )

and measurement yk ∈ R p with additive, uncorrelated noise wk ∈ R n , vk ∈ R p of known statistics at the instance k ={0,1,2,3…}. x k +1 = f ( x k ) + wk

(6.1)

y k = g ( xk ) + vk

(6.2)

The vectors f ( x k ) and g ( x k ) are general (without any assumption of smoothness) nonlinear function of x k and k . The generalized cost function for risk-sensitive estimator at any instance k can be defined as [Boel 2002]. k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp( µ1 ∑ ρ1 (φ ( xi ) − Φ i 2 2 k i =0

(6.3)

The constant parameters µ1 and µ 2 are called risk-sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted by ˆ , and ς is a parameter. Functions ρ (.) and ρ (.) are both strictly convex, continuous and Φ i 1 2

bounded from below, attaining global minima at 0. Here, the variables to be estimated are the state variables themselves ( φ ( x) = xˆ ), and both the convex functions ρ1(.) and ρ 2(.) are known quadratic functions of vectors i.e., ρ j (α ) = α T α for j=1,2,…. We denote the optimal estimate as xˆ i and accordingly we change the notation for the information state. k −1

J RS (ς , k ) = E[exp( µ1 ∑ ( x i − xˆ i ) T ( xi − xˆ i ) + µ 2 ( x k − ς ) T ( x k − ς ))] i =0

(6.4)

The initial state x 0 is independent of the noise processes mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by µ 2 .The current optimum estimate xˆ k is obtained by finding the optimum value of ς , which

minimises J RS (ς , k ) , i.e. xˆ k = arg min J RS (ς , k )

(6.5)

ς

The constant parameters µ1 and µ 2 are called risk sensitive parameters.

94

Chapter 6

6.2.2

Recursive Solution of Risk Sensitive Estimation The recursive solution of RSE may be realized generally in a two-step process. First, a

recursive relation of an information state [Boel 2002, Dey 1997] is formulated, which can be updated in each time step. In each time step, the optimal estimate is then obtained by extremizing the cost of another function involving the information state. The posterior estimate of state for nonlinear risk sensitive estimation can be realised by the following two recursive equations. +∞

xˆk +1|k +1 = arg min ∫ exp[µ2 ( xk +1 − ς )T ( xk +1 − ς )]αk +1dxk +1 ς

(6.6)

−∞

Where +∞

αk +1 = p( yk +1 | xk +1) ∫ exp[µ1(xk − xˆk )T (xk − xˆk )]αk p(xk +1 | xk )dxk

(6.7)

−∞

The proof of the above two recursive relations has been provided in chapter 4. 6.2.3

Grid Formulation

The integrations appeared in relations (6.6) and (6.7), may not, in general be evaluated in closed form for nonlinear or non Gaussian cases. A convenient numerical technique to evaluate this integral is point mass approximation, which has been used in grid based filter. 6.2.3.1

Representing Probability Densities as Point Mass on Grid Points

The problem is to approximate at time step k, a probability distribution pk(x) defined everywhere in state space denoted by x. Let it be known apriori that outside the region Γ bounded by x=X0 and x=Xf, the probability density is negligibly small. In the region of interest Γ, the state space is then subdivided into N grid points, (with not necessarily equal intervals). In particular, at time step k, let the nontrivial grid points (assumed to be located at the centroid of the hyper volume enclosed by intersecting adjacent grids) in state space be denoted as x kj for j= 1,2…N. These points are also called the support points. In the simplest case, consider a single state variable. From prior knowledge, one can set the minimum and maximum values and divide the interval into N grids. The central point between grid line zero and grid line 1, may be called as the grid point 1. Similarly grid point j will be in the centre of grid line j-1 and j. The j-th hyper volume in this case would be the line segment between the j-th and j-1th grid lines. In two dimensional case, state variables may be divided into N1 and N2 grid lines, creating N=N1 X N2 grid points. The area bounded by the adjacent grid lines will be the corresponding grid hyper volume (surface). The grid points need to be numbered systematically. The idea of grid and support points is shown in figure-6.1.

95

Chapter 6

For grid point x kj , let the probability mass be wkj and the corresponding grid hyper volume is q kj . N

The weight for each point mass is ϖ ki = q kj wkj . We also note that ∑ ϖ ki +1|k = 1 . The probability j =1

N

density may be approximated by point mass as p( x k ) ≈ ∑ϖ ki δ ( x k − x ki ) . i =1

Though the information state α k is not a probability density, it does represent a density. It may therefore be approximated likewise. Support points for α k are generally chosen to be the same as for p( x k ) . It may be seen easily that estimation is unaffected if α k is multiplied by a constant factor. It is often advantageous to normalize the distribution of α k . 6.2.3.2

The Grid Based RSE Algorithm N

The initial state variable is first discretized into grids and point mass as p 0|0 ( x 0 ) ≈ ∑ ϖ 0i δ ( x 0 − x 0i ) . i =1

N

The information state likewise may be discretized as α 0 (ς ) ≈ ∑ α δ (ς − x ) . The information state i =1

i 0

i 0

is updated from k-th to k+1-th step as: N

α ki +1 ( x ki +1 ) = p v ( y k +1 − g ( x ki +1 )) ∑ {exp[ µ1 ( x ki − xˆ k ) T ( x ki − xˆ k )]α kj ( x kj ) p w ( x ki +1 − f ( x kj ))} j =1

(6.8)

The convolution like summation expression comes from the theory of approximate grid based filter [Ristic 2004a] and it requires the bulk of computational effort. Unlike the expression given in [Boel 2002], the distribution pv (.) appears outside the summation and provides substantial computational economy. The alpha masses are then normalized so that these sum into unity. The objective function to be minimised may then be approximated for any arbitrary point ς as: N

J RS (ς , k ) ≈ ∑ exp[ µ 2 ( x kj − ς ) T ( x kj − ς )]α kj

(6.9)

j =1

The risk sensitive state estimate is obtained by determining the optimising solution for the objective function. Note that, compared to [Dey 1997], the objective function above is simpler and optimisation is easier. Note also the difference with [Boel 2002]. Out of several possible methods for determining the optimum, the following has been used in the present work and is therefore recommended. To obtain the optimum, solve for the zero value of the partial derivative of the objective function, i.e., N ∂ J RS (ς , k ) = 0 ⇒ ∑ ( x kj − ς )α kj exp[µ 2 ( x kj − ς ) T ( x kj − ς )]α kj = 0 j =1 ∂ς

Standard techniques like Newton-Raphson method may then be employed.

96

Chapter 6

Fig 6.1 Idea of Grid and supports

6.2.3.3 Weakness of Simple Grid Based Risk Sensitive Filter Ordinary grid based risk sensitive filtering contains all the weaknesses of simple grid based filtering. The weaknesses of simple non risk sensitive grid based filtering have been reported [Srinivasan 2007]. Here the author summarizes the limitations and weaknesses of simple grid based risk sensitive filtering.

(i)

The root cause of the weaknesses of conventional grid based filters is due to the use of fixed grid points or point masses in space. For example suppose the filter started with the fixed point masses, which covered the region of probability density function well. As time proceeds the probability density function may shift to some other region, which was not perceived earlier. As a result scarcity of point mass will occur to represent the probability density function. So to obtain the accuracy in filtering one should choose large span of grid points as well as small hyper volume of cell. Again to implement the grid-based filter one should evaluate the convolution like summation (equation 6.8) and search method. These require bulk of computational effort.

(ii)

Another well-known and cited weak ness is huge computational cost especially for higher dimensional system. This limitation is named as curse of dimensionality [Bergman1999]. Suppose Ni be the number of point mass chosen for i-th state variable. In such a case to represent nth order system number of grid n

points necessary are N = ∏ N i . With the increase of dimensionality the number i =1

97

Chapter 6

N tend to be very large. However it should be noted that Monte Carlo based estimation techniques such as particle filtering also having similar curse of dimensionality problem.

(iii)

It has been discussed later that to evaluate equation (6.6) search mechanism is necessary. The mechanism also contributes toward computational load of the filter. To get rid of search mechanism one can assume approximately the grid distribution as Gaussian and can satisfy with mean and covariance of the population.

Initially equal interval grids have been used to describe probability function in state space. Although the probability density function may have peaks in some region or may be multimodal in nature. For example, if the posterior probability density is of normal distribution, it has peak around mean and if we consider particle cloud for this distribution it is dense in one sigma region and density gradually decreases from there and become almost transparent towards three sigma region. So to represent that probability density more grid points are necessary where the particle cloud density is more and vice versa. So uniform interval grid points should be modified based on probability density function. From the discussion it is also obvious that the concept of fixed grid point should also be modified for efficient utilization of the points. It is therefore necessary to determine the bounds ([xmin xmax]) of the state variables at each time step before selecting the support points. The bounds may be obtained approximately from the mean and covariance by using the relations [xmin xmax]= mean ±3*Factor*sigma. Here sigma is the standard deviation and “Factor” is a tuning parameter, which stretched or squeezed the probability density function. Depending upon the guess of trail of the pdf that factor can be selected.

6.3

Grid Adaptation Techniques Based on our above discussion the grid method tends to be computationally more expensive

compared to particle filter [Sadhu 2005, Bhaumik 2006b, Sadhu 2008]. The numerical efficiency of grid adaptation technique depends on the choice of support points. If fixed support points are used (as is the case for ordinary grid based filters), either many such support points are redundant and contribute only to the numerical load and not the accuracy of estimates or else, if too coarse a grid is chosen, computation efficiency suffers. So it is clear that to increase the numerical efficiency, the number of grid points should be chosen such as to give best estimation of states. In this section techniques used for grid adaptation have been presented elaborately.

98

Chapter 6

6.3.1

Obtaining Approximate Boundaries of the Region of Interest To draw the point mass or grid point first we need to know the boundary or region of

interest. For a single dimensional case it can be obtained approximately from the mean and covariance by using the relations [xmin xmax]= mean ±3*Factor*sigma as discussed earlier. It should be noted that the region of interest at each time step, should be as small as possible, consistent with the accuracy requirement. From the proposal distribution, the boundaries of the region of interest may be obtained. This is done by utilizing the concept of sigma-contour (for 2-D problems), sigma-ellipsoid (for 3D) and sigma-hyper-volume (for n-D). We would use the name sigma-contour generically for all dimensions. The centroid of the sigma contour is always located at the mean value of the distribution. Let us assume only the zero mean situations. The more general case is given in the algorithm. The m-sigma contour (m=1,2,3, or any real number) for an n-Dimensional distribution with a covariance matrix P is defined as the locus of the points satisfying the n-Dimensional quadratic equation x T P −1 x = m 2 , in state space. Consider a 2-D example where P −1 =

β 11

β 12 , the locus β 22

β x 2 ] 11 β 21

β 12 [x x ]T = m 2 β 22 1 2

β 21

of

the

m-sigma

contour

is

given

by

the

equation

[x1

⇒ β 11 ( x1 ) 2 + ( β 21 + β12 ) xx2 + β 22 ( x 2 ) 2 = m 2

This is clearly an equation of ellipse. If the distribution is Gaussian-like with normal tails, a 3sigma contour will contain almost all the points with significant density. (For a heavier tailed distribution, a higher value of m may be necessary). The task is then converted to finding the extremities of the 3-sigma contour and putting a rectangular box of grid to fit the contour. The quantity m will henceforth be called as the span factor. Figure 6.1 shows one sigma and three sigma contours. Extremities (Xmax, Xmin, Ymax, Ymin) of the m-sigma ellipse can be used to box the region will also lead to wastage. The x1 , x 2 axis intercepts would then be ± m

1

β 11

,± m

1

β 22

, respectively.

If the axes of the ellipse do not coincide with the state variables, the knowledge about the intercepts is not so useful. It should be noted that the off diagonal elements of the covariance represent the correlation coefficients between the state variable. If the off diagonal terms of the covariance matrix or correlation between variables are zero then the ellipse becomes circle and the axis of the circle coincides with the axis of state variable. It should also be noted that as the

99

Chapter 6

value of correlation terms increases the electricity of the ellipse also increases. So for the cases where the axis of ellipse does not match with the axis of state variable, what we really need are the extremities. The extremities would be the point where the tangents to the contour will be parallel to the axes. However, it is better to postpone the computations of the extremities until the coordinates are properly aligned which will be discussed later. We note in this context that, if the axes of the ellipse do coincide with the state variables, the intercepts immediately become the semi-major/minor axes and the dimension of the enclosing box is immediately obtained. 6.3.2

Selecting Grid Intervals

Next step is to choose the number of division in each dimension. At each time step, the number of intervals in each direction should be chosen to be consistent with the span of the state variable and the required resolution. The first easy approach is to generate the grid uniformly. Consider first a single order example. Let the region of interest is known to be [xmin xmax] and the allowable maximum error is ε. The minimum number of uniform divisions (grids) would then be Nε =(xmax- xmin)/ε. The idea can be extended to multiple dimensions as well. In figure 6.1 it is easy to note that the grids have been generated uniformly. It has been argued that there would be enough support points in the core area where a solution is expected and adequate points to represent the density in the fringe/tail areas. Based on the prior knowledge the gross nature of the posterior distribution can be known. So based on the gross nature of the posterior distribution core area and fringe area can be chosen approximately. For Gaussian distribution the core area may be taken as the one-sigma boundary and the fringe area could be the three-sigma boundary around the estimated state, leaving out the core. The idea is to keep approximately 60% of the support points in the core area and the rest for the fringe. It should be pointed out here the concepts of core and fringe area is suitable for unimodal distribution. For bimodal or multimodal distribution which may arises due to nonlinearity in process dynamics such concept cannot be applied. The idea of generation of support point in fringe and core area has been illustrated in figure 6.2. Here a Gaussian distribution with zero mean and sigma variance (sigma=[1 0.35; 0.35 1]) has been considered. Here the core area has been defined as 1 sigma contour where the fringe area has been defined as 3-sigma contour. The figure shows the inadequacy of the one-sigma region as assumption for core. It is therefore more pragmatic to define the core area to be about 1.5 to 2 times equivalent sigma. For a given nonlinear model, some experimentation may be necessary for choosing core and fringe region.

100

Chapter 6

Fig 6.2 selections of grid intervals

As no rigid line of demarcation can be made between the core and fringe region another method has been tried where the inter grid distance gradually increases as the grid distance from the mean increases. In this method grid templates, which can be used for every time step to generate grid point, can be calculated using following simple Matlab code. The generated grid in this approach has been shown in figure 6.3. ======================================== %% Algorithm for Template generation %% Npart1 is the number of template to be generated for one state %%variable Npart1=15; Gaps=zeros(1,Npart1)+1;Ticks=zeros(1,Npart1+1);xsum=0; Ticks= -1:2/(Npart1):1; for i=1:Npart1; Gaps(i)=Gaps(i)+((fix(i-(Npart1+1)/2)/Npart1)^2)*8; xsum=xsum+Gaps(i); end %%%%Normalised gaps and ticks Gaps=Gaps/xsum; for j=1:Npart1;Ticks(j+1)=Ticks(j)+Gaps(j);end Ticks=Ticks-0.5; Ticks=2*Ticks; =========================================

101

Chapter 6

Fig 6.3 Alternative method for selection of grid intervals

6.3.3

Alignment of Axes

From the previous figures it can be noted that lot of grid points have been unutilised due to mismatch between axis of sigma contour of probability density function and axes of state variables. If the covariance of the probability density function contains no correlation terms (off diagonal terms) then the sigma contour becomes circle and its axes orientation is in the axis of state variable. But during simulation the correlation terms arise obvious way even if in the initial covariance the states are not correlated. So it is necessary to orient the grid points in the axis of sigma contour and this can be done quite easily. The distribution of grid points after alignment of the axis will be looking like the figure 6.4. Advantage of the axes alignment method can be realized by comparing the figure 6.3 and figure 6.4. As a continuation of the discussion from 6.3.1 it can be stated that axes of the ellipse/ellipsoid x T P −1 x = m 2 would be along the eigenvectors of the symmetric matrix P −1 . The extremities of the

ellipse would be again at ± m

1

β 11

,± m

1

β 22

, only, the diagonal elements βj jj would be the

corresponding eigenvalues. From this, the extent of the area / hypervolume of interest is obtained directly. To get the grid points, first the grid divisions are carried out along the eigen vectors (which are pure numbers). Intersections of grid lines (parallel to the eigenvectors) are the grid points and the grid points are then transformed back into the original state space coordinates.

102

Chapter 6

The idea of using eigenvectors as preferred grid orientation was also mentioned in [Sorenson1974]. The concerned matrix being positive definite and symmetric, the computational cost of eigenvalues and eigenvectors will be reduced. The computational cost may be further reduced by noting that the eigenvalues and eigenvectors of P −1 and P are related. P −1 v j = λ j v j ⇒ v j = λ j Pv j ⇒ Pv j = (

1

λj

) v j Thus, the eigenvalues of P −1 and P are inverse of

each other and the eigenvectors are the same. Thus these quantities for P −1 can be computed without inverting the P matrix. Srinivasan in his thesis [Srinivasan 2007] developed another method called triangular square root (cholesky) method for alignment of the axes. The method is based on Cholesky factorization.

The method has been described in detain in [Srinivasan 2007] and will not be discussed here. To obtaining approximate boundaries of the region of interest, selecting grid intervals and to orient the position of the grid, an approximate knowledge of the posterior density would be required. So proposal is necessary from which the grid points can be generated in an efficient way. Method by which the approximate distribution can be obtained will be discussed in the next section.

Fig 6.4 Alignment of axes to reduce wastage of grid

6.3.4

Determination of Approximate Posterior Distribution

As discussed in the previous section approximate knowledge of posterior probability density function is necessary to generate the grid points efficiently. This is the most difficult part of the AGRSF method. But it is not impossible task. Several options have been tried in this work and discussed below. Aligning with the nomenclature use in particle filtering, this approximate

103

Chapter 6

posterior probability density function can be named as proposal density (distribution) and here the approximate posterior probability density function obtained to place the grid points will be called by that name. (iv)

At first probability density of the previous step (prior distribution) has been tried to create the grid points. However, due to time update and measurement update, the probability density at the next step may become radically different. So use of prior density function as a proposal may leads to generate the grid point in some other region of state space. One choice to cope up with the problem is broadening the region of interest. But in this process with in a few steps the region of interest will grow and blow. Therefore this method was rejected.

(v)

The next option tried for proposal distribution is to use the time updated distribution and then eliminating the fringe areas and thus obtaining approximate boundaries of the region of interest. However, determining the region from the point mass distribution often proved to be computationally expensive and therefore this method was rejected.

(vi)

The next successful attempt was to use a posterior Gaussian distribution of state as a proposal for the next step. As Gaussian distribution requires only the mean and variance, this option was considered attractive. To compute the mean and the covariance approximately at each time step additional computation using closed form filters can be carried out using computationally efficient methods. The concept is illustrated in figure 6.5. Here Extended Kalman filter has been used to generate proposal for AGRSF. Once the approximate distribution has been known as Gaussian, the grid points can be created in efficient way by following the combination of above described methods.

(vii)

One may of course use more sophisticated estimation methods like sigma point Kalman filter, central difference Kalman filter or other filters. Such sophistication method has not been tried to generate proposal in the present work. It must be pointed out that the approximate knowledge of mean and covariance is used only to place the grid points. Small errors in computation of these quantities do not have any significant impact on the final result. Sophisticated, computational methods for these approximations therefore would not be justified.

(viii)

In the context of grid placement for AGRSF the proposal can also be generated from the simple risk sensitive filters such as extended risk sensitive filter (ERSF)

104

Chapter 6

or other sophisticated risk sensitive filters. But the method has not been tried in the present work and remains in the scope for future work. All the concepts discussed in this section entitled “grid adaptation technique”, have been implemented in Matlab environment and the figures presented above (figure 6.1 – 6.4) are all generated through Matlab programming.

Fig 6.5 Determination of mean and co variance from EKF

6.4

Search Algorithm

To obtain the estimated value of states the equation (6.6) need to be evaluated. In discrete domain N

the equation (6.6) can be written as xˆ k +1|k +1 ≈ arg min ∑ exp[ µ 2 ( x kj +1 − ς ) T ( x kj +1 − ς )]α kj+1 . ς

j =1

Determination of xˆ k +1|k +1 from this expression can be done using search mechanism in state space. To obtain or to search the appropriate value for states several mechanisms, tried in the thesis, have been described below.

(i)

At first simple search mechanism has been implemented for AGRSF. In this mechanism solution has been sought among the grid points. So ς has been considered as x kj+1 . So for each ς the summation has been evaluated for arbitrary fixed µ 2 and state value has been chosen corresponding to least value of summation.

(ii)

It is to be noted that the accuracy of the search mechanism depends on the resolution of ς value. Later for search, ς value has been taken as the grid points as well as the middle point of the grid volume. It should also be noted that as the resolution of ς is

105

Chapter 6

going to be increased the computational burden of the search algorithm is going to be increased. So designer should choose the trade off between the accuracy versus computational load.

(iii)

The combination of the above two methods has also been tried where at first approximate value of state has been searched by method (i). Next around this value the searched has been refined using method (ii). Using this scheme accurate value of states can be searched with computational efficiency.

It should be pointed out here AGRSF has been computationally loaded due to generation of grid points, assigning weight and update the grid points in state space. Again search mechanism increases its computational burden more. So search mechanism can be replaced by assuming normal probability density function and hence finding out the mean and variance.

6.5

Implementation of Risk Sensitive Adaptive Grid Filtering Algorithm The main components of the AGRSF algorithm have been discussed earlier in a little bit

scattered way. The whole algorithm of AGRSF is assembled here for the designer. At the end of this section some notes have been added which may be useful to understand and implement the algorithm properly. Grid Template Generation

1. Select N i , the number of grid intervals in each axis, adequate for typical resolution requirements. Total number of grid points N ≤ ( N i ) n , where n is the dimension of state space.

2. The radius of the m-sigma contour (hyper-sphere) needs to be specified. 3. Divide the diameter of length 2m into approximately N i intervals. The intervals will be symmetrical about the centre and spaced according to the non-uniform spacing strategy described earlier.

4. Systematically generate the grid points in the hyper-cube space. Let z i be a typical point, with coordinates ( z1i , z 2i , z 3i ,....z ij ,.., z ni ) . The support (Grid) points coordinate component z ij would be located at mid points of corresponding grid intervals.

5. The corresponding cell volumes q~ i are generated for each support points. 6. Normalize q~ i ’s to produce q i . 7. Store the N x n coordinate points in Z-plane as the grid template.

106

Chapter 6

8. Once the grid template is formed, at each time step, the grid points in original coordinate are obtained by

x i = Sz i . Where S is the lower triangular matrix square root for the

covariance matrix. Initialization

9. (i) Set initial condition (xˆ 0|0 , P0 ) , which is assumed or given. (ii) From the initial covariance P0 , the grid alignment transform S 0 is obtained using T

S 0 S 0 = P0 .

(iii) The initial support points x 0 i , i=1,2,3…N, are obtained from x 0 i = S 0 z i + xˆ 0|0 . (iv) The weights ϖ 0i , corresponding to x k i are computed from Pk , xˆ 0|0 . The weights can be calculated by multiplying the cell volume ( q i ) with probability density function ( wki ) for the grid point associated with that volume. The weights are then normalized. Recursion (to obtain estimates for k+1-th step) The estimate xˆ k |k , error covariance Pk , support points x k i , i=1,2,3, …N, pdf wki 10. An approximate value of the posterior mean ξ k +1 ≈ xˆ k +1|k +1 at k+1-th step is obtained, by a linearised approximation of the filter, say, EKF. Such filters use the mean xˆ k and ~

covariance Pk as starting point. The approximate posterior error covariance Pk +1 comes as a by-product. 11. The lower triangular matrix square root for approximate posterior covariance S k +1 is then ~

~

obtained from Pk +1 , using S k +1S k +1T = Pk +1 . 12. From the grid template, S k +1 and approximate posterior mean ξ k +1 , the new support points x ik +1 , i=1,2,3, …N are obtained from the relation x k +1 i = S k +1 z i + ξ k +1 . 13. In the time update step (convolution), the weights ϖ ki +1|k corresponding to x ik +1 are obtained from the old and new supports, old weight, the process noise distribution function and the relation N

ϖ ki +1|k = q i ∑ {ϖ ki ( x kj ) exp[ µ1 ( x ki − xˆ k ) T ( x ki − xˆ k ) p w ( x ki +1 − f ( x kj )} j =1

14. In the measurement update step, the weights ϖ ki +1|k corresponding to x ik +1 are obtained from, the relation ϖ ki +1|k +1 = ϖ ki +1|k × p v ( y k +1 − g ( x ki +1 ))

15. The set of weights ϖ ki +1|k +1 are then normalized.

107

Chapter 6

16. The expected value of the posterior estimate (optimum in minimum mean square sense is obtained from xˆ k +1|k +1 =

1 N i i ∑ ϖ k +1|k +1 x k +1 or it can be found out using search algorithm N i =1

described previously.

17. The updated posterior covariance matrix Pk +1 , may likewise be computed using the relation. Pk +1 =

1 N i i i T ∑ ϖ k +1|k +1 (x k +1 − xˆ k +1|k +1 )(x k +1 − xˆ k +1|k +1 ) . N i =1

18. Set k=k+1 and go back to step (10). 6.5.1

Notes on AGRSF Algorithm a. Notes on grid template i. Eigenvector method allows us setting up grid templates, which can be

used for every time step.

ii. The outer area or the boundary of the grid points has been selected by choosing the value of m. Choice of this span factor (m) may be obtained after preliminary experimentation. The default value is 4. iii. Often, for convenience the number of intervals for each axis is chosen to

be the same. The number of intervals in each axis N1, assuming (N1= N2=N3 etc) is chosen, from a rough idea of the range of the variables and the necessary resolution. The approximate number of grid points is then the product of N1, N2, N3 etc, i.e N = ( N 1 ) n . iv. It may be noted that only relative volumes of the grid cells are important. So after the proposed linear transformation the relative volumes of the grid cell will not be changed. To compute the relative volume of a particular (say i-th) cell the sides of the volume are determined from the grid intervals and multiplied to produce the absolute volume q~ i . All the q~ i ’s are then normalised to q i , either to sum to unity or with respect the central cell (at the origin) volume.

b. Prior estimate In this algorithm the estimated state value is posterior. Similar algorithm can be formulated for prior risk sensitive adaptive grid estimation using delayed measurement.

108

Chapter 6

c. Computational cost (i)

Computational cost for N 2 multiplication and function evaluation (necessary to evaluate equation 6.7) is high. The computational cost for this convolution depends on the number of grid points in state space. Efficient choice of grid points using proposed method could reduce the computation cost for this N 2 multiplication.

(ii)

Cost of measurement update, N number of multiplications and likelihood function evaluations, is not very high and also depends on support points. With the same number of support points, the cost is same as that for particle filter.

(iii)

The cost of either eigenvalue or eigenvector computation is not significant for N >> n . The computational cost is of the order of n 3 , with some savings because of symmetry.

(iv)

The computational cost due to search mechanism is high and approximately in the order of N 2 . Approximating the probability density function as Gaussian and thus calculating the mean instead of searching from the state space can save the computational cost.

(v)

Cost of covariance computation is equivalent to Nn 2 multiplication. It may be tempting to use the approximate covariance obtained from EKF and save this cost. This is not recommended, as the use of approximate covariance usually tends to more wasted points and eventual track loss in extreme case. Moreover, with the same number of support points, the cost is the same as that for a particle filter.

d. Different proposals It has been discussed earlier that proposal is necessary to draw the grid points in an efficient way. In this work the pdf obtained from EKF has been tried as a proposal density function. Other sophisticated filtering algorithm (UKF, CDKF) etc can be used to generate the proposal for AGRSF. Also closed form risk sensitive filters proposed in previous chapters, such as ERSF, RSUKF and CDRSF, can be used as a proposal. But these methods have not been tried in the present work and remains in the scope for future work.

109

Chapter 6

6.6 6.6.1

Example and Case Study Case Study I

First Order Linear System

The AGRSF has been validated against Risk Sensitive Kalman Filter, for linear Gaussian case. In first case we consider a single dimensional system as described in section 3.2. Here system parameters are taken as F = 0.99 , H = 1 . In the earlier chapter it has been realized that risk sensitive filter performs significantly well in presence of unmodelled bias. So to verify this property with AGRSF the same model has been taken into consideration. The bias is a constant, set equal to 0.2 and is assumed to be unknown to the filter. Figure 6.6 shows a typical run with a first order system described above for 20 steps. It may be seen that the AGRSF estimate with 50 grids is virtually coincident with the ideal risk sensitive estimate obtained by RSKF. The result validates our proposed AGRSF formulation. The improved estimation of risk sensitive filter compared to KF is also evident. The tendency of convergence of RSKF algorithm with the increase of the grid point is shown in figure 6.7, where the absolute errors between AGRSF and RSKF, with different number of grid points, have been plotted for the same example. In that figure the values against AGRSF indicate the number of grid points used in a particular run, for example AGRSF-5 indicates that 5 grid points has been used. It can be shown from the figure that with the increases of grid points the error of the AGRSF reduces significantly and also estimation value converges to RSKF. 5 TRUTH KALMAN RSKF AGRSF

4

Value

3

2

1

0 0

5

10

15

20

Steps

Fig 6.6 Comparison of AGRSF and KF for linear Systems

110

Chapter 6

0.7 AGRSF-5 AGRSF-10 AGRSF-15 AGRSF-20

absolute error

0.6 0.5 0.4 0.3 0.2 0.1 0 0

5

10

15

20

steps

Fig 6.7 Convergence of AGRSF with number of grid points.

6.6.2

Case Study II Second Order Linear System In this section an example has been provided where the AGRSF has been applied to two-

dimensional linear system. The result matches exactly with the RSKF for sufficient number of point mass. That validates the proposed AGRSF method. Like single dimension system, the error settling characteristics of AGRSF with increase number of grid points has been studied. The superiority of RSKF in presence of plant perturbation or noise characteristics mismatch has not been studied here because that has been studied in detail in chapter 4. Here a two dimensional linear system has been considered as described in section 3.3 case (ii). From the system and observation matrix it is clear that the system’s observability is poor. Here the process and measurement noises are taken as zero mean and unity covariance with proper dimensions. The Truth has been initialized as xtruth = [0 0]T and filter has been initialized with x filte = [1 1]T and with unity error covariance matrix. Srinivasan [Srinivasan 2007] in his thesis shows the effectiveness of adaptive grid filtering (AGF) with (i) variable distance support points, (ii) number of support points and (iii) orientation of axes along the eigenvectors. The grid adaptation techniques proposed by Srinivasan [Srinivasan 2007] in the context of simple adaptive grid filtering (AGF) have been implemented here for AGRSF. As discussed above the variable grid points have been used. To generate variable grid (support) points, the following Matlab command has been used to generate gaps between successive grid points in any axis. Gaps(i)=Gaps(i)+((fix(i-(Npart1+1)/2)/Npart1)^2)*8;

111

Chapter 6

(a)

The gaps, computed from the left hand extreme to the right hand extreme, are thereafter

normalized to have a span of +/- 1. This is then multiplied by a factor m (default value 4) so that +/- m sigma values in each direction are covered. The area of the grid squares and their locations has been shown in figure 6.8. Z-axis has been plotted in log after dividing by the minimum area. Here number of grid point has been taken as 16 X 16. The value of factor has been chosen as 5.5. Figure 6.9 shows the posterior probability density typically reconstructed by the AGRSF in intermediate steps. The axes represent the distance from the assumed mean and normalized by the square root of respective eigenvalues of error covariance matrix. The weights are also not normalized. It should be pointed out here that as the problem is linear Gaussian the posterior pdf will be Gaussian and the figure 6.9 also shows the Gaussian pdf for posterior distribution function. That fairly matches with the theoretical consideration of AGRSF and hence again validates the proposed AGRSF filtering.

0.1

10

-0.1

10

-0.3

10

-0.5

10

1 1

0.5 0.5

0 0 -0.5

-0.5 -1

-1

StateVariable2

State Variable1

Fig 6.8 Plot of initial support points (16 X 16) and their respective area

Next to validate the AGRSF results the estimated value obtained from the filters has been plotted with the RSKF for both the states in figure 6.10 and figure 6.11. The results show that for a typical value µ1 =0.0005 the RSKF and AGRSF with 25 X 25 support points exactly matches with each other. This gives the confidence that the proposed AGRSF is correct and can be successfully implemented and with this confidence we will apply the AGRSF in non-linear cases in the next sections. The error settling property of the AGRSF has been studied. As the number of grid points increase the estimation error with RSKF decrease. The result obtained from this study is similar to that of obtained in 1 D for both the states hence the plot has not been shown here.

112

Chapter 6

current weight

8 7

probability density function

6 5 4 3 2 1 0 5 4 3 2 1 0 -1 -2 -3 -4 -5

-5

-1

-2

-3

-4

0

2nd state variable

1

2

3

4

1st state variable

Fig 6.9 Posterior probability density function at intermediate step

10 Truth RSKF AGRSF

estimated value

8

6

4

2

0 1

6

11

16

steps

Fig 6.10 Truth and estimated value of first state variable 2 Truth RSKF AGRSF25 0 estimated value

0

5

10

15

20

-2

-4

-6 Steps

Fig 6.11 Truth and estimated value of first state variable

113

5

Chapter 6

6.6.3

Case Study III First Order Nonlinear System

We consider the following single dimensional process and measurement sequence. The problem has been appeared in Ito et al [Ito 2000] then it is revisited with a different measurement equation in Sadhu et al [Sadhu 2007]. The problem has been described in section 3.4. We have considered the time from 0 to 0.8. The state of the system is estimated using the ERSF and AGRSF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. For AGRSF, number of grid points taken is 100. The truth and estimated value of state obtained from the abovementioned filters is shown in figure 6.12 for a representative run. It is noted that for this particular run the ERSF fails to track the truth while AGRSF track the truth well. RMS errors of AGRSF, ERSF have been compared using 1000 Monte Carlo runs in figure 6.13. It can be noted that the RMSE of ERSF is quite high where as the RMSE of AGRSF convergences to a much lower value. Here the computational time taken by the AGRSF has been studied. The results and comparison with risk sensitive particle filter has been presented in next chapter. To understand more, the probability densities of the state (as evaluated from AGRSF) for first 20 steps have been plotted in figure 6.14. It has been seen from the plot that although initially we started with Gaussian distribution due to nonlinear nature of process model from the next step it became a bimodal distribution with the modes are at +1 and –1 respectively. Initially the higher peak of pdf is more near the equilibrium point +1 but as time passes and more measurements are processed the mode as well as mean of the pdf gradually shifted towards –1 equilibrium point which is the truth value of the state. So it can be said that the filter is converging. In figure 6.15 probability density of states has been plotted when sufficient number grid point points have not been taken. In figure 6.15 pdf has been plotted for only 15 grid points. It can be noticed from the figure 6.14 and 6.15 as the number of grid points decrease the detail description of the probability density function (here bimodal nature of the pdf) has been lost.

114

Chapter 6

1.5

truth & estimate

1 truth AGRSF ERSF

0.5 0 0

20

40

60

80

-0.5 -1 -1.5

step Fig 6.12 Truth & Estimated value for ERSF, RSPF and AGRSF

1.2 1

AGRSF ERSF

RMSE

0.8 0.6 0.4 0.2 0

0

20

40

60

step

80

Fig 6.13 RMSE of ERSF and AGRSF for 1000 MC run

0.04

Probability density

0.03

0.02

0.01

0 5

4

3

2

1

0

-1

-2

-3

-4

-5

0

State variable

2

4

6

8

10

12

14

16

Step

Fig 6.14 Plot of probability density of estimated states with time

115

18

20

Chapter 6

1

Probability density

0.8

0.6

0.4

0.2

0 5

4

3

2

1

0

-1

-2

-3

-4

-5

0

2

State Variable

4

6

8

10

12

14

16

18

20

Step

Fig 6.15 Plot of probability density of estimated states with time for grid starvation case

6.6.4

Case Study IV Second Order Nonlinear System (BOT Problem)

6.6.4.1 Problem Description To demonstrate the multidimensional nonlinear estimation using adaptive grid risk sensitive filtering (AGRSF), the proposed filter has been applied to bearing only tracking (BOT) problem formulated in section 3.6. To determine the pdf of the transition between two points in state space the covariance matrix should be invertible. As process noise covariance in BOT problem described in section 3.6.1 is noninvertible a slight modification has been made in process noise covariance matrix. Interesting to note that the particle filter does not have this type of limitation as samples are drawn from the pdf. In original problem described in section 3.6.1 has the 0.5

0.25 0.5 × 0.01 . Here we used the process noise 1

covariance matrix Q = × [0.5 1]× 0.01 = 1 0.5

covariance matrix whose diagonals remains unchanged where as off diagonal terms are taken as zero. The physical implication is that we assume there is no cross correlation term in the process noise and obviously matrix becomes invertible. So our modified process noise covariance matrix 0.25 0 × 0.01 . 1

is Q = 0 6.6.4.2

Understanding the Intermediate Steps in Simulation

The AGRSF has been simulated using the algorithm described earlier. Here the process model is linear and measurement model is nonlinear. So linearised measurement noise model (same as used in EKF) has been used. This linearization is necessary for computation of likelihood and generation of proposal. EKF has been used as a proposal to capture the approximate posterior probability distribution. Eigenvector method described earlier has been

116

Chapter 6

used to orient the axes and scaling. One beautiful thing with AGRSF is that probability density function of prior, posterior, and likelihood can be visualised at each step. Here some useful insight has been gain on the AGRSF algorithm by visualising density functions in multidimensional case through this 2D BOT problem. It is necessary to mention that in this problem the estimates are very sensitive to risk sensitive parameter. Here the value of risk sensitive is taken as 0.000001.

(b)

Initialization The filter has been initialized with a Gaussian distribution with a mean and

error covariance. For a typical run we take mean

x0 = [67.48 0]T and covariance as

333.92 0 P0 = . 1 0

(c)

Proposal density obtained from EKF After the initialization or the end of the previous

step the probability density for the posterior distribution has been approximately generated from the EKF algorithm. The generated grid points from the proposal density have been subject to the time update step.

(d)

Time update

After generating the distribution of point mass from the proposals the

grid points are subject to time update step, which has been described earlier. In the first step after the time update probability density (50 X 50 support points, span factor of 5.0 sigma) of states has been shown in figure 6.16. It should be noted here that for the figures 6.16 the axes represent the distance from the assumed mean and normalized by the square root of respective eigenvalues of the posterior error covariance matrix as obtained from the proposal. The weights are also not normalized.

Fig 6.16 BOT point mass distribution after time update

117

Chapter 6

(e)

Likelihood

Figure 6.17 shows a typical likelihood function using 50 X 50 support

points and with span factor 5.0. The pattern in likelihood function indicates that the likelihood is only function of the position (first state variable) and not velocity. Here X axis is for position and Y axis is for velocity. The plot agrees with the measurement equation. The centre line of the fold is the position corresponding to the bearing measurement. The likelihood function is necessary for measurement update of the prior distribution to obtain posterior distribution.

Fig 6.17 Likelihood distribution 50 X 50 supports

(f)

Posterior distribution

When the prior probability distribution, shown in figure

6.16, has been updated with the likelihood function, shown in figure 6.17, we obtained the posterior distribution as shown in figure 6.18. So from the figure it can be noted that as the likelihood probability density is more on the mean value of position the point masses located near that region have been weighted more where as other pointed masses contain less weight.

Fig 6.18 posterior distribution with 50 X 50 supports

(g)

Effect of span factor

To illustrate the effect of span factor posterior distribution has

been plotted with the same number of particles but with a span factor (m) of 3 instead of 5. The

118

Chapter 6

effect of span factor can be realized by comparing the figure 6.18 and 6.19. It can be noted that there are significant distributions at the edges of the figure, indicating that the span of the generated point masses are not adequate to represent the whole probability density function hence the accuracy must be degraded. Due to this reason the span factor has been chosen as 5.0 for that problem.

Fig 6.19 Posterior distribution with 50 X 50 supports and 3 span factor

(h)

Effect of inadequate number of support points

To figure out the drawback

associated with the small number of support point, the posterior density function has been plotted for 20 X 20 number of support points in figure 6.20. The figure has been compared with the posterior probability density function with 50 X 50 support points (figure 6.18). As the number of support points has been decreased we immediately loose the details about the probability density function. This will be reflected to the final value of the estimation sequence leading to lack of accuracy in estimation.

Fig 6.20 Posterior distribution with 20 X 20 support points

119

Chapter 6

During the numerical experiment it was found that to obtain results with moderate accuracy and error convergence, at least 30 X 30 supports are necessary. The reason for the large number of supports has been investigated and the main reason identified to be the large ratio of the initial error covariance matrix P0 to process noise covariance Q. In this particular case the ratio of the eigenvalues was about 30,000 in worst case. This can be explained as follows. For a given number of supports, larger P0 enforces a larger gap between supports. On the other hand, if the gaps are not small compared to the square root of Q, the prior weights will become either close to zero or erratic. 6.6.4.3

Simulation Results

The BOT problem has been solved using proposed AGRSF and ERSF. This shows the applicability of the AGRSF in multidimensional non-linear scenario. Although the application of AGRSF in higher dimensional system is limited by the curse of dimensionality problem discussed before. To implement AGRSF for BOT problem 35 X 35 support points have been chosen in state space. A span factor of 5 has been assigned. The risk sensitive factor is taken as 0.000001. The performance of AGRSF has been compared with ERSF. For a typical single run the truth and the estimated value of position and velocity obtained from AGRSF and ERSF have been plotted in figure 6.21 and 6.22. Also the absolute errors for position and velocity estimate have been plotted in figure 6.23 and 6.24 respectively. The error settling property of AGRSF with the increase of grid number has been studied in figure 6.25 and 6.26. Similar to single dimensional linear problem the absolute errors for both position and velocity have been plotted with the number of support points. The number of grid points used for simulation has been mentioned in legend. Estimted value and truth

95

Position (m)

90 85 Truth ERSF AGRSF

80 75 70 65 60 0

5

10 step

15

Fig 6.21 Truth and estimated value of position

120

20

Chapter 6

From the figure 6.25 and 6.26 it can be noticed that with the increase number of grid the absolute error value decreases consistently. This is exactly with the concept of AGRSF. As the number of grid points in two-dimensional space increase the points can better represent the probability density and hence estimation of the state becomes more accurate. So with the increase of number of grid points the error in AGRSF converges. Estimated and truth value

1.6

velocity (m/s)

1.2 0.8 0.4 Truth ERSF

0

AGRSF

0

5

10

15

20

-0.4 steps Fig 6.22 Truth and estimated value of velocity Absolute error for position

16

position error

12

ERSF AGRSF

8

4

0 0

4

8

steps

12

Fig 6.23 Absolute error of position

121

16

20

Chapter 6

Absolute Error for Velocity

1.2 ERSF AGRSF

Velocity Error

0.9

0.6

0.3

0 0

5

10 Steps

15

20

Fig 6.24 Absolute error of velocity

AGRSF5*5 AGRSF10*1 0 AGRSF15*1 5 AGRSF20*2 0

60

40

20

0 0

5

10

15

steps

20

Fig 6.25 Convergence of position error with number of grid points AGRSF5*5

4 absolute error of velocity

Absolute error of position

80

AGRSF10*10 AGRSF15*15

3

AGRSF20*20 AGRSF25*25

2

1

0 0

5

10

15

20

steps

Fig 6.26 Convergence of velocity error with number of grid points

122

Chapter 6

6.7

Discussion and Conclusion (i)

This chapter introduces a new class of risk sensitive filtering based on grid adaptation mechanism. The method is named as adaptive grid risk sensitive filter (AGRSF). The main disadvantage is the computational cost. To utilise the grid points more efficient way and hence to minimise the computational time significant effort has been spent trying different schemes. All the schemes tried have been described in full detail in this chapter.

(ii)

In this chapter the results obtained from the AGRSF have been compared with RSKF for linear Gaussian 1D and 2D system. The exact matching between these two estimates for single as well as multidimensional case validates the formulation of AGRSF.

(iii)

Approximate knowledge of the posterior probability density function is necessary to run AGRSF. If the posterior density is approximated with Gaussian then rough knowledge of mean and covariance is necessary. In this work the EKF has been used to get rough idea about posterior mean and covariance. This is analogous to the use of “proposal density” in particle filter. Instead of EKF some other filters such as UKF, CDF and risk sensitive version of them (RSUKF, CDRSF) can also be used as proposal. It should also be kept in mind that the small error in proposal has no significant impact on over all result. Although the more accurate be the proposal lesser the number of grid necessary. So the impact of other filters mentioned earlier as proposal remains under the scope of future work.

(iv)

Span factor m may be considered as another tuning parameter of the AGRSF. Choosing a larger value of span factor m can cover any deficiency arises due to approximation of covariance. Choosing a larger value of the span factor m can also cover the error in approximating the estimate (of the mean) in proposal density function. Again with the increasing in span factor the particle density in state space dilutes. Here the default value of span factor has been chosen as 4. If the mean and covariance generated from proposal is far away from the real one then increase of span factor gives better results. In such cases we need to think for better proposal.

(v)

To reduce support points, axis alignment method has been used. The computational cost of the alignment procedure is negligible but through this method grid points can be efficiently utilised hence efficiency in estimation is increased. Here the alignment method has been demonstrated with the help of figures.

123

Chapter 6

(vi)

The proposed AGRSF has been applied to BOT problem to demonstrate the applicability of AGRSF in multi dimensional case. The better result than ERSF has been reported. The probability density functions of prior, posterior, and likelihood in an intermediate step have been illustrated with figures.

(vii)

It has been noticed that the number of grid point necessary is 10 times less in compare to risk sensitive particle filtering discussed in the next chapter. This is not surprising, because the fact that weighted supports are more informative. But the updation method of the grid points takes much time resulting over all less computational efficiency compare to particle filtering.

Strength of the AGRSF (i)

The AGRSF algorithm is simple straight forward easy to understand debug and implement. Unlike the closed form risk sensitive filters, here the probability density function can be visualised at each step. Here the mean of the density function has been considered as the estimate. Whether other statistical parameter of the distribution such as mode or median can be considered as the estimated value is under debate.

(ii)

The AGRSF can be considered as the test bench to determine the accuracy of other risk sensitive filters such as CDRSF, RSUKF and ERSF.

(iv)

In this chapter it has been shown that for nonlinear problem AGRSF gives better result than ERSF.

(v)

Unlike risk sensitive particle filtering sampling or resampling algorithm is not necessary.

(vi)

In the absence of any computing resource and time constraints, the method can provide estimation as accurate as desired.

Weakness of the AGRSF (i)

The main disadvantage of AGRSF algorithm is its computational time. Despite the different schemes proposed for grid adaptation the computation time is very high compare to ERSF, CDRSF and RSUKF. Thus at present it is not possible to implement the method for on board application. Rather AGRSF can be used as the benchmarking algorithm to compare the performance of onboard filter on offline basis.

(ii)

It is found out that the computation efficiency of AGRSF is even less than RSPF. However, with the linear transformation used for grid alignment the AGRSF

124

Chapter 6

becomes little faster. (iii)

Another weakness of AGRSF is curse of dimensionality problem. The grid point necessary to represent the probability density increases in power with the dimension of the system. So from computation cost point of view it is very difficult to implement AGRSF for very high dimensional problem. Again for the system when the ratio of P/Q is large, large numbers of support points are necessary to describe probability density function.

Scope for further improvement (i)

In the present work only EKF has been used for generating proposal. The use of other risk neutral filter such as UKF, CDRSF and also use of risk sensitive filters such as ERSF, RSUKF, CDRSF need to be explored.

(ii)

In the present work the span factor is considered as the constant over the time. The adaptation of scan factor with time can also be incorporated. Diagnostics like variance of weights may be used for this purpose.

(iii)

One can also think about the variable number of grid points as the filter evolves with time. To capture large error initially more number of grid points is necessary and as the time progress the filter gradually converges then the less number of particle may be sufficient to represent the probability density.

125

7 7.1

Risk Sensitive Particle Filter

Introduction

The concept of risk sensitive criterion is applied to linear as well as nonlinear problems and solved to get recursive risk sensitive estimators [Dey 2001, Boel 2002, Jaykumar 1998, Moore 1997, Collings 1994]. Previous to this work, for nonlinear risk sensitive filtering only EKF like approach was available [Jaykumar 1998]. As the filter has been derived from risk sensitive cost criterion, from the birth it is known for robustness compared to risk neutral filters. However, the limitations of the EKF, including smoothness requirement for the functions and noise Gaussianity restriction, are well known and hence, the extended risk sensitive filter (ERSF) fails to take care of non-Gaussian or severe nonlinear problems. To mitigate these shortcomings in an earlier chapter of this thesis linear regression Kalman filter based risk sensitive estimators namely central difference risk sensitive filter (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) have been proposed. The estimators are approximate one. For more accurate estimation adaptive grid based method has been formulated and described in the previous chapter. But the adaptive grid risk sensitive filtering (AGRSF) is computationally not efficient especially for high dimensional system due to curse of dimensionality problem. So we make an effort to develop computationally more efficient particle filter based risk sensitive estimation algorithm. This new kind of robust filter has been named as risk sensitive particle filter (RSPF). It is worthy to mention here that the name risk sensitive particle filter [Thrun 2002] appears earlier in the literature but in completely different context. In their work risk sensitive cost function is not minimized rather a special risk function has been proposed which measures how risky not tracking a special area in state space would be. Sampling density of ordinary particle has been modified so that more samples can be obtained in risky region. But the interpretation is completely different from risk sensitive estimation specially used in robust control context where one minimises the expected cumulative exponential estimation error. So from the risk sensitive estimation point of view no particle filter based risk sensitive estimator has been available in literature prior to this work although the name has been used in a completly different context. The method of risk sensitive particle filtering significantly extends the range of applications of risk sensitive techniques and can be used as a benchmark to quantify the performance of other approximate risk sensitive filters. The work has been presented in conferences as well as accepted for publication in journal publication [Sadhu 2005, Bhaumik 2007, Sadhu 2008] and the international community recognises that [Orgunar 2007, 2008].

Chapter 7

So in this chapter a particle filter based numerical method named as risk sensitive particle filtering (RSPF), which minimises risk sensitive cost criterion has been proposed for nonlinear non Gaussian system. Compared to AGRSF, which was described in previous chapter, in this formulation, no heuristic approach like grid adaptation is necessary and the proposed filter is computationally more efficient than AGRSF. In the process of formulation of risk sensitive particle filter both prior and posterior versions of the RSPF have been proposed. The proposed filtering technique has been applied to nonlinear Gaussian single dimensional problem as well as 2D bearing only tracking problem. Convergence of the proposed filtering technique has also been discussed through numerical examples. The comparison between AGRSF and RSPF has also been made. It has been realised that like particle filter the efficiency of posterior RSPF depends on choice of proposal. Optimal proposal for linear Gaussian measurement has been derived. For nonlinear Gaussian cases the linearised version of optimal proposal is also derived and implemented. The applicability of nonlinear risk sensitive filter such as ERSF [Jayakumar 1998], central difference risk sensitive filter (CDRSF) [Sadhu 2007] as a proposal for RSPF will also be discussed.

7.2

Formulation of Risk Sensitive Estimation Problem Though risk sensitive estimation problem formulation has already been described in earlier

chapters but it has been re-described here for the sake of completeness. Let us consider the following nonlinear additive non Gaussian evolution and observation equations xk +1 = f ( xk ) + wk

(7.1)

y k = h( xk ) + v k

(7.2)

where xk ∈ R n denotes the state and y k ∈ R p is the measurement at any instance k where k=0,1,2,3,…, nstep, wk ∈ R n , vk ∈ R p are additive, independent noises with known statistics. The

vectors f ( xk ) and h( xk ) are general (without any assumption of smoothness) nonlinear functions of x k and k . The generalized cost function for risk sensitive estimator at any instance k can be defined as [Boel 2002] k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp( µ1 ∑ ρ1 (φ ( xi ) − Φ 2 2 i k i =0

(7.3)

The constant parameters µ1 and µ 2 are called risk sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted

127

Chapter 7

ˆ , and by Φ i

ς

is a parameter. Functions ρ 1 (.) and ρ 2 (.) are both strictly convex, continuous and

bounded from below, attaining global minima at 0. The initial state x0 is independent of the noise processes mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by ˆ , is obtained by finding the µ 2 . The optimal estimate of φ at the current instant, denoted by Φ k optimum value of ς which minimizes J RS (ς , k ) , i.e. Φˆ k = arg min J RS (ς , k ) ς∈R

7.3

(7.4)

Prior Risk Sensitive Particle Filter Formulation

In this section a prior formulation of risk sensitive particle filter for nonlinear problem has been summarized.

7.3.1

Solution of Risk Sensitive Estimation Problem for Prior Estimation

In this section, the solution of the risk sensitive filtering problem has been provided for prior estimation. The predicted risk sensitive estimate, which has been referred to as the one stage delay estimate by some authors [Banavar 1998, Jaykumar 1998], provides the a priori estimate of states. It is straightforward to show that both the apriori and aposteriori estimators reduce to respective risk sensitive Kalman filter (RSKF) for the linear Gaussian case. The solution to the risk sensitive estimator (RSE) problem for a priori estimation may be obtained from the following recursive relation [Boel 2002] using an information state α k (x k ) and probability density functions p( x k +1 | x k ) and p( y k | x k ) .

(

(

))

ˆ α k (x k ) = ∫ p( xk | x k −1 )p( y k −1 | xk −1 ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k − 2 α k −1 ( x k −1 )dx k −1

(7.5)

Optimal estimate can be obtained from +∞

ˆ Φ k |k −1 = arg min ∫ exp(µ 2 ρ 2 (φ ( x k ) − ς ))α k ( x k )dx k ς ∈R

−∞

128

(7.6)

Chapter 7

7.3.2

Probabilistic Interpretation

The positive measures {α k (.)} are not normalized but we can normalize them to obtain probability α k (x k ) , where α k (x k ) is given by the equation (7.5). ∫ α k (x k )dx k

distributions {α k (.)} defined by α k (x k ) =

It is also convenient to introduce the following intermediate measures β k (x k ) =

(

(

))

Where β k (x k ) = p( y k | x k ) exp µ 1 ρ 1 φ ( x k ) − Φˆ k |k −1 α k (x k )

β k (x k ) ∫ β k (x k )dx k

(7.7)

Using such notation, we can rewrite the risk sensitive filter as follows α k (x k ) = ∫ p(x k | x k −1 )β k −1 (x k −1 )dx k −1

(7.8)

+∞

and Φˆ k |k −1 = arg min ∫ exp(µ 2 ρ 2 (φ ( x k ) − ς ))α k (x k )dx k ς ∈R

Where β k (x k ) =

(7.9)

−∞

(

(

)) ))

ˆ p( y k | x k ) exp µ 1 ρ 1 φ ( x k ) − Φ k | k −1 α k ( x k ) . ˆ ∫ p( y k | x k ) exp µ1 ρ 1 φ ( x k ) − Φ k |k −1 α k (x k )dx k

(

(

(7.10)

7.3.3 Particle Implementation •

Initialization; k=0 Sample X 0(i ) ~ p 0 (.) to obtain αˆ (x 0 ) =

(

)

1 N (i ) ∑ δ x0 − X 0 , N i =1

(7.11)

where N is the no of particles and i is the particle index, Set Φˆ 0 = arg min ∑ exp(µ 2 ρ 2 (φ (X 0(i ) ) − ς )) . N

ς ∈R

i =1

(7.12)

~

Sample X 0(i ) ~ q 0 (.) , where q 0 (.) is a proposal density, and compute

(

) (

(( ) ( )

)) ( )

~ ˆ p X~ (i ) p y 0 | X 0(i ) exp µ1 ρ1 φ X 0(i ) − Φ ~ 0 0 0 W0(i ) ∝ ~ (i ) q0 X 0 N

~

Normalize weights such that ∑ W 0(i ) = 1 . i =1

{~

~

Resample particles X 0(i ) , W0(i )

•

(7.13) (7.14)

}

At time k; k>0

(

~

)

Sample X k(i ) ~ p . | X k(i−)1 . 1 N It follows that αˆ k (x k ) = ∑ δ (x k − X k(i ) ) N

i =1

129

(7.15)

Chapter 7

Set Φˆ k |k −1 = arg min ∑ exp(µ 2 ρ 2 (φ (X k(i ) ) − ς )). N

~

(7.16)

i =1

ς ∈R

(

~

)

Sample X k(i ) ~ q k . | X k(i−)1 , where q k (.) is a proposal density,

(

) (

(( )

))

~ (i ) ~ (i ) ~ (i ) ~ (i ) ˆ ~ (i ) p( X k | X k −1 ) p y k | X k exp µ1 ρ1 φ X k − Φ k |k −1 Compute weight Wk ∝ . ~ (i ) ~ (i ) q k X k | X k −1 N

(

)

~

Normalize weights such that ∑ W k(i ) = 1 .

(7.18)

i =1

{~

~

(7.17)

}

Resample particles X k(i ) , Wk(i ) .

7.3.4 Optimal Proposal One of the key factors for efficient implementation of particle filter is choice of proposal successfully. The same statement is true for risk sensitive particle filter. In the algorithm described in previous section we need to select the importance distributions q0 ( x0 ) and q k (x k | x k −1 ) . It is straightforward to check that the distribution minimizing the conditional

variance of the weights is

(

(

ˆ q 0opt (x 0 ) ∝ p(x 0 ) p( y 0 | x 0 ) exp µ1 ρ1 φ (x 0 ) − Φ 0

))

at k=0

(7.19)

which gives an importance weight proportional to ~ W0(i )opt ∝ 1 .

(7.20)

At any instance k>0, the optimal importance distribution is

(

) (

(

ˆ q kopt (x k | x k −1 ) ∝ p (x k | x k −1 ) p y k | x k exp µ1 ρ1 φ (x k ) − Φ k |k −1

))

(7.21)

It is possible to sample from this optimal importance distribution in some special cases. In other situation when it is not possible to sample from optimal distribution, some approximate distribution is used.

Case 1 Computation of Optimal Proposal for RSPF with Nonlinear System and Linear Measurements For nonlinear system and linear measurement optimal expression for proposal can be derived. To do this let us consider the following system and measurement equation. x k +1 = f ( x k ) + w k , yk = Hxk + vk ,

wk ~ N (0, Qk )

(7.22)

v k ~ N (0, R k )

(7.23)

For this type of system optimal importance distribution can be obtained from the distribution qk

opt

( x k | x k −1 ) ~ N (m k , Σ k ) .

where Σ −k 1 = H T Rk −1 H + Qk −1 − 2µ1 I and

(7.24)

130

Chapter 7

−1

m k = Σ k ( H T R k y k + Qk

−1

ˆ f ( x k −1 ) − 2µ1 Φ k |k −1 )

(7.25)

Doucet [Doucet 2000] provided the expression for optimal proposal for ordinary particle filter. But the expression has not been derived in that paper [Doucet 2000]. Here we provide a derivation for optimal proposal for risk sensitive particle filter. For particle filter similar steps may be carried out to derive optimal proposal only the reader need to put zero in place of risk sensitive parameter.

(

(

))

To obtain optimal proposal the integration ∫ p(x k | x k −1 ) p (y k | x k )exp µ1 ρ1 φ (x k ) − Φˆ k |k −1 dx k need to be evaluated. But unfortunately the integration cannot be evaluated for all class of system and noise characteristics. The integration can only be evaluated for linear measurement system and Gaussian noise sequence. For square error type of kernel and Gaussian probability density function the above expression can be written as

(

) (

)

∫ p( x k | x k −1 ) p y k | x k exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 ) dx k T

or α ∫ exp[−0.5( xk − f ( xˆk −1 ))T Q −1 ( xk − f ( xˆk −1 )) − 0.5( yk − Hxk )T R −1 ( yk − Hxk ) + µ1 ( xk − xˆk |k −1 )T ( xk − xˆk |k −1 )]dxk or α exp[(0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk )T (0.5Q −1 + 0.5 H T R −1 H − µ1 I ) −1 (0.5H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ) − (0.5 f ( xk −1 )T Q −1 f ( xk −1 ) + 0.5 ykT R −1 yk − µ1 xˆkT xˆk ) × ∫ {exp[−( xk − (0.5Q −1 + 0.5 H T R −1H − µ1I ) −1 (0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ))T ) × (0.5Q −1 + 0.5 H T R −1H − µ1I )( xk − (0.5Q −1 + 0.5 H T R −1H − µ1I ) −1 (0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ))]}dxk

From the above expression it can be seen that the term under the integration is nothing but the Gaussian probability density function with mean m k and covariance Σ k . Where 0.5Σ −k 1 = (0.5Q −1 + 0.5H T R −1 H − µ1 I ) Or Σ k−1 = (Q −1 + H T R −1 H − 2µ1 I ) And m k = (0.5Q −1 + 0.5H T R −1 H − µ1 I ) −1 (0.5H T R −1 y k + 0.5Q −1 f ( x k −1 ) − µ1 xˆ k )

Or m k = Σ k ( H T R −1 y k + Q −1 f ( x k −1 ) − 2µ1 xˆ k ) So for this class of system the optimal proposal will be the Gaussian distribution with mean m k and covariance Σ k i.e N (m k , Σ k ) . The term outside the integration will act as the weight for the particle generated from the optimal distribution.

Case 2 Computation of Approximate Optimal Proposal with Nonlinear System and Measurements This example deals with a general system with nonlinear process and measurements. The system dynamics and measurement equation are given by equations (7.1) and (7.2) respectively, where, wk ~ N (0, Qk ) and v k ~ N (0, R k ) . For such nonlinear Gaussian case no optimal proposal

distribution exists. The optimal importance distribution [Doucet 2000] may be approximated

131

Chapter 7

using local linearisation. Let us called such proposal as linearised optimal proposal. To derive such a proposal let us consider the nonlinear Gaussian measurement equation. y k = h( x k ) + v k , where noise is Gaussian i.e v k ~ N (0, R k )

Above nonlinear measurement equation can be linearised similar to [Doucet 2000] y k ≈ h( f ( x k −1 )) +

∂h( x k ) | xk = f ( xk −1 ) ( x k − f ( x k −1 )) + v k ∂x k ∂h( x k ) ∂h( x k ) | xk = f ( xk −1 ) f ( x k −1 ) = | xk = f ( xk −1 ) x k + v k ∂x k ∂x k

or

y k − h( f ( x k −1 )) +

or

∂h( x k ) ~ yk = | xk = f ( xk −1 ) x k + v k ∂x k

∂h( x k ) where ~y k = y k − h( f ( x k −1 )) + | xk = f ( xk −1 ) f ( x k −1 ) ∂x k

For prior RSPF estimator, q kopt (x k | x k -1 ) ~ N (m k , Σ k ) , T

where Σ k

−1

= Qk

−1

∂h(x ) R −1 ∂h(x k ) k + − 2 µ1 I k ∂x k x = f ( x ) ∂x k xk = f ( xk −1 ) k k −1

,

T

∂h(x ) −1 R −1 ~ k ˆ mk = Σ k (Qk f ( xk −1 ) + k yk − 2 µ1 xk |k −1 ) ∂xk x = f ( x ) k k −1

7.4

Posterior Risk Sensitive Particle Filter Formulation

7.4.1

Solution of Risk Sensitive Estimation Problem for Posterior Estimation

Similar to section 7.3 the posterior solution of risk sensitive estimation for nonlinear case has been provided in this section. The solution for posterior can be obtained as

( (

))

(7.26)

ˆ = arg min ∫ exp(µ ρ (φ ( x ) − ς ))α (x )dx Φ k |k 2 2 k k k k

(7.27)

ˆ αk (xk ) = p( yk | xk )∫ p(xk | xk −1) exp µ1ρ1 φ(xk −1) − Φ k −1|k −1 αk −1(xk −1 )dxk −1

and the optimal estimate is +∞

ς ∈R

−∞

The relation has been derived by the present author and has been presented in a form of theorem in chapter [4].

7.4.2 Probabilistic Interpretation Similar to prior RSPF probability interpretation of posterior RSPF has been given in this chapter.

132

Chapter 7

We normalize the probability distribution of {α k (.)} to obtain probability distributions {α k (.)} defined by α k (x k ) =

α k (x k ) . ∫ α k ( x k )dx k

Let us introduce another probability density function

(

(

))

β k −1 ( x k −1 ) = p( y k | x k ) exp µ 1 ρ 1 φ ( x k −1 ) − Φˆ k −1|k −1 α k −1 ( x k −1 ) The normalised version of that probability

density function can be obtained as β k −1 (x k −1 ) =

{ }

The normalised probability density α k

β k −1 (x k −1 )

∫ β k −1 (x k −1 )dx k −1

can be written using newly introduced probability

density function β k as follows: +∞

α k ( xk ) =

( ( | x ) exp(µ ρ (φ ( x

)) ))α

ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 p ( x k | x k −1 )dx k −1

−∞ +∞ +∞

∫ ∫ p( y k

− ∞ −∞

k

1

1

k −1

ˆ )−Φ k −1|k −1

k −1

p( x k | x k −1 )dx k −1 dx k

After some simple manipulation we get α k ( x k ) = ∫ p( x k | x k −1 ) β

k −1

(7.28)

dx k −1

Where β k −1 (x k −1 ) =

(

k

=

(

(

))

ˆ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 ( x k −1 ) ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 (x k −1 )dx k −1 ˆ α (x ) p( y | x ) exp µ ρ φ ( x ) − Φ k

(

(

(

1

(

1

(

k −1

k −1|k −1

ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1

(

)) )) ))α

(

k −1

k −1

k −1

(x k −1 )dx k −1

))

ˆ β k −1 (x k −1 ) ∝ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 ( x k −1 )

(7.29)

7.4.3 Particle Implementation •

Initialization; k=0 Sample X 0(i ) ~ p 0 (.) to obtain w0i =

•

(

1 N (i ) ∑ δ x0 − X 0 N i =1

)

At time k; k>0

(

~

~

Sample X k(i ) ~ q k . | X k(i−)1

)

Assigned weight i

wk ∝

(

)

(

(

~ ~ ~ ~ ˆ p y k | X k(i ) p ( X k(i ) | X k(i−)1 ) exp µ1 ρ 1 φ ( X k(i−)1 ) − Φ k −1|k −1 ~ i ~ (i ) q k ( X k | X k −1 )

Normalise weight

133

))

Chapter 7

Resample { X ki , wki } N Estimate Φˆ k |k = arg ζ min ∑ exp(µ 2 ρ 2 (φ ( x k ) − ς )) i =1

7.4.4 Optimal Proposal Case 1 Computation of Optimal Proposal for RSPF with Nonlinear System and Linear measurements Optimal proposal for posterior RSPF in Gaussian noise linear measurement situation can be derived following the same steps as done in section 7.3.4. In that situation proposal density function will be q k opt ( x k | x k −1 ) ~ N (m k , Σ k ) . Where Σ −k 1 = H T Rk −1 H + Qk −1

(7.30)

and m k = Σ k ( H T Rk −1 y k + Qk −1 f ( x k −1 ))

(7.31)

And the weights associated with each particle can be given by

(

(

))

ˆ wki ~ exp(−0.5( y k − Hf ( x k −1 )) T ( HQk H T + Rk ) −1 ( y k − Hf ( x k −1 )) + µ1 ρ 1 φ ( x k −1 ) − Φ k −1|k −1 )

It should be noted here that for prior formulation of risk sensitive particle filtering the risk sensitive parameter appears in mean and covariance of the proposal distribution as well as in expression for weight. But in posterior formulation risk sensitive parameter appears only in calculation of weight not in proposal density function.

Case 2 Computation of Approximate Optimal Proposal with Nonlinear System and Measurements The system dynamics and measurement equation for nonlinear Gaussian system are given by equations (7.1) and (7.2) respectively, where, wk ~ N (0, Q k ) and v k ~ N (0, R k ) . For such nonlinear Gaussian case no optimal proposal distribution exists as discussed in prior RSPF. The linearised optimal proposal may be approximately obtained by following the steps [Doucet 2000] described in 7.3.4. Doing similar mathematical manipulation as done in prior RSPF we get ∂h( x k ) ~ yk = | xk = f ( xk −1 ) x k + v k ∂x k

Now qk

opt

the

linearised

optimal

importance

distribution

( x k | x k −1 ) ~ N (m k , Σ k ) T

Where

Σk

−1

∂h(x ) −1 ∂h( xk ) k = Q + Rk ∂xk x = f ( x ) ∂xk x k = f ( xk −1 ) k k −1

m k = ∑ k (Q k

−1 k

−1

f (x k −1 ) + (

∂h( x k ) −1 | xk = f ( xk −1 ) ) T R k ~ yk ) ∂x k

The weights associated with each particle can be assigned as

134

can

be

computed

as

Chapter 7

∂h(x k ) T ∂h( x k ) wki ~ exp(−0.5( ~ y k − h( f (x k −1 ))) ( | xk = f ( xk −1 ) ×Qk ( | xk = f ( xk −1 ) ) T + Rk ) −1 ( ~ y k − h( f (x k −1 ))) ∂x k ∂x k ˆ + µ ρ φ(x ) − Φ )

(

7.5

1

1

(

k −1

k −1|k −1

))

Notes •

In this chapter prior and posterior risk sensitive particle filter have been formulated. Taking the value of risk sensitive factor as zero, ordinary prior particle filter can be formulated which is not very common in particle filtering literature. That type of filter may become useful for real time system with delayed measurement.

•

If we take the value of µ1 =0 in RSPF, all the expressions reduce to simple particle filter equations.

•

It should be noted that in prior RSPF the risk sensitive parameter appears in the expression of mean and covariance of optimal importance distribution (which is Gaussian) and also in weights where as in posterior RSPF the risk sensitive parameter appears in expression of weights only.

•

In particle filter we know that the efficiency of particle filtering depends on successful choice of proposal. Similar statement is true for risk sensitive particle filter. In this chapter the optimal proposal and linearised optimal proposal have been derived. Other proposals such as EKF, UKF, applicable to particle filter can also be applied to RSPF.

•

The use of other risk sensitive nonlinear filters such as extended risk sensitive filter (ERSF), Central difference risk sensitive filter (CDRSF) as a proposal of RSPF may be more appropriate instead of ordinary risk neutral filters. Further investigation is necessary in this direction.

•

The main draw back in particle filtering is its computational burden. It has been noticed that approximately 10 times of particle is necessary comparative to grid points for equivalent results obtained from AGRSF. RSPF requires random sampling and resampling method, which is not necessary for AGRSF. Further grid adaptation policy is necessary for AGRSF, which is not necessary for RSPF. However it is observed that RSPF is computationally more faster that AGRSF to obtain the same accuracy.

•

In the next section we will experimentally investigate about the convergence property of RSPF. Our investigation reveals that the risk sensitive particle filter converges quickly towards the true solution for linear Gaussian models. Most rigorous derivation of convergence is necessary. This remains under the scope of future work.

135

Chapter 7

•

To obtain the estimated value of the convex function, searching is necessary among the particles in state space as described in equation (7.6). To obtain or to search the appropriate value for states all the mechanisms tried in AGRSF have also been tried for RSPF. However it should be noted that computational burden increases heavily due to search mechanism. It can be replaced by assuming normal probability density function and hence finding out the mean and variance.

•

Throughout the formulation in this chapter the estimate of any function of state ( φ ( x k −1 ) ) and for risk sensitive cost function generalized convex functions ( ρ 1 (.) and ρ 2 (.) ) have been considered. If the variables to be estimated are the state variables themselves ( Φ ( x ) = xˆ ), and both the convex functions ρ 1 (.) and ρ 2 (.) are known quadratic functions of the error then ρ j (α ) = α 1 T α 1 where

α1

is the error in estimation of state

variables. In the following section where case studies has been considered the quadratic cost function has been assumed and values of the states have been estimated.

7.6

Case Study

Like AGRSF, first RSPF has been applied to single dimensional linear Gaussian case. The estimated value obtained from RSPF compared with risk sensitive Kalman filter (RSKF). The exact matching between RSKF and RSPF for linear Gaussian case validates the risk sensitive particle filter formulation. In this section RSPF has been applied to a single dimensional nonlinear problem as well as BOT problem. The results and different issues will be discussed.

7.6.1

1D Nonlinear Case

We consider the one-dimensional process and measurement sequence described in section 3.4. We have considered the time from 0 to 0.8 sec. The state of the system is estimated using the ERSF and AGRSF and newly proposed RSPF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. For AGRSF, number of grid points taken is 100. For RSPF 1000 number of particle has been used. The truth and estimated value of state obtained from the above mentioned filters is shown in figure 7.1 for a representative run. It is noted that for this particular run the ERSF fails to track the truth while AGRSF and RSPF track the truth well. It can also be noticed that for this particular problem the error settling property for RSPF is found to be better than AGRSF. RMS errors of AGRSF, ERSF have been compared using 1000 Monte Carlo runs in figure 7.2. It can be noted that the RMSE of ERSF is quite high where as the RMSE of AGRSF convergences to a much lower value. The convergence rate of RSPF is higher than AGRSF.

136

Chapter 7

1.5 1

truth & estimate

truth AGRSF

0.5

RSPF ERSF

0 0

20

40

60

80

-0.5 -1 -1.5

step

Fig 7.1 Truth & estimated value of state obtained from ERSF, RSPF and AGRSF 1.2 AGRSF RSPF

1

ERSF

RMSE

0.8 0.6 0.4 0.2 0 0

20

40 Step

60

80

Fig 7.2 RMSE of ERSF, RSPF and AGRSF for 1000 MC run

Study of Computational Load Computational load for AGRSF and RSPF has been studied and compared. Although it is extremely difficult to compare the computational load between these two numerical risk sensitive filters, because different number of grid points and particles are necessary to obtain satisfactory and consistent results. It has been observed for RSPF approximately 10 times of particle is necessary than that of AGRSF method. The time for a single run of AGRSF (with different number of grid points) and the time for a single run of RSPF (for different number of particles) in Matlab software in a P-IV (2.66 GHz) processor is summarised in table 7.1. From the table it is seen that that the computational efficiency of RSPF is at least 4 times better than AGRSF.

137

Chapter 7

Table 7.1: Time Calculation for a single run of RSPF and AGRSF for different number of grid point and particle

AGRSF No of grid 100 200 400 7.6.2

Run time (in sec) 80.375 312.407 1265.3

RSPF No of particle 1000 2000 5000

Runtime (in sec) 21.06 56.703 252.844

2D Bearing Only Target Tracking In this section, the risk sensitive particle filter (RSPF) has been applied to the second

order bearing only tracking test problem describe in section 3.6.1. The measurement model has been taken as discussed in section 3.6.2 (measurement model I). The filter has been initialised as discussed in section 3.6.5.

Sensitivity With Risk Sensitive Parameter In this study RMSE for both position and velocity has been calculated for different risk sensitive parameter using 1000 MC run. It is observed that the estimation is very sensitive with risk sensitive parameter. For example for risk sensitive parameter µ1 =0.001, the RMSE of position diverges (the case has not been shown in position RMSE error plot). It can be seen from the simulation results that for larger µ1 the RMSE for both position and velocity is higher than PF as risk sensitive parameter decreases to zero RSPF result matches exactly with particle filter (Fig 7.3 and Fig 7.4). It has been seen earlier for BOT problem if the initialisation error for position is large EKF diverges but PF does not. Similar to that for large initial position error ERSF diverges but RSPF does not for the same risk sensitive factor. RSPF for different risk sensitive parameter 25

mu=0 mu=0.0001

20

RMSE (position)

mu=0.000001

15

10

5

0 0

5

10

step

15

20

25

Fig 7.3 RMSE of position for different risk sensitive parameter

138

Chapter 7

RSPF for different risk sensitive parameter 1.2 mu0

1

mu=0.001

RMSE (velocity)

mu=0.0001 mu=0.000001

0.8 0.6 0.4 0.2 0 0

5

10

step

15

20

25

Fig 7.4 RMSE of velocity for different risk sensitive parameter

Another experiment on BOT problem has been done where the value of sampling time is perturbed. The nominal value of sampling time has been taken as one second and it has been perturbed to +-0.5 second for the truth model where as the filter model assume one second sampling time. RSPF has been simulated in that situation. Experiment has been done with different risk sensitive parameter. Comparison of terminal RMSE for position and velocity has been made. From the result the superiority of RSPF over PF cannot be claimed for this situation.

7.7

Convergence Study

Experiment 1: 1D Nonlinear System and Nonlinear Measurements The system taken for this experiment is described in section 3.5. Risk sensitive parameters are taken as 0.2 ( µ1 = µ 2 =0.2). Fig 7.5 shows that the variance of the error across Monte Carlo (MC) runs converges to zero as the number of particles is increased. This indicates that for this example, the risk sensitive particle filter converges as the number of particles is increased.

Experiment 2: 2D BOT Problem The RSPF has been simulated for 2D BOT problem described in section 3.6 using different number of particle. RMSE for 1000 run with different number of particle has been shown in fig 7.6 and 7.7. The value of risk sensitive parameter has been chosen as 0.00001. From the figure it can be seen that as the number of particle increases the RMSE for both position and velocity decreases. As the number of particle increased the error between two consecutive run (for example run with 400 particle and 500 particle) decreases. It indicates RSPF is converging.

139

Chapter 7

Again for BOT problem variance of error for position across several Monte Carlo runs at a particular time step, (i.e. 18 sec), versus the number of particles has been plotted in fig 7.8. It is seen from the figure that RSPF converges with increasing number of particles for this nonlinear application example. The variance of error for velocity also shows a very similar trend.

Variance of Error across MC Runs

0.09 0.08 0.07 0.06 0.05 0.04 0.03 0.02 0.01 0 10

100

1000

10000

No. of particles

Fig 7.5 Variance of error across MC runs vs no of particles for 1D nonlinear system RSPF for different number of particle

25

N=100 N=200 N=300 N=400 N=500

20

RMSE

15

10

5

0 0

5

10

step

15

20

Fig 7.6 RMSE of position for different particle

140

25

Chapter 7

RSPF for different numbe of particle

1.2 N=100

RMSE(velocity)

1

N=200 N=300

0.8

N=400 N=500

0.6 0.4 0.2 0 0

5

10

step

15

20

25

Fig 7.7 RMSE of velocity for different particle

7.8

Scope for Further Work (i)

It is well known and discussed above that the estimation accuracy of RSPF depends on the choice of proposal similar to ordinary particle filter. For PF it has been proven that using other nonlinear filter such as extended Kalman filter, unscented Kalman filter (UKF), the estimation accuracy can be increased in comparison to prior proposal. Similarly using other risk sensitive filter such as ERSF, RSUKF, and CDRSF as proposal for RSPF the accuracy may be increased. The possibility need to be explored more.

(ii)

In this chapter convergence of RSPF has been proved numerically using simulation results. But this is not a rigorous way to prove the convergence of RSPF. So in future the convergence of RSPF need to be proved in a more rigorous mathematical way. In this regards Crisan’s work [Crisan 2002] may be helpful.

(iii)

In this chapter developed RSPF has been applied to real time situation in a simple BOT problem. But no improvement can be claimed in comparison to PF even for the situation when truth model differs from filter model. So it is necessary to find out the situations under which RSPF behave significantly better than PF. This can be achieved through proper understanding of properties of RSPF.

141

Variance of Error for position across MC runs (m)

Chapter 7

3 2.5 2 1.5 1 0.5 0 50

150

250

350

450

550

650

750

850

No. of Particles

Fig 7.8 Variance of error for position vs no. of particles in 2D BOT problem

7.9

Discussion and Conclusion Particle filter based solutions of the risk sensitive estimation problem for nonlinear non

Gaussian systems has been developed in the present chapter. Both the prior and posterior formulation has been discussed. The developed filter is expected to be robust with respect to model uncertainty. As for standard particle filters, it is important in practice to use or approximate the optimal importance distribution to obtain good performance. Here optimal proposal distribution has been derived for a Gaussian system with linear measurement. Applicability of different type risk sensitive filter as a proposal has been discussed. Experimentally, our investigation reveals that the risk sensitive particle filter converges quickly towards the true solution. Rigorous mathematical proof of convergence remains under scope of future work. Computational load is obviously a key concern for considering RSPF in real time scenario. In the earlier chapter AGRSF has been formulated. In this chapter comparison is made with RSPF in terms of computational efficiency and accuracy of estimation. It has been found that RSPF remains numerically superior although a larger number of particles compared to the number of grid points has to be used to achieve the same accuracy.

142

8

Cramer Rao Lower Bound and Preliminary Design for Target Tracking Systems

8.1

Introduction Cramer Rao Lower bound (CRLB) for parameter estimation provides the minimum

variance of estimation error that is achievable by any unbiased filter and the theory for the same had been around for more than half a century. According to [Ristic 2004a], the Cramer Rao inequality was first stated by Ronald Fisher (1925) later reformulated simultaneous by C R Rao and Harold Cramer in 1945. Although the bound parametric estimation evolved nearly fifty years ago, assigning bounds for state estimation of nonlinear multivariable dynamic system has a fairly recent origin, mainly due to the work of Taylor [Taylor1979] and Tichavsky [Tichavsky1998]. The bound developed by Taylor for state estimation does not have mathematical rigour what now a days available with CRLB. But that has started enjoying fairly wide patronage [Kerr 1989, Doerschuk 1995] among target tracking practitioners. Though some authors have reservations, we would call the techniques for providing lower bounds for state estimation of nonlinear multivariable dynamic system by the generic name CRLB. As for linear estimation problem Kalman filter gives the optimal solution, CRLB coincides with the error covariance calculated from Kalman filter algorithm and its use is limited. For nonlinear problems, the CRLB gives lower bound of the error covariance. As of now no general optimal filtering method is available for nonlinear system. So no matter what filtering technique we use, CRLB will provide the lowest error covariance. We can compare the different nonlinear filtering technique about to what extent it is close to CRLB. So the bound acts as a benchmark to demonstrate efficiency of a specific filter [Ristic 2003a]. In this work we extend its use to answer questions like whether a set of projected filtering specification is realistic with the given kinematic scenario and tracker instrumentation /signal processing performance restrictions, before detailed design effort is undertaken. We demonstrate further, how the technique also permits a design trade off analysis with performance improvement versus cost of improved instrumentation and signal processing equipment. The present worker had possibly been the first [Bhaumik 2006 c] to carry out a systematic parametric analysis of the Crammer Rao bound for tracking systems and to point out the potential use of CRLB for preliminary system design of tracking filters. This section is primarily based on the above publication. For additive noise models simple algorithms have been published for CRLB computation [Tichavsky 1998]. Noting that such popular algorithms for CRLB computation require either a

Chapter 8

non singular [Ristic 2003a] process noise covariance matrix ( Q ) or assumes it to be a null matrix [Taylor 1979]. Simple expression provided by the present author in [Bhaumik 2006c] can be used to compute CRLB for linear process in presence of singular Q matrix. In Srinivasan’s thesis [Srinivasan 2007] informal formulation of the CRLB recursive equation has been done which can handle singular Q matrix for both linear as well nonlinear processes. The reformulation also provides a deeper insight into the variation of error with respect to several parameters. In the tracking scenario the seeker usually provides the line of sight (LOS) angle and the LOS rate with respect to the interceptor body frame or inertial frame for guidance [Oshman 2003, Bar Shalom 1993]. When the target tracking is made using LOS only the tracking is known as bearing only tracking (BOT). It has become an important field of study and has attracted the attention of many workers [Sadhu 2006, Lin 2002, Bar Shalom 1993]. The primary motivation for studying BOT is it’s practical military and civil applications including under water weapon systems, Infra Red seeker based tracking, sonar based robotic navigation and TV camera or stereo microphone based people tracking. For weapon system guidance, BOT allows use of passive trackers [Chan 2002] with the consequent tactical advantages. No less important is the fact that BOT problems have nonlinearity and poor observability [Song 1996], which generally stresses the tracking filters. Detailed literature available on this has been reviewed in chapter 2. Use of CRLB was initially restricted to underwater weapon system designers [Kerr 1989] but of late, its increasing popularity among aerospace system designers [Ristic 2003a, Chan 2002] had become noticeable. One possible reason could be the availability of newer sensors and newer filtering techniques like particle filter and sigma point Kalman filter [Sadhu 2006] and renewed interest in homing interceptors against tactical ballistic missiles using bearing only sensors [Ristic 2003b]. Yesteryears extended Kalman Filter was probably the only choice available for nonlinear tracking problems. Now a days a variety of filters [Ristic 2003a] like unscented Kalman filter and particle filter are available. So choice of filter becomes as important as the choice of guidance laws. The configuration selection has to be carried out at an early phase of the design life cycle of the interceptor, when even the airframe and propulsion system of the interceptor may still be evolving, subsystem data may be only partially available. The design/selection toolset for this phase should have been entirely different from the detailed design and validation tools. In reality, a team either depends on the intuitive feel of an experienced designer or goes for a detailed analysis. For example, the effects of faster sampling, lower measurement noise etc. on filtering

144

Chapter 8

performance are known qualitatively, for specific nonlinear problems, it is difficult to estimate even “ball park” quantitative results without extensive Monte Carlo simulation. Proper Monte Carlo analysis with 6 DOF simulation mode, need far more design details than available in the configuration selection phase. We therefore advocate taking a middle road between the “gut feelings” of a veteran and a detailed MC study. Use of CRLB fits our requirement admirably. The CRLB gives an indication of the achievable lower bound of the error covariance and acts as a benchmark to demonstrate efficiency of a specific filter [Ristic 2003a], especially so, as the filters for nonlinear problems are suboptimal. CRB also permits design trade off analysis with performance improvement versus cost of improved instrumentation and signal processing equipment [Taylor 1979]. We demonstrate the power of CRLB analysis with the help of a bearing only tracking (BOT) problem. Results of earlier parametric studies sometimes indicated conflicting trends across different families of filters. In the present chapter, instead of using individual filtering techniques we use the generic Cramer Rao Lower Bound (CRLB) [Taylor 1979, Sadhu 2006] to study the effect of (i) sampling time (ii) measurement error covariance (iii) process excitation covariance (iv) initial error covariance (v) additional measurement (vi) eclipsing effect on state estimation. The study has already been in published [Bhaumik 2006b, Sadhu 2004a]. 8.2

CRLB Computation In this section the algorithm used to calculate CRLB has been demonstrated. The version that

has been stated here is the posterior version of CRLB called as posterior Cramer Rao lower bound or PCRLB [Tichavsky1998]. Let us consider nonlinear system and measurements with additive gaussian (process as well as measurement) noise described by: x k +1 = f k ( x k ) + wk

(8.1)

z k = h( x k ) + v k

(8.2)

Let Qk and Rk be the covariances of zero-mean noises wk , and v k respectively. Given a state estimation problem, the lower bound of the RMS error (LBRE) of individual state are given by the square root of the corresponding diagonal elements of Fisher information matrix J k . ( LBRE ) {x j (k )} =

J −jj1,k

(8.3) The minimum variance covariance matrix Pmv (often referred as the CRB matrix) is related to the information matrix as Pmv ,k = J k −1 .

145

Chapter 8

The CRLB computation becomes simplified if the state equation is linear, noise is Gaussian, additive and the output variable is a scalar. Applying these simplifying assumptions on the algorithm proposed in [Tichavsky 1998], the information matrix may be recursively computed [Tichavsky 1998] as follows: J 0 = P0

−1

J k +1 = Dk

(8.4)

22

− Dk [ J k + Dk ]−1 Dk 21

11

12

(8.5)

The expressions D ij are explained below. Dk

11

Dk

12

Dk

21

Dk

22

−1 T = − E{Fk }Q k ; 12 T = [ Dk ] −1 −1 = Q k + E{[∇ xk +1 hk +1 ( x k +1 )]R k +1 [∇ xk +1 hk +1 ( x k +1 )]T } T

−1

= E{Fk Q k Fk };

(8.6)

Where Fk = [∇ xk f k T (x k )]T and H k +1 = [∇ xk +1 hk +1T (x k +1 )]T are evaluated at the true state, where E{}is the

exception

operator

(to

be

determined

by

Monte

Carlo

simulation)

and

∇x = [

∂ ∂ ∂ T , , ..., ] . For linear plant model the process model will be xk +1 = Fk xk + wk . In this ∂x1 ∂x2 ∂xn

case the expression D ij will be 11

T

−1

Dk = E{Fk Qk Fk } ; Dk

12

T

−1

= − E{Fk }Qk ;

(8.7)

For linear measurement model the measurement equation will be z k = H k x k + v k and D ij will be Dk

22

8.3

−1

T

−1

= Qk + E{H k +1 Rk +1 H k +1}

(8.8)

Parametric Studies of BOT Problem Using CRLB In this section CRLB has been applied to BOT problem described in section 3.6.

Variation of CRLB with different model parameter has been observed. The first difficulty in implementation of CRLB to BOT problem arises due to singular Q matrix because to evaluate equation 8.6, Q matrix needs to be invertible. This however, does not imply that CRLB is undefined for singular Q matrix. For this BOT problem initially the Q matrix was perturbed by infinitesimal small value from singularity (typical value 0.001 in diagonal terms) so that it becomes invertible and apply the equation 8.6. The formulation by Tichavsky [Tichavsky 1998], does mention this problem but provides a very round about way of tackling it. Later the problem

146

Chapter 8

was addressed by the present author in [Bhaumik 2006c]. CRLB with a null Q matrix may be computed [Taylor 1979] from the relations −1

J k +1 = ( F −1 )T J k F −1 + E ( H k +1 Rk +1 H k +1 ) T

(8.9)

The difficulties of singular Q matrix may be avoided by reformulating eqn (8.9). We use matrix inversion lemma [Bar Shalom 1993] and the fact that the state matrix is linear. It may be noted that [Ristic 2003b] mentions a similar formula but there are obvious errors in their version. The recursion formula for the information matrix may be reformulated as −1

−1

J k +1 = (Qk + Fk J k Fk ) −1 + ( H k +1 Rk +1 H k +1 ) T

T

(8.10)

For the initial covariance matrix P0 , the difference between CRLB approach and the other filters should be noted. In the present BOT problem, the truth model of the initial condition is deterministic. As the truth is unknown or partially known, for specific filter a designer is free to choose a suitable P0 . However, the CRLB formulation assumes the problem initial uncertainty to be solely defined by the initial covariance matrix P0 . For the initial position uncertainty, this does not pose a problem. But for the initial velocity uncertainty, the specific filters cited here have some advantage (over the CRLB) as prior knowledge has been used. This would be reflected in the simulation results. 8.3.1

Nominal Performance Comparison

The LBRE results obtained from Monte Carlo run, when different simulation parameters (measurement error covariance, initial error covariance, process noise covariance and sample time) are in nominal condition, are shown in fig 8.1 and 8.2. The nominal condition of these parameters means the simulation has been carried out with the values mentioned in section 3.6. The corresponding RMS error for EKF [Sadhu 2006, Lin 2002] and Particle filter [Lin 2002] has been plotted alongside. No risk sensitive filter has been compared with CRLB because no lower bound for risk sensitive filtering is available presently. 8.3.2

Influence of Sampling Time on CRB

One important design issue in tracking system is the sampling time (inverse of sampling frequency). A faster sampling rate generally involves faster computation and possibly costlier computers. Faster sampling time may have other consequences like it may increase measurement noise, specially for radar based systems, where higher integration time (inter sample time) reduces noises. In this section we do not consider such inter coupling issues and wish to

147

Chapter 8

investigate the effect of sampling time on the CRLB performance while other parameters remain the same.

LBRE and RMSE of position (m)

30

LBRE PF

25

ekf

20 15 10 5 0 0

5

10

15

20

25

Time (sec)

LBRE and RMSE of velocity(m/s)

Fig 8.1 Comparison RMS error performance of EKF and PF with LBRE of position at nominal condition of Rk , P22,0 , Q and T

1.2

LBRE PF

1

ekf

0.8 0.6 0.4 0.2 0 0

5

10

15

20

25

Time (sec)

Fig. 8.2 Comparison of RMS error performance EKF and PF with LBRE of velocity at nominal condition of Rk , P22,0 , Q and T

With the assumptions as above, it is reasonable to expect that CRLB and the LBRE would decrease with the decrease of sampling time. However, whether such expectations are justified and to what quantitative extent need to be investigated for specific cases. From Figs 8.3 and 8.4, where the LBRE is calculated and plotted for both position and velocity for different sampling times (0.2s, 0.5s, 0.8s and 1s) it is seen that a lower sampling time indeed noticeably decreases the CRLB for both position and velocity. More specifically, while for the nominal sampling time (T=1 sec), the time required for the position LBRE to settle to 5 m (about

148

Chapter 8

one fifth of the initial) is about 9 sec, the corresponding time for T=0.5 sec and T=0.2 sec are respectively 5.5 sec and 2.8 sec. For the position error case the beneficial effects of reduced sampling time is more pronounced in initial error settling and the differences taper out as the filter converges. For the velocity LBRE, the time required to settle to 0.6 m/s are respectively 14s (for T=1s), 11.5s (for T=0.5s), 9.2s (for T=0.2s). It may be seen that for velocity error, the effect of faster sampling is not visible up to about first 3 seconds, whereas, at the later phase the effects become substantial. It may be noted that the above effects could not have been predicted without the CRLB analysis. 30

T=0.2s T=0.5s T=0.8s T=1s

LBRE of position (m)

25 20 15 10 5 0 0

5

10

15

20

25

Time (sec)

Fig. 8.3 Influence of the sampling time on LBRE of position

8.3.3

Influence of Measurement Error Covariance

The measurement error covariance indicates basically the quality of the sensor and its allied electronics. This is therefore of fundamental importance. A sensor with a lower error covariance is generally costlier. Therefore a trade off may be necessary. The effect of substantial changes in measurement error covariance is evident from eqn (8.6). With increase in R k , the terms of matrix J k +1 decreases, thus increasing the LBRE of position and velocity. The reverse is true with decrease in R k . Figs 8.5 and 8.6 show that increase in measurement error covariance causes poorer settling of position error. When the measurement error covariance is 1/5th of the nominal value, the terminal position error reduces to 0.48 m compared to 1.01 m in the nominal case. Velocity error reduces to 0.2 m/s compared to 0.29 m/s in the nominal case at 20s.

149

Chapter 8

In the sampling time studies in the previous section, the measurement error covariance has been kept constant. However, the measurement error covariance may be coupled with sampling time for some types of passive tracking instruments. A longer sampling interval sometimes provide lower measurement error and this should be kept in mind, while interpreting the result. 1.2

T=0.2s T=0.5s T=0.8s T=1s

LBRE of velocity(m/s)

1 0.8 0.6 0.4 0.2 0 0

5

10 Time (sec) 15

20

25

Fig 8.4 Influence of the sampling time on LBRE of velocity

LBRE of position (m)

30

0.1*R 0.2*R 0.5*R R 2*R 5*R

25 20 15 10 5 0 0

5

10

15

20

25

Time (sec) Fig. 8.5 Influence of measurement error covariance on LBRE of position

150

Chapter 8

1.2

0.1*R 0.2*R 0.5*R R 2*R 5*R

LRBE of velocity(m/s)

1 0.8 0.6 0.4 0.2 0 0

5

10

15

Time(sec)

20

25

Fig. 8.6 Influence of measurement error covariance on LBRE of velocity

8.3.4 Influence of Process Excitation Covariance The effect of changes in process excitation covariance is that, with increase in Qk , J k +1 decreases as is evident from eqn 8.6. This has the effect of increasing the LBRE of position and velocity. However, it is observed from figs 8.7 and 8.8 that large variation of Q produces little change in position LBRE. For velocity LBRE, however, error settling becomes poorer with large error overshoots in velocity as the norm of Q is increased. This implies that there is an overall increase in LBRE with the increase in Q . A larger value of Q indicates larger manoeuvres by the target or greater modelling inaccuracies. The result shows that these may adversely affect the velocity estimation but not so much for position estimation. A wide range of Q shows the applicability of the proposed formulation without any numerical problem.

8.3.5 Influence of P22,0 It would be of interest to know the effect of the initial covariance term ( P22, 0 ) associated with velocity, as this value was tinkered with and a nominal value of 1 was used in the original filtering problem [Lin 2002] to avoid large values of P22 , 0 . Figs 8.9 and 8.10 show the variation of LBRE of position and velocity respectively with P22 , 0 changed over some reasonable range. It is observed that P22 , 0 , does not have any effect on the LBRE of position even for large values of P22 , 0 . However, it does have a significant effect on the LBRE of velocity. Also, a change in 151

Chapter 8

P22, 0 does not have significant effect on the final error settling after 20s, which is 1.01 m (for

position) and 0.29 m/s (for velocity) irrespective of the initial condition chosen. Thus the gain obtainable by using prior knowledge is to a large extent quantified.

30

0.1*Q 0.2*Q 0.5*Q Q 5*Q 10*Q 15*Q 20*Q 25*Q

LBRE of position(m)

25 20 15 10 5 0 0

5

10 15 Time(sec)

20

25

Fig. 8.7 Influence of process excitation covariance on LBRE of position 1.6

0.1*Q 0.2*Q 0.5*Q Q 5*Q 10*Q 15*Q 20*Q 25*Q

LBRE of velocity (m/s)

1.4 1.2 1 0.8 0.6 0.4 0.2 0 0

5

10 15 Time (sec)

20

25

Fig. 8.8 Influence of process excitation covariance on LBRE of velocity

152

Chapter 8

LBRE of position (m)

30

P22(0)=0.2 P22(0)=0.5 P22(0)=1 P22(0)=2 P22(0)=5

25 20 15 10 5 0 0

5

10

15

Time (sec)

20

25

Fig. 8.9 Influence of P22,0 on LBRE of position

LBRE of velocity (m/s)

2.5 P22(0)=0.2 P22(0)=0.5 P22(0)=1 P22(0)=2 P22(0)=5

2 1.5 1 0.5 0 0

5

10

15

Time (sec)

20

25

Fig. 8.10 Influence of P22,0 on LBRE of velocity

8.3.6 Adding Another Measurement Sightline rate signal is usually available from the seeker. It would be of interest to know whether use of such signal would improve the estimation accuracy. Measurement Model-II discussed in chapter 3.6.3 and a sampling time of 0.1 s have been used for this study. Three cases are considered for SLR noise covariance ( R22 ) of 0.01, 0.001 and 0.0001 rad/ s 2 . The last figure is equivalent to the one sigma ( 1σ ) error of 10 mil/s and considered fair for this class of seeker. While introducing this additional measurement has hardly impact on the LBRE of position (fig 8.11), the same for velocity is shown in fig 8.12 indicates substantial improvement over the single measurement case (with only sight line angle measurement) when R22 is small. For large value of R22 the results is the same as that for single measurement.

153

Chapter 8

LBRE of position(m/sec)

30 R22=0.0001 R22=0.001 R22=0.01 SLAmeasure

25 20 15 10 5 0 0

5

10

Time (sec)

15

20

Fig-8.11. Effect of introducing sight line rate measurement (position)

LBRE of velocity (m/sec)

1.2

R22=0.0001 R22=0.001 R22=0.01 SLA mesure only

1 0.8 0.6 0.4 0.2 0 0

5

10 Time(sec)

15

20

Fig-8.12. Effect of introducing sight line rate measurement (velocity)

8.3.7 Influence of Eclipsing Effect The eclipsing effect [Zrnic 2001] is studied with the LOS-only measurement scheme (Measurement Model III in section 3.6.4). In pulse radars, the echo from the target can be completely or partially eclipsed, depending on the relative moment of its return (relative to the transmission pulse). The eclipse is defined as a situation when the echo has returned, but the receiver is off, since the transmitter is on. The SNR for a high PRF MMW seeker is shown qualitatively in fig 3.9. Note that as the range decreases, the SNR improves as shown by the envelope of the curve. The eclipsing is characterised (approximately) by an absolute sine wave modulated by the SNR envelope. The corresponding error covariance matrix in logarithmic scale is shown in fig 3.10 in chapter 3.6.4.

154

Chapter 8

The effect of eclipsing, as contrasted to the same for the SNR envelope on the LBRE of the position and velocity is shown in fig 8.13 and 8.14. It may be noted that in this example, the effect of eclipsing is not as severe as one would apprehend. 30

with eclipsing without eclipsing

LBRE of position

25 20 15 10 5 0 0

5

10 Time (sec)

15

20

Fig. 8.13 Effect of Changing SNR on LBRE of Position

1.2 LBRE of velocity (m/s)

with eclipsing

1

without eclipsing

0.8 0.6 0.4 0.2 0 0

5

10 Time

15

20

Fig.8.14 Effect of Changing SNR on LBRE of velocity

8.4

Preliminary Tracking System Design Using CRLB

The case study presented above paves the way to discuss how such analysis can be useful in preliminary design of a tracking system. Though some points have been discussed to a limited extent in the previous sections, we consolidate the main issues here. We have noted that selection of nonlinear filter can be finalized only after substantial experimentation. Selection of hardware components cannot wait till such experiments are complete. This is because software can be changed as the project proceeds but the hardware

155

Chapter 8

issues need to be frozen earlier. CRLB allows a quick analysis of the parametric sensitivity of filter performance in the manner shown in the foregoing study and helps to freeze the important hardware issues. For example, before going to the detailed design of the tracking system the designer needs to know whether the target tracking with a specified accuracy is at all possible or not with the present instrumentation and sensor system. If CRLB result indicates infeasibility, then either the specifications of the filter performance must be relaxed (with consequence in guidance and control performance) or better instruments (lower measurement error covariance) and/or higher sampling frequency (and consequent costlier on board computer) must be chosen. Costs are involved in better instruments and computer. A cost-performance trade-off can then be carried out. Only if the CRLB results indicate that the desired accuracy is possible (with sufficient margin for specific filter efficiency) then only the designer should proceed with trying different nonlinear filtering techniques and compare the results among them. The studies in the previous section also exemplify how the impact of specific sensors can be assessed beforehand by the CRB analysis. The case in point is the eclipsing effect which occurs with milimetric wave based seeker with high pulse repeat rate. The study similar to above could immediately dispel the doubt whether such eclipsing would substantially reduce the filter performance.

8.5

Discussion and Conclusion

The role of CRLB for analyzing tracking system specification and preliminary system design has been presented. The analysis indicated how a quantitative understanding of the influence of design parameters on the tracking filter performance could be obtained through the CRLB. The potential of using Cramer Rao bound (CRB) as a tool for selecting sensors and sampling frequency has thereby been established. A co-worker [Srinivasan 2007] has subsequently extended this to aid guidance system configuration design. In order that the CRLB tool can be effectively used, alternative expressions are provided to calculate CRLB for (singular and) non singular process noise covariance. The concepts have been demonstrated by the fairly well known bearing only target tracking (BOT) problem. Specifically, the effects of (i) sampling time (ii) measurement error covariance (iii) process excitation covariance (iv) initial error covariance (v) additional measurement have been clearly brought out. For a typical case of high PRF, MMW seeker, it has been demonstrated that the adverse effects of eclipsing is limited. Also explored are the effects of using more

156

Chapter 8

measurements on tracking performance. All these have been achieved without going into the specifics of filter design and without elaborate Monte Carlo performance analysis for evaluating such filters. Such analysis brought out that fact that large estimation errors may occur from limitations of tracking instruments and signal processing limitation (and also due to target-observer relative kinematics) even if the theoretically best filters were available. Before trying to put substantial effort in redesigning the filter or applying novel filtering techniques, a CRB analysis would provide valuable insight. It may be noted that the trends discovered through the CRLB analysis were of the same nature that obtained from EKF and UKF though such results had not been presented here. A caveat is in order. Filter parameters like filter initialisation scheme, choice of the process noise covariance and initial error covariance matrix are often heuristically changed (keeping the truth model as it is) to obtain performance improvement of tracking filters. The CRB results should not be used as a guide blindly to obtain a cue whether such effort is justified and whether further improvement in the filtering performance is possible or not. This is because of the fact that CRB uses identical models for truth and the minimum variance estimator. Another issue called “super efficiency” needs to be mentioned here which needs further investigation and has been included in the suggested future work section. In some of the simulations runs presented herein, the RMS errors of specific filters were found to be smaller than that predicted by the CRLB at some sample times. In the parameter estimation scenario, if a specific estimator gives a performance equaling that of CRLB, the estimator is called an efficient estimator. The nomenclature has now been extended to state estimation of dynamic systems as well. Wherever, a specific filter outperforms CRLB, reason of such super-efficiency [Stoica 1996] needs to be resolved by examining whether identical assumptions had been made. It may be noted in the provided simulation results, that though there is a mild tendency of superefficiency for EKF and PF, on the whole, CRLB results are better than individual filter results. While this issue needs to be addressed as a future work, this hardly invalidates the usefulness of CRLB for design studies. Nor this trend of super efficiency unique to the study presented here. The phenomenon of super efficiency is also noticeable in results obtained by [Ristic 2003a], though it has not been explicitly discussed therein. The reasons (bias variance trade-off) for super efficiency suggested by [Bergman 1999 b, page 70] do not apply in this case as is evident from the PF result, which is generally unbiased.

157

9 9.1

Risk Sensitive Filter For INS Self Alignment

Introduction

Having developed a number of methods for nonlinear risk sensitive estimation, in this chapter we explore the applicability of such filters for alignment of strap down inertial navigation system (SDINS). The literature survey done in chapter 2 points out that Inertial Navigation had been a fertile application area for estimation theory especially for nonlinear estimation techniques. The topic, therefore, has drawn significant amount of interest from the researchers [Barth 1999, Moon 1999, Basil 2005]. In Navigation, estimation techniques are primarily employed in Alignment, Calibration and Fusion. Among the three types of problems, alignment problem has been considered in this thesis for subject of study and augmentation. In the context of Strap down INS, it may be recalled that alignment means finding the instrument axis system orientation with reference to a known coordinate axis system and is typically done in two phases, namely, (i) Coarse alignment and (ii) Fine alignment. In coarse alignment the INS axis (the analytic axis) is roughly and rather rapidly aligned [Ali 2004, Feng 1996] with respect to the reference axis with residue misalignment errors, which are less typically than 3 degree that is 0.05 radian. After the coarse alignment is done, small angle approximations ( cos θ = 1, sin θ = θ ) may be employed for the fine alignment and the resultant equations are often linear. This allowed application of linearized filters like EKF or even KF to be successfully employed for the fine alignment. In this chapter we would consider both small initial error (linear problem) and large initial error (nonlinear) problem. In this chapter, we would restrict our attention to self-alignment where the SDINS is aligned from the gravity vector and the earth rotation vector. Though coarse self-alignment is possible in the presence of zero mean small oscillation of the platform, we would consider when the platform is essentially stationary, as required for fine alignment. For self-alignment, it is customary to consider only the (horizontal) velocity outputs from the INS. As the INS is stationary, the ideal output would be zero. The vertical velocity component is seldom used firstly because it does not effectively contribute towards alignment and secondly as the built in navigation algorithm might sometimes exhibit slow instability. It is well known that observability in self-alignment process using only velocity measurements and stationary base (or with unmeasured platform motion) is poor as discussed in [Chuanbin 2004]. This primarily affects the settling of azimuth error in the case of instrument biases. Unless

Chapter 9

special measures are taken (for example using filtered gyro outputs as additional measurement [Chuanbin 2004]) fine alignment is virtually ruled out. However, from [Chuabin 2004] it appears that coarse alignment is still possible with residual errors through a long error settling time. We examine this and demonstrate that this assumption is invalid and the lack of observability can not be cured with out augmenting the measurement. The initial results on risk sensitive filtering by the present worker in this thesis and in [Bhaumik 2005a] indicated that risk sensitive filters (RSF) are good at estimating despite the presence of unmodelled biases. It is therefore considered interesting to explore whether the RSF can provide faster convergence and lower residual error in the presence of instrument biases. Probably this is the first work where nonlinear risk sensitive filtering has been applied to a navigation problem. The problem has been formulated without the assumption of small misalignment angle which results in nonlinear equation. Under small angle assumption the problem becomes linear one. Initially biases are considered as a part of state variable as discussed in [Chuanbin 2004]. Different aspects of that scenario including observability, bias sensitivity and condition for obtaining good performance have been discussed. Also best achievable performance has been determined by considering biases as known inputs. Sensitivity analysis has been carried out in presence of different unmodelled bias. To illustrate the benefits of additional measurements, Gyro outputs have been considered as additional measurements and study for estimation accuracy and bias sensitivity have been carried out. Risk sensitive estimation has been done for system with only velocity measurement and also for system with additional gyro measurements in unmodelled bias situation. Later nonlinear risk sensitive filters such as extended risk sensitive filter (ERSF), risk sensitive unscented Kalman filter (RSUKF) and risk neutral EKF have been applied to the nonlinear problem. The results obtained from these filters are compared.

9.2

Problem Formulation

Here a self-alignment problem has been formulated where the misalignment angles between the INS frame and computer frame is to be estimated. As the misalignment does not vary during the alignment process, this could have been considered as a parameter estimation problem. However, as is customary in KF literature, the problem is reformulated as a state estimation problem by considering the unknown misalignment as additional state variables. In western literature the North-East-Down (NED) coordinate system is more commonly used [Kelly 1994], whereas Chinese and Russian literature tend to use the ENU convention [Gao 2007], [Kong 2000]. Here East north up (ENU) co ordinate is chosen as navigational co ordinate

159

Chapter 9

system and general nonlinear form of the INS equations in the ENU frame are derived. For small angle assumption it has been shown that the expressions match with the expressions provided in [Chuanbin 2004]. The motivation behind the choice of ENU co ordinate system is to compare Chuanbin’s result in linear case. Formulation of nonlinear ENU model became necessary as the publication referred by Chuanbin is not generally available in public domain. 9.2.1

Velocity Error Equation

The velocity error equation resolved in INS frame or computer frame can be written as [Kong 2000]: c δv& = (Ccp − I ) f c − (2ωiEc + ω Ec ) × δv + ∇

(9.1)

Where p denotes Navigational co ordinate system and c denotes Computer (or INS) co ordinate system. δv is velocity error vector expressed in INS frame. So δv = [δve δvn δvu ] . f c is T

specific force vector comprising gravity plus vehicle acceleration in INS co ordinate system. ωiEc c is the angular velocity of earth vector resolved in the INS frame. The quantity ω Ec is the angular

velocity of INS co ordinate with respect to earth fixed frame. The rate vector of INS with respect c to earth fixed frame ( ω Ec ) arises due to motion of the vehicle. If we assume the vehicle is

stationary then this term can be assumed as zero. From now on for the rest of the discussion we assume the vehicle is stationary. ∇ is accelerometer bias vector expressed in INS (navigation) co ordinate frame. So ∇ = [∇ e ∇ n ∇ u ] . If it is assumed that the acceleration bias is in body frame T

then appropriate transformation need to be applied and after that they can be used in equation (9.1). Ccp is co ordinate transformation matrix from INS frame to navigational frame. It has been mentioned earlier that in this study east north up [ENU] convention of co ordinate frame has been chosen. So in east north up co ordinate system the ωiEc

can be written as

ωiEc = [0 ωiE cos(ϕ ) ωiE sin(ϕ )]T , where ϕ is the latitude of the vehicle. Let us assume the

misalignment angle in east, north and azimuth (up) direction is φe , φ n and φu . Then the transformation matrix Ccp can be written according to [Kong 2000] as cn cu − sn se su Ccp = − ce su sn cu + cn se su

cn su + sn se cu ce cu sn su − cn se cu

− s n ce se cn ce

Where c e = cos(φ e ), c n = cos(φ n ), c u = cos(φ u ) s e = sin(φ e ), s n = sin(φ n ), s u = sin(φu ) . From equation (9.1) we obtain

160

Chapter 9

δv&e δve 0 c p c δv&n = (Cc − I ) f − 2ω iE cos(ϕ ) × δv n + ∇ c δv&u 2ω sin(ϕ ) δvu iE δv&e − 2ωiEc sin(ϕ ) 2ω iEc cos(ϕ ) δve ∇ e 0 p c c 0 0 δv& n = (C c − I ) f − 2ω iE sin(ϕ ) δv n + ∇ n c δv&u − 2ω iE cos(ϕ ) δvu ∇ u 0 0

or,

Now we consider only north and east velocity error because with vertical velocity component navigation algorithm might sometimes exhibits slow instability. As the vertical velocity is not considered the acceleration bias in vertical component will not come into picture. δv&e 0 − 2ωiE sin(ϕ ) δve ∇ e p c = (C c − I ) f − + & δ v 2 ω sin( ϕ ) 0 n iE δvn ∇ n

or

δv&e = (cn cu − s n se su − 1) f e + (cn su + s n se cu ) f n − s n ce f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.2)

δv&n = −ce su f e + (ce cu − 1) f n + se f u − 2ωiE sin(ϕ )δve + ∇ n

(9.3)

Small Misalignment Angle: If it is assumed that coarse alignment had been completed before, initial misalignment angle between the axes can be taken as small enough. In such scenario φ e , φ n and φ u can be considered as very small. Under this assumption we can use small angle

approximation rule ( cos θ ≈ 1, sin θ ≈ θ ). So in this situation equations (9.2) and (9.3) will be δv&e = φu f n − φn f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.4)

δv&n = −φu f e + φe f u − 2ωiE sin(ϕ )δve + ∇ n

(9.5)

If we consider vehicle itself does not experience any acceleration other than gravity. So the specific force vector f = [0 0 g ]T . Here the gravity vector in up direction is taken as positive because the sensor measure specific force needs to overcome gravity [Titterton 1997 pp 283]. So equations (9.4) and (9.5) become δv&e = −φn g + 2ωiE sin(ϕ )δvn + ∇ e

(9.6)

δv&n = φe g − 2ωiE sin(ϕ )δve + ∇ n

(9.7)

It can be noted here the equation (9.6) and (9.7) is same as described in [Chuanbin 2004].

9.2.2

Attitude Error Equation

Attitude error equation for large angle is c c φ& = ( I − Ccp )(ωiEc + ω Ec ) + δω Ec +ε

(9.8)

161

Chapter 9

Where Ccp is the DCM for transformation from INS frame to navigation frame. T c δω Ec = [− δvn / R δve / R δve tan(ϕ ) / R ] ; where R is the radius of the earth. ε is the gyro drift

vector ( ε = [ε e ε n ε u ]T ). So equation (9.8) can be written as φ&e 1 − cn cu + sn se su − (cn su + sn se cu ) s n ce 0 − δvn / R ε e & ω cos(ϕ ) + δv / R + ε = c s 1 − c c − s φ e u e u e e n iE n φ& − ( s c + c s s ) − ( s s − c s c ) (1 − c c ) ω sin(ϕ ) δv tan(ϕ ) / R ε n u n e u n u n e u n e iE e u u

or φ&e = −(cn su + sn se cu )ωiE cos(ϕ ) + s n ceωiE sin(ϕ ) − δvn / R + ε e

(9.9)

φ&n = (1 − ce cu )ωiE cos(ϕ ) − seωiE sin(ϕ ) + δve / R + ε n u

(9.10)

φ&u = −( sn su− cn se c u )ωiE cos(ϕ ) + (1 − cn ce )ωiE sin(ϕ ) + δve tan(ϕ ) / R + ε

(9.11)

Small Misalignment Angle: If it is assumed that coarse alignment had been completed before, initial misalignments between the axes can be taken as small enough. In such scenario φ e , φ n and φ u can be considered as very small. Under this condition the equations (9.9-9.11)

become φ&e = −φuωiE cos(ϕ ) + φnωiE sin(ϕ ) − δvn / R + ε e

(9.12)

φ&n = −φeωiE sin(ϕ ) + δve / R + ε n

(9.13)

φ&u = φeωiE cos(ϕ ) + δve tan(ϕ ) / R + ε u

(9.14)

It should be noted that the equations (9.12-9.14) are exactly same as the equations described in [Chuanbin 2004].

9.3

Estimation of Misalignment Angle Considering Biases in State Variable

In this section a self alignment problem has been formulated where the velocity errors, misalignment angles, accelerometer and gyro biases have been considered as state variable. Small angle assumption has been taken as a result the process equation becomes linear one. It has been verified that the process equation exactly matches with that of described in [Chuanbin 2004]. As the system is unobservable, decrease in error with time as shown in [Chuanbin 2004] seems to be illusive. Again the Chuanbin’s paper did not provide the Monte Carlo Simulation results. Only estimation error obtained from typical run had been shown. From earlier chapters we have seen that error for single instance is not sufficient to assess the performance of any filter. Because in single instance filter may track the truth but RMSE may diverge due to track loss cases. So our primary objective behind this study was to get more insight about self alignment and assess the filter’s performance in more precise way through Monte Carlo simulation.

162

Chapter 9

Let us consider the state vector consists of east and north velocity error, three misalignment angles and accelerometer and gyro biases. So X = [δv e δv n φ e φ n φ u ∇ e ∇ n ε e ε n ε u ]T . Combining equations (9.6), (9.7) and (9.12 - 9.14) we can write process dynamics as 0 2ωiE sin(ϕ ) 0 0 −g δv&e δv& − 2ω sin(ϕ ) 0 0 0 g iE n & φe ωiE sin(ϕ ) − ωiE cos(ϕ ) 0 0 −1/ R & 1/ R 0 0 0 − ωiE sin(ϕ ) φn φ& tan(ϕ ) / R ω ϕ 0 cos( ) 0 0 iE &u = 0 0 0 0 0 ∇e & ∇ 0 0 0 0 0 n & ε 0 0 0 0 0 e ε& 0 0 0 0 0 n ε&u 0 0 0 0 0

1 0 0 0 0 δve 0 1 0 0 0 δvn 0 0 1 0 0 φ e 0 0 0 1 0 φ n 0 0 0 0 1 φu + w 0 0 0 0 0 ∇ e 0 0 0 0 0 ∇ n 0 0 0 0 0 ε e 0 0 0 0 0 ε n 0 0 0 0 0 ε u

Where w is process noise assumed to be zero mean Gaussian with covariance Q ( w ~ N (0, Q) ). The measurement equations are written as Z = HX + v

(9.15) 1 0 0 0 0 0 0 0 0 0 and v is measurement noise assumed to be zero mean 0 1 0 0 0 0 0 0 0 0

Where H =

Gaussian with covariance η i.e. ( v ~ N (0,η ) ). An observability matrix of the system has been formulated. It is seen that the rank of the observability matrix is seven. But the system is 10th order system. So three states of the system are unobservable. The system is linear Gaussian so the states are estimated using Kalman filter. The parameters for simulation are chosen as described in [Chuanbin 2004].

•

The truth model misalignment angles are initialized as φ e = φ n = φ u = 10 .

•

Initial acceleration biases for truth are chosen as ∇ e = ∇ n = 100µg

•

Initial gyro constant drift rates for truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Initial error velocities of truth model are chosen as 0.01 m/s.

•

Sampling time 1 sec

•

Initial state values of the estimator are chosen as zero.

•

Latitude of the vehicle is chosen as 45 degree.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

Angular velocity of earth ω iE = 7.27220e-5.

163

(0.005 0 h −1 ) 2

0 0 0 0 0

]

Chapter 9

•

Radius of the earth R=6378137.0 m

•

Acceleration due to gravity = 9.81 m/s2

•

Initial error covariance is chosen as

[

P0 = diag ( 0.012

0.012 10 10 10 100 µg 100 µg

0.010 h −1

0.010 h −1

]

0.010 h −1 )

From this point till rest of the chapter the values of sampling time, latitude, angular velocity, radius of the earth and acceleration due to gravity will remain same for each simulation.

Simulation Results The Kalman filter has been realized in discrete domain. But the process equation described above is in continuous domain. So discretization of the process has been done with one second sampling time interval. The errors in estimation of east, north and azimuth misalignment angles obtained from KF have been calculated and the result matches fairly with that of the figure shown in [Chuanbin 2004], hence is not shown in this section. Root mean square errors (RMSE) of three misalignment angles have been plotted in figure 9.19.3 for 100 Monte Carlo run. It has been seen from the figure that all the RMS errors of misalignment angles are converging well. Although convergence rate of east and north misalignment angle estimation is very high and almost instantaneous (with in 8 second RMSEs reach 0.0058 degree which are very near to their respective settling point), RMS error in azimuth misalignment angle converges slowly (in 50 second it reaches to 0.1568 degree and it settles on 0.0656 degree). Chuanbin [Chuanbin 2004] in his paper did not provide RMSE for misalignment angles. Figure 9.1 - 9.3 may be referred for completeness of their analysis. One point needs to be mentioned here that, linear velocities are the measurements with which the self-alignment is carried out. So following convention, the results for velocity error estimation are not presented throughout this chapter. The system described above is unobservable and in spite of that all the RMS errors are settling well. This is quite surprising. Being curious about the unrealistically high efficiency of the estimator, RMS errors for different values of initial gyro and accelerometer biases are calculated. The RMSE for different initial accelerometer and gyro biases have not been shown here as the nature of the figures are more or less same as the figures 9.10 – 9.13. The RMS error settling value of different misalignment angles for different gyro and accelerometer biases have been summarized in table 9.1. It has been seen from the table that as the initial values of accelerometer and gyro biases increase the error settling values also increase. So it is found out through the study that, the good performance shown by Chuanbin, in spite of unobservibity, is due to very small choice of initial accelerometer and gyro biases.

164

Chapter 9

RMSE plot of east misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Step

Fig 9.1. RMSE for east misalignment estimation considering biases as state variable RMSE plot of north misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Step

Fig 9.2. RMSE for north misalignment estimation considering biases as state variable RMSE plot of azimuth misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

step

Fig 9.3. RMSE for azimuth misalignment estimation considering biases as state variable

165

Chapter 9

Table 9.1 RMS error settling values for different unmodelled gyro and accelerometer bias

Values of unmodelled bias

RMSE error settling values (deg)

Gyro biases Accelerometer biases East (deg/h) (micro g)

9.4

North

Azimuth

0.01

0.005

0.0058

0.064

0.05 0.1 0.15 0.2

100

0.0056 0.0056 0.0057 0.0057

0.0056 0.0058 0.0058 0.0058

0.3279 0.6538 0.8976 1.0518

0.01

100 1000 2000 3000 40000

0.005 0.0056 0.027 0.0377 0.0433

0.0058 0.0633 0.1365 0.2165 0.2998

0.064 0.1211 0.1943 0.274 0.3571

Estimation of Misalignment Angles Considering Biases as Known Input

From the above discussion it might be realised that it is worthy to know about the estimator’s performance in presence of accurately known gyro and accelerometer biases. Because that performance can be considered as the achievable lower bound and hence treated as benchmark. So towards this goal the system has been modified as fifth order system where the state variables are velocity errors and misalignment angles. So

X = [δve

δvn φe φn φu ]T . Gyro and

accelerometer biases are considered as known inputs. Again we consider small angle assumption so that system remains linear. Process dynamics of the system can be written as −g 0 2ωiE sin(ϕ ) 0 0 δv&e δve ∇ e δv& − 2ω sin(ϕ ) δv ∇ 0 g 0 0 iE n n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + ε e + w − 1/ R 0 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn ε n φ&u tan(ϕ ) / R φu ε u ωiE cos(ϕ ) 0 0 0

(9.16)

Where w is process noise assumed to be zero mean Gaussian with covariance Q ( w ~ N (0, Q) ). The measurement equation is taken as discussed in equation 9.15 in section 9.3.

Simulation parameters •

The truth model misalignment angles are initialized as φ e = φ n = φ u = 10 .

•

Initial error velocities of truth model are chosen as 0.01 m/s.

•

Gyro constant drift rate for both filter as well as truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Acceleration biases are chosen as ∇ e = ∇ n = 100µg

166

Chapter 9

•

Initial state values of the estimator are chosen as zero.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

]

Initial error covariance is chosen as P0 = diag ([0.012 0.012 10 10 10 ])

The errors in estimation of east, north and azimuth misalignment angles obtained from KF have been plotted in figure 9.4 - 9.6 respectively. From the figures it can be realized that the convergence rate of east and north misalignment angle estimation is very good and almost instantaneous where as the estimation in azimuth misalignment angle converges slowly. The root mean square errors (RMSE) of east, north and azimuth misalignment angle out of 100 MC run have been calculated and shown in figure 9.7- 9.9 respectively. Results show RMSE of all the misalignment angles are converging well and settling on almost zero degree although the convergence rate of azimuth RMSE is significantly slower that other two misalignment angles. For example at 50 second the RMSE of east and north misalignment angle reach at 0.001 degree where as the RMSE of azimuth reaches at 0.1 degree. So these RMS error profile may be considered as the benchmark if self alignment is carried out using velocity error measurements. Estimation error of east misalignment angle

Estimation Error (deg)

1.2 1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.4 Estimation error of east misalignment angle for a typical run

167

Chapter 9

Estimation error of north misalignment angle

Estimation Error (deg)

1.2 1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.5 Estimation error of north misalignment angle for a typical run Estimation error of azimuth misalignment angle

Estimated Error (deg)

2 1.6 1.2 0.8 0.4 0 0

50

100

150

200

250

300

-0.4 Time (sec)

Fig 9.6 Estimation error of azimuth misalignment angle for a typical run RMSE plot of east misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.7 RMSE of east misalignment angle for 100 MC run

168

Chapter 9

RMSE plot of north misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.8 RMSE of north misalignment angle for 100 MC run RMSE of azimuth misalignment angle 1.2

RMSE(deg)

1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig 9.9 RMSE of azimuth misalignment angle for 100 MC run

9.5

Effect of Unmodelled Bias

The presence of accelerometer and gyro biases make the self alignment most difficult. If they have been considered as state variables the system becomes unobservable and the estimation accuracy degrades. So it may be worthy to see how worst will be the estimation if filter model consider the gyros and accelerometers as bias free but in truth they have biases. So this situation may be considered as the worst case situation for self alignment problem. Truth process model is described as

169

Chapter 9

0 2ωiE sin(ϕ ) 0 −g 0 δv&e δve ∇ e δv& − 2ω sin(ϕ ) δv ∇ 0 g 0 0 iE n n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + ε e + w 0 − 1/ R 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn ε n & φu tan(ϕ ) / R φu ε u ωiE cos(ϕ ) 0 0 0

(9.17)

Where as the process model for filter is 0 2ωiE sin(ϕ ) 0 −g 0 δv&e δve δv& − 2ω sin(ϕ ) δv 0 g 0 0 iE n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + w 0 − 1/ R 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn φ&u tan(ϕ ) / R φu ωiE cos(ϕ ) 0 0 0

The measurement model is taken as described in equation 9.15. For this experiment the initial values of truth and initial values of filter, process and measurement noise covariance have been chosen as described in section 9.4. Accelerometer unmodelled biases have been kept constant value of 100 µg . Gyroscope biases have been changed from 0.01 deg/h to 0.2 deg/h in step. It has been observed that for the change of gyro bias in this range there is almost no effect on east and north misalignment estimation. For this reason RMSE plots of east and north misalignment angle have not been shown here. Azimuth misalignment estimation is heavily affected for unmodelled gyro bias. The RMSE of azimuth misalignment angle obtained from 100 MC run in presence of unmodelled biases has been shown in figure 9.10. From the figure it has been seen that as the value of unmodelled gyro biases increase the RMSE settling value for azimuth misalignment angle also increases. RMSE for different unmodeled gyro biases gyrobias=0.2deg/h gyrobias=0.15deg/h gyrobias=0.1deg/h gyrobias=0.05deg/h gyrobias=0.01deg/h

1.4

RMSE (Deg)

1.2 1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig. 9.10 RMSE of azimuth misalignment angle for different unmodelled gyro biases

170

Chapter 9

Similar experiment has been carried out to know the effect of unmodelled accelerometer biases. For that gyro biases are kept at constant value of 0.01 degree/h where as unmodelled accelerometer biases have been varied from 100 µg to 5000 µg . Interestingly unmodelled accelerometer bias has the effect on the estimation of each misalignment angle. Figure 9.11 – 9.13 show the effect of unmodelled acceleration biases on RMSE of east, north and up mis alignment angle. For each case RMSE settling values increase with the increase of unmodelled acceleration biases.

RMSE of east misalignment angle for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

-0.2 Time (sec)

Fig 9.11 RMSE of east misalignment angle for different unmodelled accel bias RMSE of North misalignment for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

-0.2 Time (sec)

Fig 9.12 RMSE of north misalignment angle for different unmodelled accel bias

171

Chapter 9

RMSE of Azimuth misalignment for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

Time (sec)

Fig 9.13 RMSE of azimuth misalignment angle for different unmodelled accel bias

9.6

Risk Sensitive Kalman Filter With Unmodelled Bias In earlier chapter it has been shown that risk sensitive filter performs better in presence of

unmodelled bias compare to risk neutral filter. So it seems to be worthy to apply risk sensitive Kalman filter in the situation of unmodelled bias and examine whether the estimation would be better in case of RSKF. Initial state values of truth, process and measurement noise covariance and initial co variance are chosen as described in section 9.4. The values of unmodelled accelerometer and gyro biases have been chosen as 2000 µg and 0.1 degree/h respectively. The value of risk sensitive parameter is taken as 9. The RMSE plots of east, north and up misalignment angle obtained from 100 MC run of KF and RSKF have been shown in figure 9.14, 9.15 and 9.16 respectively. From the figures it is obvious that instead of unmodelled bias no improvement in estimation accuracy by RSKF over KF has been observed.

172

Chapter 9

RMSE of East misalignment angle under unmodeled bias 1.2 KF 1

RSKF

RMSE (deg)

0.8 0.6 0.4 0.2 0 0

50

100

150

200

Time (sec)

250

300

Fig 9.14 RMSE of east misalignment angle under unmodelled bias RMSE of North misalignment angle under unmodeled bias 1.2 KF

1 RMSE (deg)

RSKF

0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig 9.15 RMSE of north misalignment angle under unmodelled biases RMSE of up misalignment angle under unmodeled bias 1.4 KF RSKF

RMSE (deg)

1.2 1 0.8 0.6 0.4 0

50

100

150

200

250

300

Time(sec)

Fig 9.16

RMSE of azimuth misalignment angle under unmodelled biases

173

Chapter 9

9.7

Misalignment Angle Estimation Using Gyro Output as Measurement

Measurement Equation It is worthy to consider the gyro output as measurement of the system along with velocity errors because it will cure the unobservability problem encountered by Chuanbin [Chuanbin 2004]. Also additional measurement will increase the observability of the system with known input bias, which has been described in section 9.4. So considering gyro output the measurement equation becomes Z = HX + v 1 0 0 0 0 0 1 0 0 0 ωiE sin(ϕ ) − ωiE cos(ϕ ) H = 0 −1/ R 0 0 − ωiE sin(ϕ ) 0 0 1/ R tan(ϕ ) / R 0 cos( ) 0 0 ω ϕ iE

v is measurement noise assumed to be zero mean white Gaussian. Strictly speaking we cannot use gyro outputs as measurements directly. The reason behind this is, gyro output equations are described in navigational frame where as measurements are in instrumental frame. But here we make approximation that the gyro output in navigational frame is same as that of in instrumental frame. The error arises due to this approximation is taken care of by large measurement noise covariance.

Biases Considered as Known Input If biases are considered as known input the process dynamics of the system can be represented using equation (9.16). The simulation parameters except measurement noise covariance are taken as described in section 9.4. The covariance of measurement noise is taken as

[

R = diag (1e − 4) 2

(1e − 4) 2

]

2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h . RMSEs for

all the three misalignment angles have been calculated in this scenario and the values are compared with respect to velocity error only measurement case. The RMSE for east and north misalignment are same in nature as shown in figures 9.7 & 9.8 except the fact that errors settle at little lower value compare to velocity error only measurement case, hence is not shown here. For example after 200 second, for velocity error only measurement scenario, the RMSE of east and north reach at 0.0051deg and 0.0058 deg respectively. If additional measurements are incorporated the east and north RMSE misalignment angles settle to 4.2048e-005 and 4.0229e005 degree respectively. For azimuth misalignment angle with five measurements significant amount of improvement has been achieved in terms of convergence. With these additional measurements the rate of convergence of azimuth misalignment estimation become comparable

174

Chapter 9

with that of east and north. The RMS errors of azimuth misalignment angle for both cases have been shown in fig 9.17. RMSE of azimuth misalignment angle for system with additional gyro output measurement 1.2

Velocity error only meas with additional gyro meas

1

RMSE (deg)

0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

Time

Fig 9.17 RMSE for azimuth misalignment for velocity error only measurement case and additional gyro output measurement case with known input bias

Estimation With Unmodelled Bias Now experimentation has been carried out to investigate whether increase of observability can minimise the effect of unmodelled bias. For this experiment the gyro and accelerometer bias have been considered in truth but not in filter equations. So process dynamics is represented by equation (9.17). Here the simulation has been carried out using two different measurement error covariances ( R) to know the effect of. Initially the measurement error covariance is taken as

[

R = diag (1e − 4) 2

(1e − 4) 2

]

2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h .

Accelerometer biases are kept constant at 100 microg and gyro biases have been varied from 0.01 deg/h to 0.2 deg/h. The effect of different unmodelled bias is negligible in east and north misalignment estimation. For this reason RMSE results of east and north misalignment estimation for different unmodelled biases have not been presented. The RMSE of azimuth misalignment estimation for different unmodeled gyro biases has been presented in fig 9.18. It is observed from the figure that as the value of unmodelled gyro bias increases the settling value of RMSE is also increase. For example, after 200 second when gyro bias 0.01 deg/h RMSE reaches at 0.0082 deg and for gyro biases 0.2 deg/h RMSE reaches at 0.0481. For in between unmodelled gyro biases RMSE remains in between the above two values. In high value of gyro bias (0.2deg/h) the RMSE shows slightly diverging nature. The reason for that may be more severe effect of biases with the increase of time.

175

Chapter 9

RMSE of Azimuth with Unmodelled bias 1.2

gyrobias=0.01 gyrobias=0.05

1

gyrobias=0.1 gyrobias=0.15

0.8 RMSE

gyrobias=0.2 0.6 0.4 0.2 0 0

50

100

150

200

-0.2 Time (sec)

Fig 9.18 RMSE of azimuth estimation for different unmodelled gyro biases.

Similar simulation has been carried out for higher value of measurement error covariance ( R = diag [(1e − 4) 2 (1e − 4) 2 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h] ) to realize the effect of high measurement noise covariance. The RMSE obtained for this case is very similar with that obtained for small R (see fig 9.19). The only difference is that the RMSE becomes little higher for higher value of R. For example for lower value of R, for unmodelled gyro bias 0.01, the RMSE after 200 second become 0.00481 degree, where as for higher value of R and for same gyro bias after 200 second it is 0.1002 degree. So from this study it may be concluded that the effect of unmodelled biases can be reduced by increasing observability. RMSE of Azimuth with large gyro measurement noise 1.2 gyrobias=0.01 1

gyrobias=0.05 gyrobias=0.1

RMSE

0.8

gyrobias=0.15 gyrobias=0.2

0.6 0.4 0.2 0 0 -0.2

50

100

150

200

Time

Fig 9.19 RMSE of azimuth estimation for different unmodelled gyro biases for higher value of R.

176

Chapter 9

Risk Sensitive Estimation: In this problem RSKF and KF have been simulated to test whether any improvement can be achieved using RSKF under unmodelled accelerometer and gyro bias condition. In this simulation initial values of state, process noise covariance and initial error co variance are chosen as described

in

[

R = diag (1e − 4) 2

section

9.4

The

measurement

noise

covariance

is

taken

as

(1e − 4) 2

5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h . The values of

]

unmodelled accelerometer and gyro biases have been chosen as 100 µg and 0.2 degree/h respectively. The value of risk sensitive parameter is chosen as 3275. RMSE obtained from RSKF and KF is plotted in fig 9.20. From the figure it is obvious that instead of unmodelled bias situation and good observability no improvement in estimation accuracy by RSKF over KF has been observed. RMSE for RSKF & KF

1.2

RSKF 1

KF

RMSE

0.8 0.6 0.4 0.2 0 0

50

100

150

200

-0.2 Time

Fig 9.20 KF and RSKF for gyro output measurement situation

9.8

State Space Model For Large Misalignment Angle

For large misalignment angle the state space equation is the combination of (9.2), (9.3) and (9.9 – 9.11) δv&e = (cn cu − s n se su − 1) f e + (cn su + s n se cu ) f n − s n ce f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.18)

δv&n = −ce su f e + (ce cu − 1) f n + se f u − 2ωiE sin(ϕ )δve + ∇ n

(9.19)

φ&e = −(cn su + sn se cu )ωiE cos(ϕ ) + s n ceωiE sin(ϕ ) − δvn / R + ε e

(9.20)

φ&n = (1 − ce cu )ωiE cos(ϕ ) − seωiE sin(ϕ ) + δve / R + ε n

(9.21)

φ&u = −( sn su− cn se c u )ωiE cos(ϕ ) + (1 − cn ce )ωiE sin(ϕ ) + δve tan(ϕ ) / R + ε u

(9.22)

177

Chapter 9

In this formulation state vector is consists of velocity errors, and misalignment angles ie, X = [δve

δvn φe φn φu ]T . Bias vector consists of accelerometer biases in two channel and gyro

biases in three channels. So for nonlinear case the process equations (9.18 -9.22) can be written as X& = f ( X ) + biasvector + w

(9.23)

We use two velocity errors as measurement as described in equation (9.15).

9.8.1

Alignment Considering Biases as Known Input

In this case bias is considered as known input hence the effect of biases is minimized. For nonlinear case the process model can be described with the equation (9.23). The measurement equation is considered same as in linear case described by the equation (9.15). For simulation the parameters are chosen as follows:

•

The truth misalignment angles are initialized as φ e = φ n = φ u = 10 0 .

•

Initial error velocities for truth model are chosen as 0.01 m/s.

•

Gyro constant drift rate for both filter as well as truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Acceleration biases are chosen as ∇ e = ∇ n = 100µg

•

Initial state values for the estimator are chosen as zero.

•

Latitude of the vehicle is chosen as 45 degree.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

]

Initial error covariance is chosen as P0 = diag ([0.012 0.012 10 0 10 0 10 0 ])

It should be noted that the process equations described by (9.23) are in continuous domain. During simulation the truth value of the states are generated by numerical integration starting from initial value of the state. Here accelerometer and gyro biases have been considered in process model of both truth as well as filter. Extended Kalman filter (EKF) and unscented Kalman filter (UKF) have been simulated. The RMSE in estimation of east, north and azimuth misalignment angle obtained from 100 MC run have been shown in figures (9.21- 9.23). From the figures it is observed that all the misalignment angles are converging to almost zero value. For east and north misalignment angle the RMS errors of the estimator reach at 0.0001 degree after 200 second. For azimuth misalignment angle the RMSE of estimator reaches at 0.0076 degree after 200 second.

178

Chapter 9

RMSE of East Misalignment Angle 12

EKF

10

UKF RMSE (deg)

8 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.21 RMSE of east misalignment angle RMSE of North Misalignment angle 12

EKF

10

UKF RMSE (deg)

8 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.22 RMSE of north misalignment angle RMSE of Azimuth Misalignment Angle

12

EKF

10

UKF RMSE(deg)

8

6

4

2

0 0

50

100

150

200

250

Time(sec)

Fig 9.23 RMSE of azimuth misalignment angle

179

300

Chapter 9

9.8.2

Alignment in Presence of Unmodelled Bias

In this section experimentation has been done with nonlinear system with unmodelled bias situation. The truth has been considered with accelerometer and gyro biases but the filter process equation does not know that. So the truth-value of the variables has been generated using the equation (9.23) where as the filters are realized using the process equation X& = f ( X ) + w

(9.24)

The simulation parameters are chosen as described in section 9.8.1 except gyro and accelerometer biases. Gyro constant drift rate for truth are chosen as, ε e = ε n = ε u = 0.2 0 h −1 . The filters assume the gyro biases as zero. Truth acceleration biases are chosen as ∇ e = ∇ n = 2000µg . The filters assume zero accelerometer biases. Under this unmodelled bias condition EKF, ERSF and RSUKF have been simulated. For ERSF the value of risk sensitive parameter has been chosen as 8 and for RSUKF it was chosen as 0.0001. RMS errors have been calculated for east, north and azimuth misalignment angles from 100 Monte Carlo runs. RMSE for east north and up misalignment angles for EKF, ERSF and RSUKF are shown in figure 9.24-9.26. No improvement in estimation accuracy has been observed using ERSF and RSUKF in comparison to EKF in presence of unmodelled bias. It can also be observed from figure 9.26 that RMSE is wrongly at a value of 0.98 degree which is quite large. It is confirmed that this occurs due to unmodelled bias not due to nonlinearity. RMSE of east misalignment angle 12 EKF

10

ERSF RMSE (deg)

8

RSUKF

6 4 2 0 0 -2

50

100

150

200

250

300

Time (sec)

Fig 9.24 RMSE of east misalignment angle with unmodelled bias

180

Chapter 9

RMSE of north misalignement angle

12 EKF

10

ERSF

RMSE (deg)

8

RSUKF 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.25 RMSE of north misalignment angle with unmodelled bias RMSE of azimuth misalignment angle

12 EKF

10 RMSE (deg)

ERSF 8

RSUKF

6 4 2 0 0

Fig 9.26

50

100

150 Time(sec)

200

250

300

RMSE of azimuth misalignment angle with unmodelled bias

9.9 Discussion and Conclusion In this chapter a self-alignment problem for stationary base has been formulated independently where the misalignment angles between the INS frame and navigational frame is to be estimated. Considering biases in state variable the problem reduces exactly same as described by Chuanbin [Chuanbin 2004]. After verifying Chuanbin’s results for single run, RMS errors, which were not mentioned by Chuanbin, have been plotted in this chapter. The RMSE, shown here may be referred for completeness of the result presented in [Chuanbin 2004]. In Chuanbin it is wrongly concluded that in spite of unobservability the estimator is capable enough to estimate the misalignment angles. Further study made by present author reveals that the good performance shown by Chuanbin, in spite of unobservibity, is due to very small choice

181

Chapter 9

of initial accelerometer and gyro biases. It has been shown that if higher values of initial accelerometer and gyro biases are taken the estimator’s performance degrades rapidly. Results are provided for the system with exactly known biases for benchmarking the achievable lower limit. Also RMSE results for different misalignment angles are provided for different unmodelled gyro and accelerometer biases. Being motivated by the superiority of RSKF in presence of unmodelled bias, the results obtained from KF and RSKF has been compared. But no benefit has been observed. It has been argued that gyro measurements could be used as additional measurement to overcome the problems arises from unobservability. Again RSKF has been applied with this additional gyro measurements. But no improvement has been reported. With out small angle assumption the developed self-alignment problem is nonlinear. In that case misalignment angles have been estimated using extended Kalman filter (EKF) and unscented Kalman filter (UKF) for large initial condition. From the results it is seen that the RMSE of the misalignment angles are converging at zero value for both the filters. The same problem has been solved in presence of unmodelled gyro and accelerometer biases using extended Kalman filter (EKF), extended risk sensitive filter (ERSF) and risk sensitive unscented Kalman filter (RSUKF). The results obtained from nonlinear risk sensitive estimators are compared with EKF. But no improvement in estimation accuracy has been observed with nonlinear risk sensitive filtering in comparison to nonlinear risk neutral filtering. In this case RMSEs of azimuth misalignment for both the cases settle wrongly at unaccepted large value. It is concluded that this occurs due to unmodelled biases not due to nonlinearity. So in self alignment problem biases would be the main concern not nonlinearity. The additional measurement as considered in linear case should also be considered in nonlinear situation to increases observability. This remains in the scope for future work.

182

10 Discussions and Conclusions

10.1

Discussions

The main motivations behind this present work had been to develop and characterize novel nonlinear estimators. The objective of the thesis had been further narrowed down (see the introduction chapter) so as to obtain deeper insight and understanding about the properties of risk sensitive filtering, developing various advanced nonlinear risk sensitive filtering and investigate the scenario where risk sensitive filters can outperform existing filters. The work presented in this thesis has the following aspects: •

Development of various novel techniques for nonlinear filtering, leading to alternative formulation of risk sensitive filtering.

•

Exploring new properties and possible application areas, including aerospace tracking and navigation problems, of risk sensitive filters, using the algorithms.

•

Exploring the use of Cramer-Rao bound for tracking system preliminary design.

In the rest of this chapter we summarise the main findings of this thesis and also discuss about the scope for future work. Some of the concluding comments made in individual chapters would be repeated for the sake of completeness.

10.2 (i)

Conclusions An alternative formulation for posterior recursive solution for nonlinear risk sensitive estimation was proposed. The proof of the theorem has been derived through an alternative information state based induction method.

(ii)

Recursive equations for risk sensitive Kalman filter are alternatively derived using linear matrix inequality based approach.

(iii)

Robustness properties of risk sensitive estimator have been studied and some unknown properties have been found out.

(iv)

The central difference risk sensitive filter (CDRSF), which has been developed jointly with another scholar M.Srinivasan, is based on the central difference approximation. The risk sensitive unscented Kalman filter (RSUKF) based on the unscented transform, has been developed independently by the author. Both the scaled and unscaled versions of RSUKF have been derived. Both the CDRSF and RSUKF may be used for on board application due to its computational efficiency.

Chapter 10

Superiority of proposed filtering techniques with respect to ERSF has been demonstrated through case study (v)

Two new types of (approximate) risk sensitive filtering algorithms namely (i) central difference risk sensitive filter (CDRSF) and (ii) risk sensitive unscented Kalman filter (RSUKF) have been proposed. Both the scaled and unscaled version of RSUKF have been derived. These two filters may be used for on board application due to its computational efficiency. Superiority of proposed filtering techniques with respect to ERSF has been demonstrated through case study.

(vi)

Grid based method has been utilized to formulate risk sensitive filter for nonlinear non Gaussian cases. The adaptation of grid improves the numerical efficiency substantially. The proposed technique has been named as adaptive grid risk sensitive filtering (AGRSF). The algorithm has been found to be straight forward and simple to understand. Superiority of AGRSF, over existing ERSF has been demonstrated.

(vii)

The accuracy of AGRSF can be improved by choosing larger number of support points.

(viii)

Though the grid adaptation tends to reduce computation time, substantially, the AGRSF was found to be numerically less efficient than risk sensitive particle filter (RSPF) algorithm for the same level of accuracy. Though AGRSF is relatively numerically inefficient, it has been used successfully to validate risk sensitive particle filter (RSPF) algorithm.

(ix)

A novel technique based on particle filtering has been proposed for nonlinear risk sensitive estimation. The proposed method has been named as risk sensitive particle filter (RSPF). Both the prior and posterior versions of the algorithm have been developed. The superiority of RSPF over ERSF has been demonstrated through a highly nonlinear system.

(x)

The role of Cramer Rao lower bound (CRLB) for quantitative understanding of the influence of decision parameters on the tracking filter performance has been demonstrated through an example. The potential of using CRLB as a tool to aid guidance and tracking system configuration design has been established. The effects of different parameters such as (i) sampling time, (ii) measurement error covariance, (iii) process noise covariance, (iv) initial error covariance, (v) additional measurement; on estimation accuracy have been studied for bearing only tracking problem. In that problem the filtering performance in presence of eclipsing effect of

184

Chapter 10

the seeker has also been studied using CRLB. It has been concluded that the eclipsing effect on state estimation of target is not as severe as perceived by tracking community. (xi)

A self alignment problem for aerospace strapdown Inertial navigation system (INS) has been formulated. The problem is in general nonlinear in nature but under small angle assumption it became linear. Both the linear and nonlinear problems have been solved with risk sensitive and risk neutral filtering. For linear problem KF and RSKF have been applied and for nonlinear problem EKF, ERSF and RSUKF have been applied. The comparison has been made between risk neutral filter and risk sensitive filter under unmodelled bias condition. Under this condition no significant improvement with risk sensitive filtering has been found in comparison to risk neutral filter. Different aspects of the problem including observability, bias sensitivity and effect of additional measurement have been discussed. It has been concluded that the effect of gyro and accelerometer biases is severe in comparison to system nonlinearity.

10.3

Suggestion for Further Research

In the course of the present work, several ideas for further work have emerged. A significant subset of such ideas is presented here. (i)

Nonlinear risk sensitive filters need to be applied rigorously in target tracking and navigation problems. It is necessary to find out situations where the risk sensitive filters are the best choice. Reasons why risk sensitive filters are not exhibiting sufficient and substantial advantages need to be explored. Performance comparison with other robust filters, such as extended H ∞ filters may be useful.

(ii)

As discussed in chapter 5 it is necessary to explore the possibility of existence of square root risk sensitive unscented Kalman filter. Also the development of risk sensitive version of Gauss-Hermite filter (GHF) remains under the scope of future work.

(iii)

Risk sensitive particle filter formulated in this thesis used ordinary sequential sampling resampling method and prior proposal density function. Risk sensitive particle filter may also be formulated using different risk sensitive LRKF as proposal. Also the possibility of different other variant of risk sensitive particle filter such as Auxiliary risk sensitive particle filter Rao blackwellised risk sensitive particle filter need to be explored.

185

Chapter 10

(iv)

In the present work only EKF has been used for generating proposal for AGRSF. The use of other risk neutral filters such as UKF, CDF and also use of risk sensitive filters such as ERSF, RSUKF and CDRSF as proposal remains under the scope of future work.

(v)

Cramer Rao bound used in this thesis, are applicable for additive Gaussian noise. Similar relations for non additive and non Gaussian noise would be welcome. Again Cramer Rao lower bound is known to be a weaker bound. Tighter alternatives are available [Reece 2005] for example Bhattacharyya, Bobrovsky-Zakai and WeissWeinstein lower bounds. Recently these lower bounds are being applied for filtering problems. The advantages and applicability of those bounds in comparison to CRLB need to be figured out. Also lower bound for robust filtering would be appreciated.

(vi)

In linear Gaussian system where process and measurement noises are unknown or difficult to assume designers try to estimate the noise covariance at each step using different techniques (among them auto covariance least square technique is most popular), and then estimate the values of state variable using Kalman filter. This method is well known as adaptive Kalman filter. For nonlinear system adaptive unscented Kalman filter [Lee 2005] is available where process and measurement noise covariance have been estimated at each step then UKF is used to estimate state variables. In such scenarios where noise covariance is unknown risk sensitive nonlinear filtering proposed in this thesis may become competitor to nonlinear adaptive filtering methods. Even adaptive risk sensitive filters can also be formulated. Future research is necessary in this direction.

186

Bibliography V.J. Aidala, S.C. Nardone, "Biased Estimation Properties of the Pseudolinear Tracking Filter ", IEEE Transaction on Aerospace and Electronic System Vol AES-18, No.4 July 1982. V.J.Aidala, S.E.Hammel,"Utilization of Modified Polar Coordinates for Bearings[Aidala 1983] Only Tracking"IEEE Transaction on Automatic Control Vol Ac 28, No 3, March 1983. Jamshaid Ali, Fang Jiancheng, "Alignment of strapdown inertial navigation [Ali 2004] system: A literature survey spanned over the last 14 years", School of instrumentation Science and Optoelectronics Engineering, Beijing University of Aeronautics and Astronautics, Beijing 100083, China, 2004. [Anderson 1979] B.D.O.Anderson, J.B.Moore, "Optimal Filtering", Prentice Hall Inc, Englewood cliffs,New Jersey, 1979 [Andrieu 2003] C. Andrieu, M. Davy, and A. Doucet, Efficient Particle Filtering for Jump Markov Systems.Application to Time-Varying Autoregressions", IEEE Transaction on Signal Processing, Vol. 51, No. 7, March 2003. [Andriseu 2001] C.Andriseu, N.D.Feritas, A Doucet, “Rao-Blackwellised Particle Filtering Via Data Augmentation",Advances in Neural Information Processing Systems (NIPS13), 2001. [Angelavo 2003] D Angelova, I Simeonova and T Semerdjiev, "Monte Carlo Algorithm for Ballistic Object Tracking with Uncertain Drag Parameter",International conference on large-scale scientific computing No 4, Sozopol, Bulgarie 2003. [Arulampalam S.Arulampalam, S.Maskell, N.Gordon, T.Clapp,"A Tutorial on Particle Filters for Online Nonlinear / Nongaussian Baysian Tracking", IEEE transaction on signal 2002] processing, Vol 50, issue 2, Feb 2002. [Banavar 1994] R.N.Banavar, J.L.Spere,"Risk Sensitive Estimation and a Differential Game",IEEE Transaction on Automatic Control, Vol. 39, No.9, June 1994 [Banavar 1998] R.N.Banavar, J.L.Spere,"Properties of Risk Sensitive Filters/ estimators", IEE Proc-Control Theory Appl Vol 145, No 1, January 1998. [Banbrook 1970] Banbrook H.W, Halamandaris H, "Navigation at Sea using the invariants form of Kalman Filtering ", in Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970. [Bar Shalom Bar-Shalom Y, Li X, Estimation and Tracking- Principles, Techniques and Software, Artech House, 1993. 1993] [Barbour 2003] N. Barbour, "Advances in Navigation Sensors and Integration Technology", NATO/RTO Educational Notes, EN-SET-064, Oct. 2003 [Barham 1970] Barham,P.M.,Manville P,"Application of Kalman Filtering to Baro/Inertial Height Systems", Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970 Helen Basil, “Studies on a low cost integrated navigation system using MEMS[Basil 2005] INS and GPS with adaptive and constant gain Kalman filters”, MSc Thesis, Department of Aerospace Engineering, Indian Institute of Science, Bangalore, India, February 2005. Battin R.H., Levine G.M., "Application of Kalman Filtering Techniques to the [Batin1970] Apollo Program",in " Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970 [Aidala 1982]

Bibliography

[Bellantoni1 970] Bellantoni J.F., "Some Applications of Kalman Filtering in Space Guidance" Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970. [Bergman 1997] N. Bergman, L. Ljung , F.Gustafsson,"Point-mass filter and Cramer-Rao bound for Terrain- Aided Navigation",Proceedings of the 36th Conference on Decision & Control San Diego, Califomla USA December 1997. [Bergman 1999 N. Bergman, L. Ljung, and F.Gustafsson, "Terrain Navigation Using Bayesian Statistics",IEEE control system magazine, Vol 19, issue 3, Jun 1999. a] [Bergman 1999 Niclas Bergman,"Recursive Bayesian Estimation Navigation and Tracking Applications", Phd thesis, Department of Electrical Engineering, Linkoping 1999. b] Matthew Barth and Jay A. Farrell, "The Global Positioning System and Inertial [Barth 1999] Navigation", McGraw Hill, January 1999 [Bhaumik 2005a] S.Bhaumik, S Sadhu, T.K.Ghoshal, "Risk Sensitive Estimators for Inacurately Modelled system", IEEE Indicon 2005 Conference Chennai India, 11-13 Dec 2005. [Bhaumik 2005b] S.Bhaumik, M. Srinivasan, S Sadhu, T.K.Ghoshal,"Adaptive Grid Solution of Risk Sensitive Estimator Problems",IEEE Indicon 2005 Conference Chennai India, 11-13 Dec 2005. [Bhaumik 2006a] S. Bhaumik, M. Srinivasan, S. Sadhu, T. K. Ghoshal ,"A Risk Sensitive Estimator for Nonlinear Problems Using the Adaptive Grid Method, Nonlinear Statistical Signal Processing Workshop 2006, University of Cambridge, U.K. [Bhaumik 2006b] S.Bhaumik, S.Sadhu, and T.K.Ghoshal, “Alternative Formulation for Risk Sensitive Particle Filter (Posterior)”, IEEE INDICON, September, 2006. [Bhaumik 2006c] S.Bhaumik, S.Sadhu, T.K.Ghoshal,"Parametric Performance Analysis of Tracking System Using Posterior Cramer Rao Lower Bounds", Journal of the institution of Engineers (India), Volume 87, december 2006. [Bhaumik 2007] Shovan Bhaumik, Smita Sadhu, M.Srinivasan and T.K.Ghoshal, “Adaptive grid method for nonlinear risk sensitive estimation problem” submitted to International journal of modelling and simulation. [Bhaumik 2008] S.Bhaumik, S.Sadhu, T.K.Ghoshal “Risk Sensitive Formulation of Unscented Kalman Filter”, accepted for publication, IET Control Theory & Applications, 2008. B.Boberg and S.L. Wirkander,"Integrating GPS and INS: Comparing the Kalman [Boberg 2002] Estimator and Particle Estimator", 7th International Conference on Control, Automation, Robotics And Visioo, Dec 2002. Rene K. Boel, Matthew R. James, and Ian R. Petersen,"Robustness and Risk[Boel 2002] Sensitive Filtering", IEEE Transaction on Automatic Control, Vol. 47, No. 3, March 2002. Gilbert Strang, K. Borre, "Linear Algebra, Geodesy, and GPS", Wellesley [Borre 1997] Cambridge, October S.Boyd, L.El Ghaoui, E. Feron, and V. Balakrishnan, "Linear Matrix Inequalities [Boyd 1994] in System and Control Theory", Society for Industrial and Applied Mathematics, Philadelphia, 1994. Mark Briers, Simon R. Maskell, Robert Wright, "A Rao-Blackwellised Unscented [Briers 2003] Kalman Filter", 2003 0 lSlF, 2003 [Britting 1968] Kenneth R. Britting, "Analysis of Space Stabilised Inertial Navigation Systems", MIT Experimental Astronomy Laboratory, Report RE-35, January 1968. Britting 1969 "Error Analysis of Strapdown and local Level Inertial systems [Britting1969] Which Compute Geographic Coordinates". MIT report RE52, NASA 19700007517_1970007517. 188

Bibliography

Brown and Hwang, "Introduction to random signal and applied Kalman filtering", Wiley 1996. Alison Brown and Dan Sullivan, "Precision Kinematic Alignment Using a Low[Brown 2002] Cost GPS/INS System", Proceedings of ION GPS, Portland, Oregon, September 2002. Marcelo G. S. Bruno and Anton Pavlov, "Improved Particle Filters for Ballistic [Bruno 2004] Target Tracking" IEEE International Conference on Acoustics, Speech, and Signal Processing, 2004. Marcelo G. S. Bruno and Anton Pavlov, "A Density Assisted Particle filter [Bruno 2005] Algorithm for Target Tracking with Unknown Ballistic Coefficient", IEEE International Conference on Acoustics, Speech, and Signal Processing, 2005. [Buchler 2006] Robert J. Buchler, Peter Kyriacou, Daniel A. Tazartes, "Attitude Alignment of a Slave Inertial Measurement System", United States Patent, Patent No. US 7133776 B2, Nov. 7, 2006. R.S.Bucy, K.D.Senne, “Digital synthesis of nonlinear filters”, Automatica, Vol. 7, [Bucy1971] pp 287-298, 1971. J-Pierre. LE Cadre, O T Mois, "Bearings-Only Tracking for Maneuvering [Cadre 1998] Sources", IEEE Transaction on Aerospace and Electronics Vol. 34, No. 1 January 1998. [Cardillo 1999] G.P.Cardillo, A.V.Mrstik, T.Plambeck, "A Track Filter Reentry objects with uncertain drag", IEEE Transaction on Aerospace and Electronic System Vol 35, No2, April 1999. [Carpenter 1999] J carpenter, P Cliffford, P Fearnhead, "Improved particle Filter for nonlinear Problem", IEE Proc Radar sonar Navigation Vol 145, No 1 February 1999. Y.T Chan, T.A.Rea, “Passive Tracking Scheme for a Single Stationary Observer”, [Chan 2002] IEEE Transactions on Aerospace and Electronic Systems, Vol 38, No.3, 2002 pp 1046-1054. [Chatterji 1997] G. B. Chatterji, P. K. Menon, B. Sridhar, "GPS Machine Vision Navigation System for Aircraft", IEEE Transaction on Aerospace and Electronic systems Vol 33, no 3 July 1997. Zhe Chen, “Bayesian filtering: from Kalman filters to particle filters, and beyond”, [Chen2003] Technical report, Adaptive Systems Lab,. McMaster University, 2003. Y.Cheng; J. L. Crassidis, "Particle Filtering for Sequential Spacecraft Attitude [Cheng 2004] Estimation", AIAA Guidance, Navigation, and Control Conference, Providence, RI, Aug. 2004. Y.T. Chiang, L.S. Wang, F.R. Chang and H.M. Peng, "Constrained filtering [Chiang 2002] method for attitude determination using GPS and gyro", IEE Proc Radar Sonar Navigation vol 149, No 5, October 2002. [Chuanbin 2004] Zhang Chuanbin, Tian Weifeng and Jin Zhihua, "A novel method improving the alignment accuracy of a strapdown inertial navigation system on a stationary base", Meas. Sci. Technol. V 15, April 2004, page 765-769. D.Y. Chung, J.G. Lee, C.G. Park and H.W. Park, "Strapdown INS Error Model for [Chung 1996] Multiposition Alignment", IEEE Transactions on Aerospace and Electronic Systems, Vol. 32, No. 4, 1996, pp. 1362-1366. J. M. C. Clark R. B. Vinter M. M. Yaqoob, "The Shifted Rayleigh Filter for [Clark 2005] Bearings Only Tracking", 7th International Conference on Information Fusion 2005 [Collings 1994] lain B. Colhgs,Matthew R. James,John B. Moore,"An Information-State App roach to Linear/Risk-Sensitive /Quadra/Gaussian Control", Proceedings of the 33rd Conference on Decision and Control - December 1994. [Brown 1996]

189

Bibliography

[Collings 1996]

[Cooperman 2002] [Coutinho 2003]

[Crassidis 1997]

[Crassidis 2000]

[Crassidis 2005]

[Crassidis 2003]

[Crisan 2002]

[Crocker1970]

[Dey 1995] [Dey 1997]

[Dey 2001]

[Dey 2003]

[Dezert 1999] [Dissanayake 2001]

[Dong 2006]

[Doraiswami 1996]

Iain B. Collingsy Matthew R. Jamesy John B. Moorey, "An Information-State Approach to Risk-Sensitive Tracking Problems", Journal of Mathematical Systems, Estimation, and Control, Vol. 6, No. 3, 1996. Robert L. Cooperman, "Tactical Ballistic Missile Tracking using the Interacting Multiple Model Algorithm", Proceedings of the Fifth International Conference on Information fusion 2002. D. E. Coutinho, K. A. Barbosa, A.Trofino, Carlos E. de Souza, "Linear Hinf Filter Design for a Class of Uncertain Nonlinear Systems", Proceedings of the 42nd IEEE Conference on Decision and Control, December 2003. John L. Crassidis; F. Landis Markley, "Predictive Filtering for Attitude Estimation Without Rate Sensors", AIAA Journal of Guidance, Control, and Dynamics, Vol. 20, No. 3, May-June 1997, pp. 522-527. John L. Crassidis, Roberto Alonsoy,John L. Junkinsz, "Optimal Attitude and Position Determination from Line-of-Sight Measurements", the Journal of the Astronautical Sciences, Vol. 48, Nos. 2-3, April-Sept. 2000, pp. 391-408 . John L. Crassidis, "Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation", AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, Aug. 2005. J.L. Crassidis, F. LandisMarkley, "Unscented Filtering for Spacecraft Attitude Estimation", Journal of Guidance Control and Dynamics, Vol. 26, No. 4, JulyAugust 2003. Dan Crisan and Arnaud Doucet, "A Survey of Convergence Results on Particle Filtering Methods for Practitioners", IEEE Transaction on Signal Processing, Vol. 50, No. 3, March 2002. Crocker E.D., Rabins L.," Application of Kalman Filtering Techniques to Strapdown System Initial Alignment", in " Theory and Applications of Kalman Filtering" (Agardograph 139), C.T.Leondes (Ed).AGARD 1970 S. Dey, J.B. Moore, "Risk sensitive filtering and smoothing for hidden Markov models", Systems & Control Letters, Vol 25, issue 5, 1995 Subhrakanti Dey and John B. Moore, "Risk-Sensitive Filtering and Smoothing via Reference Probability Methods", IEEE Transaction on Automatic Control Vol 42, No.11, November 1997. Subhrakanti Dey and Charalambos D Charalambous, “ Discrete time risk sensitive filters with non gaussian initial conditions and their ergodic properties” Asian Journal of Control, vol. 3, no.4, pp. 262-271, December 2001. Subhrakanti Dey and John B. Moore, "Finite-Dimensional Risk-Sensitive Filters and Smoothers for Discrete-Time Nonlinear Systems", IEEE Transaction on Automatic Control Vol 44, No.6, June 1999. Jean Dezert, "Improvement of Strapdown Inertial Navigation Using PDAF", IEEE Transaction on Aerospace and Electronic System, Vol 35 No3 April 1999. G. Dissanayake, S.Sukkarieh, E Nebot, H. D.Whyte, "The Aiding of a Low-Cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications", IEEE Transaction on Robotics and Automation, Vol 17, no 5, October 2001. Zhe Dong and Zheng You, "Finite-Horizon Robust Kalman Filtering for Uncertain Discrete Time-Varying Systems With Uncertain-Covariance White noises", IEEE Signal Processing Letters, Vol 13, No 8, Aug2006. D Doraiswami, "A Noven Kalman Filter Based Navigation Using Beacons", IEEE Transaction on Aerospace and Electronic System Vol 32 No2 April 1996.

190

Bibliography

Peter C. Doerschuk, "Cramer-Rao Bounds for Discrete-Time Nonlinear Filtering Problems", IEEE Transaction On Automatic Control, Vol 40. No 8. August 1995. A.Doucet, "On Sequential Monte Carlo Sampling Methods for Bayesian Filtering", Technical Report CUED/F-INFENG/TR310, 1998. A Doucet, S.Godsill, C. Andrieu, "On Sequential Monte Carlo Sampling Methods [Doucet 2000] for Bayesian Filtering", statistics and computing, vol 10, pp 197-208, 2000. [Doucet 2001 a] A Doucet, Nando De Freitas, N Gordon, "Sequential monte carlo methods in practice", Springer, 2001. [Doucet 2001 b] A.Doucet, N.J. Gordon, and V.Krishnamurthy, "Particle Filters for State Estimation of Jump Markov Linear Systems", IEEE Transaction on Signal Processing, Vol. 49, No. 3, March 2001 [Eduardo 2006] Nebot Eduardo, Durrant-Whyte Hugh, "Initial Calibration and Alignment of an Inertial Navigation", 4th Annual Conference on Mechatronics and Machine Vision in Practice, IEEE Computer Society, pp 175-180, 1997. R.J.Elliott, J.B.Moore, S.Dey, "Risk Sensitive Maximum Likelihood Sequence [Elliott 1996] Estimation", IEEE Transaction on Circuits and systems I Fundamental Theory and Application, vol 43, 1996. [El-Sheimy 2004] N. El-Sheimy, S. Nassar and A. Noureldin, "Wavelet De-noising for IMU Alignment", IEEE Aerospace and Electronic Systems Magazine, October 2004, pp. 32-39. P.K.Enge, F.B.Vicksell, R.B.Goddard, "Combining Pseudoranges from GPS and [Enge1990] Loran C for Air Navigation", IEEE Symposium on Position Location and Navigation, 1990. [Erzberger 1967] Heinx Erzberger, " Application of Kalman filtering to error correction of inertial navigators", NASA Technical note D-3874, February 1967. Jiang Cheng Fang, De Jun Wan, "A fast initial alignment method for strapdown [Fang 1996] inertial navigation system on stationary base", IEEE Transactions on aerospace and electronic systems VOL. 32, NO. 4, October 1996. Alfonso Farina, "Target tracking with bearings Only measurements" Signal [Farina 1999] Processing Vol 78, 1999. [Farina 2002a] A. Farina, D. Benvenuti, B. Ristic, "Estimation accuracy of a landing point of a ballistic target", Proceedings of the Fifth International Conference on Information Fusion, 2002. [Farina 2002b] A. Farina, B. Ristic, D. Benvenuti, "Tracking a Ballistic Target:Comparison of Several Nonlinear Filters" IEEE Transaction on Aerospace and Electronic Systems Vol. 38, No. 3 July 2002. [Farina 2002c] A.Farina, M.G.Del Gaudio, U.D.Elia, S.Immediata, L.Ortenzi, "Detection and Tracking of Ballistic Target", the IEEE Radar Conference, Proceedings 2004. Jay Farrel, Matthew Barth, "The Global Positioning System and inertial [Farrel 1998] Navigation", McGraw Hill companies, Inc, 1998. Gang Feng, "Robust Filtering Design of Piecewise Discrete Time Linear [Feng 2005] Systems", IEEE Transaction On Signal Processing, Vol 53, No2, Feb 2005. Jason Ford, " Risk Sensitive Filtering and Parameter Estimation" Technical report [Ford 1999] Weapons Systems Division Aeronautical and Maritime Research Laboratory, January, 1999. [Frykman 2003] Petter Frykman, "Applied particle filters in integrated aircraft navigation", Master's thesis, Automatic Control and Communication Systems, Dept. of Electrical Engineering, LinkÄopings universitet, 28th April 2003 M. Fujitat, A. Mamyamat, T. Taniguchit and K. Uchidai, "Finite Horizon Discrete[Fujitat 1993] Time H, Filter with Application to an Active Vision System", Proceeding. of the [Doreschuk 1995] [Doucet 1998]

191

Bibliography

32nd Conference on Decision and Control San Antonio, Texaa, December 1993. Wei Gao, Yueyang Ben, Feng Sun and Bo Xu, "Performance Comparison of Two Filtering Approaches for INS Rapid Transfer Alignment", Proceedings of IEEE international conference on mechatronics and automation, Aug 5-8, 2007, China. [Gerome 1999] J. C. Geromel, J. Bernussou, G. Garcia, and M.C.D.Oliveria, "H2 and H∞ Robust Filtering for Discrete Time Linear Systems", SIAM J. Control & Optimization , Vol. 38, No. 5, pp. 1353–1368. [Geromel 2000] J. C. Geromel, J. Bernussou, G. Garica and M. C. De Oliveira, “H2 and H∞ robust filtering for discrete time linear system”, SIAM J. Control Optimization, Vol. 38, No. 5, pp. 1353–1368 2000. Saurabh Godha, "Performance evaluation of low cost MEMS-based IMU [Godha 2006] integrated with GPS for land vehicle navigation application", MSc Thesis, Department of geomatics engineering, University of Calgary, Calgary, Alberta, Canada, February 2006. [Gordon 1993] N.J. Gordon, D.J. Salmond, A.F.M. Smith, "Novel approach to nonlinear/nonGaussian Bayesian state estimation IEE Proceedings-F, Vol. 140, No. 2, April 1993. M.S.Grewal, R.S.Miyasako and J.M. Smith, "Application of fixed point smoothing [Grewal 1988] to the calibration alignment and navigation data of inertial navigation systems", IEEE Position Location and Navigation Symposium, 1988. M.S.Grewal, V.D. Henderson, R.S. Miyasako, "Application of Kalman Filtering to [Grewal 1990] the calibration and Alignment of Inertial Navigation Systems", Proceedings of the 29th. Conference. on Decision and Control. Honolulu, December. 1990. M.S.Grewal, and P.S.Andrews, “Kalman Filtering: Theory and practice using [Grewal 2001] Matlab”, 2nd edition, John Wiley & Sons, 2001. [Grimble 1990] M. J. GRIMBLE and A. El Sayed, "Solution of the Hinf Optimal Linear Filtering Problem for Discrete-Time Systems", IEEE Transaction On Acoustics, Speech and Signal Processing, Vol 38. No 7, July 1990. Paul D. Groves, "Optimising the Transfer Alignment of Weapon INS", The journal [Groves 2003] of Navigation, Vol. 56, pp. 323-335, 2003. Clay E. Hagan, Christopher S. Lofts, "Accelerometer Based Alignment Transfer", [Hagan 1992] IEEE, Position Location and Navigation Symposium, 1992. Record. 500 Years After Columbus - Navigation Challenges of Tomorrow. IEEE PLANS '92, 1992 [Halamandaris Halamandaris H., Ozdes D. "A Kalman Filter Augmented Marine Navigation System" Theory and Applications of Kalman Filtering" (Agardograph 1970] 139),C.T.Leondes (Ed).AGARD 1970. Peter D. Hanlon and Peter S Maybeck, "Characterization of Kalman Filter [Hanlon 2000] Residual in the Presence of Mismodelling", IEEE Transaction on Aerospace and Electronic System Vol 36 No1 April 2000. Yanling Hao, Zhilan Xiong, Wei Wang and Feng Sun, "Rapid Transfer Alignment [Hao 2006] Based on Unscented Kalman Filter", Proceedings of American control conference USA June 2006. B. Hassibi, Ali H. Sayed, T.Kailath, "Linear Estimation in Krein Spaces- Part I&II [Hassibi 1996] : Theory", IEEE Transaction On Automatic Control, Vol. 41, No. 1, Jan 1996 [Helmick 1993] R.E.Helmick, T.R.Rice, "Removal of alignment errors in an integrated system of two 3-D Sensors", IEEE Transactions on Aerospace and Electronic Systems, Vol. 29, No. 4, 1993, pp. 1333-1348 M. Hernandez, B.Ristic, A. Farina, and L.Timmoneri, "A Comparison of Two [Hernandez Cramér–Rao Bounds for Nonlinear Filtering with Pd < 1", IEEE Transaction on 2004] [Gao 2007]

192

Bibliography

Signal Processing, Vol. 52, No. 9, September 2004. [Hewitson 2003] S.A. Hewitson, J. Wang, A.H.W. Kearsley, "Performance evaluation of inertial navigation systems for surveying", The 6th International Symposium on Satellite Navigation Technology Including Mobile Positioning & Location Services (SatNav 2003), Melbourne, Australia, 22-25 July 2003 C.Hide, T.Moore and M.Smith, "Adaptive Kalman Filtering for Low-cost [Hide 2003] INS/GPS", The Journal of Navigation, Vol, 56, 2003, pp143-152. K. C. Ho and I Z Chan, "An Unbiased Estimator for Bearing Only Tracking and [Ho 2003] Doppler Bearing Tracking", IEEE International conference on Acoustics, Speech, and Signal Processing, 2003. H.S. Hong, J.G. Lee and C.G. Park, "Performance improvement of in-flight [Hong 2003] alignment for autonomous vehicle under large initial heading error", IEE Proceedings - Radar Sonar Navigation, Vol. 151, No. 1, February 2004. Daiwen Hou, Fuliang Yin and Liyan Zhang, "Joint Sigma-Point Kalman Filter [Hou 2006] Based Bearing-Only Tracking "Proceedings of the 6th World Congress on Intelligent Control and Automation, June 21 - 23, Dalian, China, 2006. Li-Sheng Hu, James Lam, Yong-Yan Cao, and Hui-He Shao, "A Linear Matrix [Hu 2003] Inequality (LMI) Approach to Robust H2 Sampled-Data Control for Linear Uncertain Systems", IEEE Transaction on Systems man and cybernetics -P Art B:Cybernetics Vol 33 No1 Feb 2003. Yufei Huang, and Petar M. Djuric, "A Hybrid Importance Function for Particle [Huang 2004] Filtering", IEEE Signal Processing Letters , Vol. 11, No. 3, March 2004. Carine Hue, Jean-Pierre Le Cadre, Patrick Pérez, "Sequential Monte Carlo [Hue 2002a] Methods for Multiple Target Tracking and Data Fusion", IEEE Transaction on Signal Processing, Vol. 50, No. 2, February 2002. C.Hue, J.P.Le Cadre and P Perez, "Tracking Multiple Objects with particle [Hue 2002b] filtering", IEEE Transaction on aerospace system vol 38, No 3, 2002. Andreas Huster, Eric W. Frew and Stephen M. Rock, "Relative Position [Huster 2002] Estimation for AUVs by Fusing Bearing and Inertial Rate Sensor Measurements", Oceans 2002 Conference, Biloxi, MS, pp.1857-1864, October, 2002. G.Hyslop, D.Gerth, J.Kraemer, "GPS/INS Integration On the Standoff Land [Hyslop 1990] Attack Missile (SLAM)", Aerospace and Electronic Systems Magazine, IEEE, 1990. G.Hyslop, D.Gerth, J.Kraemer, "GPS/INS Integration On the Standoff Land [Hyslop1990] Attack Missile (SLAM)", Aerospace and Electronic Systems Magazine, IEEE, 1990. A.G.iremus, A. Doucety, A.C.Escher, J.Y.Tourneret, "Nonlinear Filtering [Iremus 2004] approach for INS/GPS Integration", European Signal Processing Conference 2004. K. Ito and K. Xiong, "Gaussian Filters for Nonlinear Filtering Problems", IEEE [Ito 2000] Transaction on Automatic Control Vol 45 No 5, May 2000 Joseph Ben-Jaacov, "Method for transfer alignment of an inertial measurement [Jaacov 2003] unit in the presence of unknown aircraft measurements delays", United States Patent, Patent No US 6556895 B2, Apr. 29, 2003 C.-W.Jang, J.-C.Juang and F.-C.Kung, "Adaptive fault detection in real-time GPS [Jang 2000] positioning", IEE Proc-Radar Sonar Navigation., Vol. 147, No. 5, October 2000 [Jauffret 1996] C.Jauffret, D.Pillon, "Observability in passive Target Motion analysis" IEEE Transaction on aerospace and electronics systems Vol 32 No 4 Oct 1996. M. Jayakumar and Ravi N. Banavar, "Risk-Sensitive Filters for Recursive [Jayakumar Estimation of Motion From Images ", IEEE Transaction on Pattern Analysis and 1998]

193

This thesis is submitted to the Jadavpur University in partial fulfillment of the requirements for the degree of Doctor of Philosophy

By SHOVAN BHAUMIK

Department of Electrical Engineering Jadavpur University Kolkata, India 2008

Names, Designations and Affiliations of Supervisors 1. Dr. SMITA SADHU Reader Department of Electrical Engineering Jadavpur University Kolkata-700 032 India

2. Dr. TAPAN KUMAR GHOSHAL Professor Department of Electrical Engineering Jadavpur University Kolkata-700 032 India

Certificate from Supervisors This is to certify that the candidate Shri SHOVAN BHAUMIK was registered on …………………., and fulfilled the residence and other requirements for submitting the thesis for the Ph.D. (Engg) degree of this University as per rules. The thesis is a genuine piece of research carried out by the candidate under our supervision and neither his thesis nor any part of part of the thesis has been submitted for any degree / diploma or any other academic award anywhere before. In our opinion this is a fit piece of work for submission for the Ph.D. degree.

1. -------------------------------------------Signature of the supervisor and date with Office Seal

2.--------------------------------------Signature of the supervisor and date with Office Seal

ABSTRACT This dissertation deals with nonlinear filtering and estimation methods for systems with parametric uncertainties. The work focuses primarily on risk-sensitive estimators, which are known to be robust in the presence of uncertainties in the system parameters. Risk sensitive filters (RSF), which are based on risk-sensitive control law, minimize the expected value of an exponential of a convex function of the estimation error. A designer-chosen parameter, called the risk-sensitive parameter, provides a tool for design trade-off between the filtering performance for the nominal model and the robustness to model uncertainty. Though the work was motivated by the need to evolve improved methods of filtering for aerospace systems, the scope of the thesis has been broadened to include non-aerospace problems as well. Thus the dissertation deals with aerospace problems and other nonlinear problems with uncertain system model, un-modeled biases, unknown inputs and non-nominal noise covariance. Though existing literature on risk-sensitive filters claims that these filters are robust, the implementation of these filters for nonlinear problems had been practically impossible as these filters admit closed form expressions only for a very limited class of models including finite statespace Markov chains and linear Gaussian models. Until very recently, the only method of implementation of risk-sensitive filters for nonlinear problems was by using the extended Risk sensitive filter (ERSF), which uses an EKF-based linearization method. However, all the shortcomings of the EKF are inherited by the ERSF. In this dissertation, some novel filters have been proposed, which significantly extend the range of applications of risk-sensitive techniques to nonlinear, non-Gaussian systems. The proposed filters would enable one to study the properties of risk-sensitive filters and to explore the possibility of application of RSFs for a wide variety of problems including aerospace problems like target tracking and navigation. Some simple test problems have been selected for characterizing the existing filters and testing the applicability of developed filters. Though it cannot be claimed that RSFs have been found to be superior to other existing (risk-neutral) nonlinear filters, this dissertation makes an effort to summarize the results of comparison of the various filters for each chosen application area.

i

The salient contributions of this research work are as follows: •

Development of Risk sensitive unscented Kalman filter (RSUKF), which is a novel method for non-linear risk-sensitive estimation based on the unscented Kalman filter. (Paper accepted for publication Applications, UK, 2008.)

•

in

IET

Control

Theory

and

Development of Central difference risk-sensitive filter (CDRSF), which is a novel numerically efficient algorithm for risk-sensitive filters of nonlinear plants, using central difference approximation. (Paper published in IEEE Signal Processing Letters, 2007.)

•

Development of Risk sensitive particle filter (RSPF), which is a novel particle implementation of Risk-sensitive filters (RSF) for nonlinear, non-Gaussian statespace models based on a probabilistic re-interpretation of the RSF recursions. (Paper accepted for publication in Elsevier Journal of Signal Processing and available online since 1st October, 2008. One IEEE INDICON conference papers published in 2006 have been cited and critically appreciated by international researchers.)

•

Development of Adaptive grid risk sensitive filter (AGRSF), which is a novel adaptive grid based method employing the point-mass approximation for computation of risk-sensitive state estimates in non-linear non-Gaussian problems. (Work presented and published in the proceedings of the IEEE Nonlinear Statistical Signal Processing Workshop, Cambridge, UK, 2006 and IEEE INDICON, 2005.)

•

Formulation of an approach for preliminary design of guidance system for homing missiles by using the Cramer Rao Bound (CRB) to provide a quantitative understanding of the influence of model parameters and instrumentation/signal processing capabilities of the tracking filter performance, without going into the specifics of filter design and elaborate Monte Carlo simulation for performance analysis. (Work presented and published in the proceedings of the IEEE International Symposium on Signal Processing and Information Technology (ISSPIT), Rome, Italy, 2004, paper published in the Journal of the Institution of Engineers, India, 2006.)

•

Comparative performance analysis of existing and proposed filters for the Bearing only tracking problem with different measurement models.

•

Application of the filters to a self-alignment problem for Inertial Navigation System.

ii

ACKNOWLEDGEMENTS My supervisors Dr. Smita Sadhu and Professor Tapan Kumar Ghoshal deserve my deepest gratitude. This thesis is, to a large extent, a result of their skillful meticulous guidance and encouraging support to my work. I would also like to acknowledge them for their valuable comments suggestions during work and review of manuscript at different stages that have lifted the thesis to a level I never would have reached on my own. I would like to thank my co-worker M Srinivasan for research cooperation, comparing simulation results and fruitful discussions. The work has been partly sponsored by Aeronautical Research and Development Board (India) and Council of Scientific & Industrial Research (CSIR). I thankfully acknowledge their support. I have great pleasure in expressing my thanks to, Prof. Shyamal Kumar Goswami, Prof. Kalyan Kumar Dutta and Prof Samar Bhattacharya, Jadavpur University, for their encouragement. I also wish to thank the successive Heads of Electrical Engineering and the Coordinators of Center for Knowledge Based System for providing research infrastructure. Lastly I would like to express my heart-felt gratitude to my parents, family members and friends for their love, affection, support and encouragement.

Shovan Bhaumik Jadavpur University, October 2008

iii

LIST OF PUBLICATION International Journal (Published/Communicated) 1. Shovan Bhaumik, Smita Sadhu, T.K.Ghoshal “Risk Sensitive Formulation of Unscented Kalman Filter”, accepted for publication to IET Control Theory & Applications, 2008. 2. Smita Sadhu, M. Srinivasan, S. Bhaumik, T. K. Ghoshal, Central Difference Formulation of Risk-Sensitive Filter, IEEE Signal Processing Letters, Vol 14, issue 6, pp 421-424, June 2007. 3. Smita Sadhu, Shovan Bhaumik, Arnaud Doucet, T.K.Ghoshal “Risk Sensitive Filtering by Particle Methods” accepted for publication in Elsevier Journal of Signal Processing, 2008. 4. S.Bhaumik, S.Sadhu, M.Srinivasan, T.K.Ghoshal, “Adaptive grid method for nonlinear risk sensitive estimation problems”, submitted to International journal of modelling and simulation.

National Journal (Published) 5. S.Bhaumik, S.Sadhu and T.K. Ghoshal “Parametric Performance Analysis of Tracking System Using Posterior Cramer Rao Lower Bounds” Journal of Institution of Engineers (India), electrical engineering division, issue 3, vol 47, December 2006.

Papers Presented/Published in International Conferences 6. S.Bhaumik, M. Srinivasan, S. Sadhu, T.K.ghoshal “A Risk Sensitive Estimator for Nonlinear Problems using the Adaptive Grid Method” in Proceedings of Nonlinear Statistical Signal Processing Workshop- University of Cambridge, UK 2006. 7. S. Sadhu, S. Bhaumik, and Prof. T. K. Ghoshal “Homing Guidance Configuration using Cramer Rao Bounds”- in Proceedings of 4th IEEE international symposium on signal processing and information technology, Rome, Italy, pp 437-440, Dec 2004. 8. M.Srinivasan, S.Bhuamik, S.Sadhu, T.K.Ghoshal, “Exo-Atmospheric IR Tracking of Ballistic Objects”, in Proceedings of International Conference of IEEE INDICON 2005, IIT Madras, Chennai, India, Dec 12-13, 2005

iv

9. S.Bhaumik, S. Sadhu, T.K.Ghoshal “Risk Sensitive Estimators for Inaccurately Modelled Systems”, in Proceedings of International Conference of IEEE INDICON’2005, IIT Madras, Chennai, India, Dec 12-13, 2005. 10. S.Bhuamik, M.Srinivasan, S.Sadhu, T.K.Ghoshal, “Adaptive Grid Solution of Risk Sensitive Estimator Problems”, in Proceedings of International Conference of IEEE INDICON 2005, IIT Madras, Chennai, India, Dec 12-13, 2005. 11. S.Bhaumik, S.Sadhu, and T.K.Ghoshal, “Alternative Formulation for Risk Sensitive Particle Filter (Posterior)”, in Proceedings of International Conference of IEEE INDICON’2006, New Delhi, India, Sep 15-17, 2006.

Papers Presented/Published in National Conferences 12. S. Bhaumik, S. Sadhu “Auxiliary Particle filter for Bearing Only Tracking” in proceedings of 28th national systems Conference, Vellore institute of technology, pp 263-267, Dec 2004. 13. S.Bhaumik, S. Sadhu and T.K.ghoshal “Formulation of LMI Based Risk Sensitive Estimator”, in proceedings of International Conference on “Emerging Trends in Electrical Engineering” Kolkata, January 12-14, 2007.

v

CONTENTS

Abstract Acknowledgement List of Publication Content List of Figures List of Table Chapter 1 Introduction 1.1Motivation 1.2Thesis Objective 1.3 Background on aerospace Problem Domain 1.3.1 Target Tracking 1.3.2 Navigation and Its Use 1.4Background in Solution Domain 1.4.1 Background on Filters and State Estimators 1.5Problem Statement and Scope 1.6The Approach and Methodology 1.7Contribution 1.7.1 Publication Generated from this Work 1.7.2 Credits to Co-Workers 1.8Organization and Content of the Thesis Chapter 2 Literature Survey 2.1Introduction to Literature Survey 2.2Problem Domain Literature Survey 2.2.1 Tracking of Maneuvering Target 2.2.1.1 Bearing Only Tracking 2.2.1.2 Ballistic Object Tracking 2.2.2 Navigation 2.2.2.1 Estimation Problems in Inertial Navigation 2.2.2.2 Literature on Error Models 2.2.2.3 Alignment Problem

2.2.2.4

2.2.2.3.1 Self Alignment: Coarse 2.2.2.3.2 Self Alignment: Fine 2.2.2.3.3 Transfer and Aided Alignment 2.2.2.3.4 In Flight Alignment 2.2.2.3.5 GPS Based Alignment GPS INS Estimation and Data Fusion 2.2.2.4.1 GPS INS Estimation Using Kalman Filter

vi

i iii iv vi xi xiv

1 3 3 3 5 6 7 8 9 10 11 11 12

13 13 13 14 16 19 21 22 23 24 25 27 28 29 29 30

2.2.2.4.2 GPS INS Estimation Using Post Kalman Filter 2.3Literature Survey Solution Domain 2.3.1 Linear Regression Kalman Filter (LRKF) Techniques 2.3.1.1 Unscented Kalman Filter (UKF) 2.3.1.2 Central Difference Filter (CDF) and Divided Difference Filter (DDF)

33 34 35 35 36

2.3.2 Pseudo Measurement Filter 2.3.3 Approximate Grid Based Filter 2.3.3.1 Adaptive Grid Filter 2.3.4 Particle Filter 2.3.5 Robust Filtering 2.3.6 Robust H2 and H ∞ Filter 2.3.6.1 Riccati Equation Based Approach 2.3.6.2 LMI Based Approach 2.3.7 Risk Sensitive (RS) Filtering 2.3.7.1 Nonlinear Risk sensitive Filter 2.3.8 Cramer Rao Lower Bound (CRLB) 2.4Discussion and Conclusion

37 38 38 39 41 41 41 43 44 46 47 48

Chapter 3 Test Problems 3.1 Introduction 3.2First Order Linear System (L1) 3.3Second Order Linear (L2) 3.4First Order Nonlinear Problem (N1-1) 3.5Another First Order Nonlinear System (N1-2) 3.62nd Order Bearing Only Tracking (BOT) Problem (non-linear) 3.6.1 BOT Problem Formulation 3.6.2 Measurement Model I 3.6.3 Measurement Model II 3.6.4 Measurement Model III 3.6.5 Filter Initialisation

49 49 49 50 51 52 53 54 56 57 58

Chapter 4 Risk Sensitive Estimator 4.1 Introduction 4.2 Formulation of Risk Sensitive Estimation Problem 4.3 Recursive Solution of Risk Sensitive Estimation 4.3.1 Posterior Solution 4.3.2 Prior Solution 4.4 Risk Sensitive Estimation For Linear Gaussian Problem 4.4.1 Posterior Risk Sensitive Kalman Filter 4.4.2 Prior Risk Sensitive Kalman Filter 4.5 LMI Based Formulation of RSKF 4.5.1 LMI Based Kalman Filter Formulation

59 60 61 61 62 63 63 64 65 65

vii

4.5.2 LMI Based Risk Sensitive Kalman Filter Formulation 4.6 Robustness Properties of RSKF 4.6.1 Robustness Properties 4.6.2 Pragmatics Related to RS Parameter 4.7 Numerical Experiment and Illustration for RSKF 4.7.1 Sensitivity to State Model Error 4.7.2 Sensitivity to Unmodelled Bias 4.7.3 Sensitivity to Noise Modeling Error 4.8 Discussion and Conclusion Chapter 5 Risk Sensitive LRKF 5.1 Introduction 5.2 Risk sensitive Filter 5.3 CDRSF for Prior Estimation 5.3.1 Relations for Prior Estimation 5.3.2 Implementation of CDRSF for Prior Estimation 5.4 Formulation of Risk Sensitive Unscented Kalman Filter 5.4.1 The Prior Form of RSUKF 5.4.2 The Posterior Form of RSUKF 5.4.3 Risk-Sensitive Unscented Kalman Filter Algorithm for Prior Estimation 5.4.3.1 General RSUKF Algorithm for Nonlinear Systems 5.4.3.2 Special Cases 5.4.3.3 Scaled Risk Sensitive Unscented Kalman Filter (Scaled RSUKF) 5.5 Case Study 5.6 Scope for Future Work 5.7 Discussion and Conclusion

66 67 68 69 69 69 70 73 74

75 76 77 77 78 79 80 81 82 82 83 84 86 89 91

Chapter 6 Adaptive Grid Risk Sensitive Filter 6.1 Introduction 6.2 Grid Based Solution of Risk Sensitive Estimation Problem 6.2.1 Risk Sensitive Estimation Problem 6.2.2 Recursive Solution of Risk Sensitive Estimation 6.2.3 Grid Formulation 6.2.3.1 Representing Probability Densities as Point Mass on Grid Points 6.2.3.2 The Grid based RSE Algorithm 6.2.3.3 Weakness of Simple Grid Based Risk Sensitive Filter 6.3 Grid Adaptation Techniques 6.3.1 Obtaining Approximate Boundaries of the Region of Interest 6.3.2 Selecting Grid Intervals 6.3.3 Alignment of Axes

viii

92 94 94 95 95 95 96 97 98 99 100 102

6.4 6.5 6.6

6.7

6.3.4 Determination of Approximate Posterior Distribution Search Algorithm Implementation of Risk Sensitive Adaptive Grid Filtering Algorithm 6.5.1 Notes on AGRSF Algorithm Example and Case Study 6.6.1 Case Study I First Order Linear System 6.6.2 Case Study II Second Order Linear System 6.6.3 Case Study III First Order Nonlinear System 6.6.4 Case Study IV Second Order Nonlinear System (BOT Problem) 6.6.4.1 Problem Description 6.6.4.2 Understanding the Intermediate Steps in Simulation 6.6.4.3 Simulation Results Discussion and Conclusion

103 105 106 108 110 110 111 114 116 116 116 120 123

Chapter 7 Risk Sensitive Particle Filter 7.1 Introduction 126 7.2 Formulation of Risk Sensitive Estimation Problem 127 7.3 Prior Risk Sensitive Particle Filter Formulation 128 7.3.1 Solution of Risk Sensitive Estimation Problem for Prior Estimation 128 7.3.2 Probabilistic Interpretation 129 7.3.3 Particle Implementation 129 7.3.4 Optimal Proposal 130 7.4 Posterior Risk Sensitive Particle Filter Formulation 132 7.4.1 Solution of Risk Sensitive Estimation Problem for Posterior 132 Estimation

7.5 7.6

7.7 7.8 7.9

7.4.2 Probabilistic Interpretation 7.4.3 Particle Implementation 7.4.4 Optimal Proposal Notes Case Study 7.6.1 1D Nonlinear Case 7.6.2 2D Bearing Only Target Tracking Convergence Study Scope for Further Work Discussion and Conclusion

132 133 134 135 136 136 138 139 141 142

Chapter 8 Cramer Rao Lower Bound and Preliminary Design for Target Tracking Systems 8.1 Introduction 8.2 CRLB Computation 8.3 Parametric Studies of BOT Problem Using CRLB 8.3.1 Nominal Performance Comparison 8.3.2 Influence of Sampling Time on CRB

ix

143 145 146 147 147

8.3.3 Influence of Measurement Error Covariance 8.3.4 Influence of Process Excitation Covariance 8.3.5 Influence of P22,0 8.3.6 Adding Another Measurement 8.3.7 Influence of Eclipsing Effect 8.4 Preliminary Tracking System Design Using CRLB 8.5 Discussion and Conclusion

149 151 151 153 154 155 156

Chapter 9 Risk Sensitive Filter For INS Self Alignment 9.1 Introduction 9.2 Problem Formulation 9.2.1 Velocity Error Equation 9.2.2 Attitude Error Equation 9.3 Estimation of Misalignment Angle Considering Biases in State Variable 9.4 Estimation of Misalignment Angles Considering Biases as Known Input 9.5 Effect of Unmodelled Bias 9.6 Risk Sensitive Kalman Filter With Unmodelled Bias 9.7 Misalignment Angle Estimation Using Gyro Output as Measurement 9.8 State Space Model For Large Misalignment Angle 9.8.1 Alignment Considering Biases as Known Input 9.8.2 Alignment in Presence of Unmodelled Bias 9.9 Discussion and Conclusion

158 159 160 161 162 166 169 172 174 177 178 180 181

Chapter 10 Discussions and Conclusions 10.1Discussions 10.2Conclusions 10.3Suggestion for Further Research

183 183 185

Bibliography

187

Appendices

204

x

LIST OF FIGURES Figure 3.1 Figure 3.2 Figure 3.3 Figure 3.4 Figure 3.5 Figure 3.6 Figure 3.7 Figure 3.8 Figure 3.9 Figure 3.10

Truth value for state stabilizes around the equilibrium point -1 Measurement for state when stabilizes around –1 equilibrium point Truth values for state stabilizes around the equilibrium point +1 Measurement for state stabilizes around +1 equilibrium point Tracking platform kinematics. Truth value of position for a particular run Truth value of velocity for a particular run Measurement versus position versus time Signal to noise ratio (with and without eclipsing) Measurement error covariance with eclipsing

51 51 52 52 54 56 56 57 57 58

Figure 4.1 Figure 4.2 Figure 4.3 Figure 4.4 Figure 4.5 Figure 4.6 Figure 4.7

RMSE of first state variable for KF and RSKF RMSE of 2nd state variable for KF and RSKF Terminal RMSE of 1st state variable for KF and RSKF for different δ The truth and estimated value of the state with unknown bias Truth and estimated value of 1st state variable with unknown bias for 2D case RMSE for 1st state with unknown bias 2D case RMSE for RSKF and KF for Q mismatch

70 71 71 72 72 73 73

Figure 5.1 Figure 5.2 Figure 5.3 Figure 5.4 Figure 5.5 Figure 5.6

Identical nature of RSUKF and RSKF for linear Gaussian system Performance of ERSF and RSUKF for a representative run RMSE plot of ERSF, CDRSF and RSUKF for 1000 runs RMSE of position obtained from UKF & RSUKF RMSE of velocity obtained from UKF & RSUKF Position estimation obtained from ERSF and RSUKF for large initial position error Velocity estimation obtained from ERSF and RSUKF for large initial position error

87 87 88 89 90 90

Figure 5.7

Figure 6.1 Figure 6.2 Figure 6.3 Figure 6.4 Figure 6.5 Figure 6.6 Figure 6.7 Figure 6.8 Figure 6.9 Figure 6.10 Figure 6.11

Idea of Grid and supports Selections of grid intervals Alternative method for selection of grid intervals Alignment of axes to reduce wastage of grid Determination of mean and co variance from EKF Comparison of AGRSF and KF for linear Systems Convergence of AGRSF with number of grid points Plot of initial support points (16 X 16) and their respective area Posterior probability density function at intermediate step Truth and estimated value of first state variable Truth and estimated value of first state variable

xi

90

97 101 102 103 105 110 111 112 113 113 113

Figure 6.12 Figure 6.13 Figure 6.14 Figure 6.15 Figure 6.16 Figure 6.17 Figure 6.18 Figure 6.19 Figure 6.20 Figure 6.21 Figure 6.22 Figure 6.23 Figure 6.24 Figure 6.25 Figure 6.26

Truth & Estimated value for ERSF, RSPF and AGRSF. RMSE of ERSF and AGRSF for 1000 MC run. Plot of probability density of estimated states with time Plot of probability density of estimated states with time for grid starvation case BOT point mass distribution after time update Likelihood distribution 50 X 50 supports Posterior distribution with 50 X 50 supports Posterior distribution with 50 X 50 supports and 3 span factor Posterior distribution with 20 X 20 support points Truth and estimated value of position Truth and estimated value of velocity Absolute error of position Absolute error of velocity Convergence of position error with number of grid points Convergence of velocity error with number of grid points

115 115 115 116 117 118 118 119 119 120 121 121 122 122 122

Figure 7.1 Figure 7.2 Figure 7.3 Figure 7.4 Figure 7.5 Figure 7.6 Figure 7.7 Figure 7.8

Truth & estimated value of state obtained from ERSF, RSPF and AGRSF RMSE of ERSF, RSPF and AGRSF for 1000 MC run RMSE of position for different risk sensitive parameter RMSE of velocity for different risk sensitive parameter Variance of error across MC runs vs no of particles for 1D nonlinear system RMSE of position for different particle RMSE of velocity for different particle Variance of error for position vs no. of particles in 2D BOT problem

137 137 138 139 140 140 141 142

Figure 8.1

Comparison RMS error performance of EKF and PF with LBRE of position at nominal condition of R, P22,0 , Q and T

148

Figure 8.2

Comparison of RMS error performance EKF and PF with LBRE of velocity at nominal condition of , P22,0 , Q and T

148

Figure 8.3 Figure 8.4 Figure 8.5 Figure 8.6 Figure 8.7 Figure 8.8 Figure 8.9 Figure 8.10 Figure 8.11 Figure 8.12 Figure 8.13 Figure 8.14

Influence of the sampling time on LBRE of position Influence of the sampling time on LBRE of velocity Influence of measurement error covariance on LBRE of position Influence of measurement error covariance on LBRE of velocity Influence of process excitation covariance on LBRE of position Influence of process excitation covariance on LBRE of velocity Influence of P22,0 on LBRE of position Influence of P22,0 on LBRE of velocity Effect of introducing sight line rate measurement (position) Effect of introducing sight line rate measurement (velocity) Effect of Changing SNR on LBRE of Position Effect of Changing SNR on LBRE of velocity

149 150 150 151 152 152 153 153 154 154 155 155

Figure 9.1

RMSE for east misalignment estimation considering biases as state variable

165

xii

Figure 9.2 Figure 9.3 Figure 9.4 Figure 9.5 Figure 9.6 Figure 9.7 Figure 9.8 Figure 9.9 Figure 9.10 Figure 9.11 Figure 9.12 Figure 9.13 Figure 9.14 Figure 9.15 Figure 9.16 Figure 9.17

RMSE for north misalignment estimation considering biases as state variable RMSE for azimuth misalignment estimation considering biases as state variable Estimation error of east misalignment angle for a typical run Estimation error of north misalignment angle for a typical run Estimation error of azimuth misalignment angle for a typical run RMSE of east misalignment angle for 100 MC run RMSE of north misalignment angle for 100 MC run RMSE of azimuth misalignment angle for 100 MC run RMSE of azimuth misalignment angle for different unmodelled gyro biases RMSE of east misalignment angle for different unmodelled accel bias RMSE of north misalignment angle for different unmodelled accel bias RMSE of azimuth misalignment angle for different unmodelled accel bias RMSE of east misalignment angle under unmodelled bias RMSE of north misalignment angle under unmodelled biases RMSE of azimuth misalignment angle under unmodelled biases RMSE for azimuth misalignment for velocity error only measurement case and additional gyro output measurement case with known input bias

165 165 167 168 168 168 169 169 170 171 171 172 173 173 173 175

Figure 9.18 RMSE of azimuth estimation for different unmodelled gyro biases. Figure 9.19 RMSE of azimuth estimation for different unmodelled gyro biases for higher value of R.

176 176

Figure 9.20 Figure 9.21 Figure 9.22 Figure 9.23 Figure 9.24 Figure 9.25 Figure 9.26

177 179 179 179 180 181 181

KF and RSKF for gyro output measurement situation RMSE of east misalignment angle RMSE of north misalignment angle RMSE of azimuth misalignment angle RMSE of east misalignment angle with unmodelled bias RMSE of north misalignment angle with unmodelled bias RMSE of azimuth misalignment angle with unmodelled bias

xiii

LIST OF TABLES Table 7.1

Time Calculation for a single run of RSPF and AGRSF for different number of grid points and particle

138

Table 9.1

RMS error settling values for different unmodelled gyro and accelerometer bias

166

xiv

1 Introduction 1.1

Motivation Many real world problems need estimation of unknown states and other parameters of

dynamic systems using available noisy observations. Such problems include tracking an intruding aircraft or missile using radar or seeker measurements, filtering of digital communication signals using noisy measurements, monitoring, diagnosis and prognosis of industrial equipments, speech processing and image processing from noisy data, localization of mobile robot, navigation using GPS INS fusion, or estimation of the volatility of financial instruments using stock market data. In most of these applications prior knowledge of the system is available although the knowledge may not be accurate. So the challenge is to estimate the values of the state variables of the system accurately from the noisy observation and prior inaccurately known process dynamics. The broad heading under which most of such topics are discussed is called Signal Processing. If the data and system dynamics can be modelled in linear Gaussian state space, it is possible to derive an exact analytical expression to compute the optimal posteriori distribution at any instance of time. If the problem is formulated in discrete domain for computational simplicity, the state can be obtained recursively. This recursion is well known as the Kalman filter. Optimality of Kalman filter depends on various assumption such as (i) linearity of the model, (ii) whiteness and Gaussianity of the noise sequence and (iii) absence of correlation between process and measurement noise. Violation of these assumptions may lead to non-optimal solution of the estimator. In real life application most of the systems do not follow the above assumptions due to nonlinearity and non-Gaussianity. During the last four decades considerable amount of research has been done to estimate the states in an approximate way when the signal model is nonlinear and/or non-Gaussian. Many approximation schemes such as extended Kalman filter (EKF), central difference filter (CDF), Gauss-Hermite filter (GHF), unscented Kalman filter (UKF) etc are proposed for this purpose. With the increase of computational power, the possibility of computationally intensive but more accurate numerical solutions for such problems (nonlinear and/or non-Gaussian) have also been explored and as a result estimators such as grid-based filter, particle filter have been proposed. All these techniques for nonlinear estimation problems, are sometimes collectively referred as “Post Kalman” filtering technique. Although substantial work has been done on nonlinear filtering techniques, but still as of now, no optimal estimator exists for nonlinear estimation problem. So estimation of states in real life still remains a challenging job to a practitioner

Chapter 1

especially for the system with high nonlinearity and poor observability. Many of such techniques have been reviewed in the recent monograph by Ristic et. al [Ristic 2004a], entitled as “ Beyond Kalman Filter”. It may be noted that the present work was started before the book has been published. So from the above discussion it is clear that the post Kalman filtering techniques are capable of accommodating nonlinear systems and non-Gaussian noise and are therefore considered or claimed to be powerful. The advanced techniques in post Kalman filtering method such as unscented Kalman filter (UKF), particle filter (PF) etc are claimed to be superior in terms of accuracy in estimation. Although it should be kept in mind that the superiority of the proposed novel technique can wrongly be demonstrated by choosing a problem carefully. In real life scenario the choice of appropriate filter for specific mission is again very difficult. There is no direct method by which the best filter for the specific problem can be chosen. The designer needs to first list down the probable techniques and implement them for that specific problem or similar problems. A comparison table regarding the accuracy and computational time need to be built. From that table the designer can choose the most suitable filter for that particular mission profile. Nonlinear filtering therefore calls for substantial experimentation. Again it has been realised that if the system and or noises are not perfectly modelled (that means if the system contains modelling uncertainty) the traditional filters may not track the truth accurately enough. The so named “robust filters” have been proposed to deal with the situations where modelling uncertainty is substantial. Some robust estimators such as robust Kalman filter, H2 and H ∞ filter, risk sensitive filter etc are available in literature to cope up with the problem due to uncertain truth. Most of these filters have been studied for linear Gaussian cases. The present work attempts to extend such techniques for nonlinear and possibly non-Gaussian problems. In particular, for this thesis work, risk sensitive filtering has been chosen for the subject of study and further enhancement. The risk sensitive estimator originates from risk sensitive control law, which minimises an exponential cost criterion. Like Kalman filtering the closed form solution for risk sensitive filtering exists only for linear Gaussian case. Risk sensitive filtering theory has been extended to non-linear systems in [Jayakumar 1998] where an EKF like approach has been adopted. However, the well-known limitations of the EKF, including smoothness requirement for the functions and noise Gaussianity restriction, are also transferred to extended risk sensitive filtering. To cope up with the limitation of existing extended risk sensitive filtering (ERSF) in this thesis some novel techniques for nonlinear risk sensitive estimation have been proposed. So the primary motivation for the present work had been to

2

Chapter 1

(i) Investigate the properties of risk sensitive filtering (ii) Develop novel advanced methods for nonlinear risk sensitive estimation problems (iii) Investigate the scenarios where risk sensitive filter can out perform the non-robust filters. To obtain meaningful insight and properties of the proposed novel nonlinear risk sensitive filtering these methods are applied to (i) known nonlinear poorly observable state estimation problems, and (ii) selected tracking and navigation problem.

1.2

Thesis Objective With the above motivation and background, the objective of the present work was cast as, to (i)

Develop novel estimation and filtering methods for nonlinear problems with emphasis on risk sensitive filters.

(ii)

Investigate the properties of risk sensitive filtering

(iii)

Investigate the scenarios, with emphasis on the aerospace problems where the developed filter algorithms can out perform the existing filters.

(iv)

To examine whether the Crammer Rao or similar bound can be utilized for system level choice of sensors and algorithms.

To obtain meaningful insight and properties of the proposed novel nonlinear risk sensitive filtering these methods are applied to (i) known nonlinear poorly observable state estimation problems, and (ii) selected tracking and navigation problem.

1.3

Background on Aerospace Problem Domain The area of interest of the present work is in the domain of aerospace. However the scope

must be further narrowed down. In aerospace specifically we are interested in two problem areas, namely target tracking, and navigation where the estimation techniques play key roles. For reasons other than pure technical, the application area of this work had a bias towards defence related application. A brief background of the domain is covered in this section. 1.3.1

Target Tracking

In the next chapter a comprehensive literature survey on target tracking has been presented and hence the discussions here would be brief. In our present context target tracking means to know the position about an enemy aircraft or enemy ship, missile, or other potentially moving object, which needs to be intercepted and or destroyed from the military point of view. The tracking system means the system from which the target is being tracked. The tracking system may be stationary, on ground or mounted in a moving

3

Chapter 1

platform like ship, aircraft, submarine, missile or torpedo. As an example for target tracking anti aircraft missile continuously track the path of the enemy aircraft. The position of the target can be up linked from the ground station or can be determined on board using on board seeker. Now a days generally the initial phase target tracking is done with the help of ground radar, while the seeker continuously searches for the target and once located, the seeker keeps the target in its field of view (tracking). Once the seeker in the missile is locked to the target the missile tracks the target on its own. Ground radar can track multiple targets. Once the tracks are generated the number of targets, and states of the target such as position, velocity, acceleration can be estimated and predicted with the help of state estimators. The topic of target tracking involves a broad area of research such as (i) target modelling and dynamics, (ii) measurement models (iii) Filtering and prediction (iv) sensor data fusion So the key of success in target tracking lies in effective extraction of the information about the target state from the available observation from sensors. The sensor data may be obtained from the ground radar or on board seeker. In the initial stage of the flight, the radar data are important. The sensor data may be the bearing angle, angle rate, range, range rate or combinations of the above. The effectiveness of target tracking depends on the assumed target model’s good ness of fit for the particular engagement scenario. It is generally argued that a good model is worth than a thousand pieces of data. Most tracking algorithm are model based because prior knowledge of the target is available. It is observed that a good model based tracking algorithm will out perform the tracking mechanism without model. And a large amount of effort has been devoted to model the target in different engagement scenarios. For example if it is assumed that the target will not manoeuvre, then non manoeuvring model can be used for the target to be tracked. If it is assumed that target may manoeuvre, different manoeuvring models can also be used. Some times, decision based method is used to detect the manoeuvre of the target and switching between the two types of model is also very common. A comprehensive literature survey has been done by Li and Jilkov [Li 2003] on the different model for both manoeuvring and non manoeuvring target tracking. Successful target tracking also depends on good measurement model and less noisy sensor data. For example, if the measurement model of the system is highly nonlinear, the accuracy in target tracking degrades considerably. In fact, it is practically observed that in different co ordinate system, the accuracy of estimator is different. It is also important to know the noise characteristics of the sensors. It has been observed that more sensors or more observations some times help to improve the estimation accuracy about the state of a target.

4

Chapter 1

Another important aspect for successful tracking of the object is to choose efficient filtering algorithm to estimate the state of the target. As the target and sensor measurement model are generally nonlinear and no optimal filter exists for non-linear system the choice of estimation technique is not unique. The choice of filtering algorithm for a particular engagement scenario requires high insight and understanding of the problem as well as well awareness of the properties of different filtering techniques exist for non linear estimation. It is important to note that the assumed dynamics of the target cannot always be captured through model and the noise associated with the sensor cannot always be known perfectly. So the performance of the filtering algorithm may degrade in real life scenario due to mismatch in modelling. In this work keeping these factors in mind, some novel methods for nonlinear robust filtering which might be useful in target tracking problems have been proposed. 1.3.2

Navigation and its Use

The basic function of a navigation system is to accurately determine the position of a moving vehicle in which it is mounted. The navigation system has three functions (i) sensing the motion (ii) computing and (iii) outputting the information periodically. The history of evolution of navigation technology is very interesting and it has been well documented in [Kayton 1988]. Although, now a days, the technology for navigation of terrestrial, subsurface and airborne vehicles is very similar, we will keep focus on navigation technology for aerospace vehicles. The mainstay of modern navigation system is Inertial Navigation (INS), which uses accelerometers and gyros (Gyroscopes, rate gyro). The fundamental principle of inertial navigation system is fairly simple. An accelerometer measures the acceleration of the vehicle while the gyroscope measures the angular rate. If we know the initial position, velocity and attitude very accurately, integrating the output from the accelerometer and gyro, we can be able to know the position velocity and attitude of the flight at any instance of time. INS system may be gimbaled (mechanized platform) system or strap down system. In gimbaled system, a stabilized physical platform is maintained through a servo system on which the accelerometers and gyros are mounted and thus isolated from the rotations of the vehicle. If this mechanism can be achieved, the platform does not experience any rotation relative to navigation frame in spite of vehicle motion. In this approach, accelerometers aligned with the platform measures along the navigation co ordinate system axis. Vehicle attitude is determined by measuring the relative angles between the vehicle and platform axis. In “strapdown” INS there is no stabilized platform. The accelerometers and gyros are mounted on the vehicle body directly. The required reference frame is to be maintained mathematically by a computer. This, however, increases the on board computation load.

5

Chapter 1

Being an integrating system, the position output must be initialised. That means the initial angular and linear positions must be known. The method by which the angular orientation of an inertial navigation system (INS) is determined with respect to the reference axis system is called alignment procedure. There are two fundamental types of alignment process: self-alignment and transfer alignment [Kong 2000]. Self-alignment is carried out when the platform is nearly stationary. Normally, the self alignment process is divided into two phases, namely coarse alignment and fine alignment. The purpose of coarse alignment is to provide fairly good initial condition for fine alignment process. Coarse self alignment often uses raw gyro and accelerometer outputs (from IMU) and compares them with gravity and earth rate. Fine self-alignment is not carried out using IMU output but with its own navigation computer output (INS) of velocities. In transfer-alignment, one INS is aligned with the help of another instrument (say INS). As an example, we can take a fighter aircraft carrying missiles in it. Here the carrier aircraft has its own INS and the missiles also have their own INS. It is necessary to align the INS (slave INS) of the missile with the aircraft INS (Master INS). Transfer-alignment may be carried out in quasistationary as well as in motion. It is traditionally carried out by comparing linear velocities of the master and test INS's. So estimation methods can be applied to determine misalignment angle between MINS and SINS by fusing the velocity mismatch measurement with navigation dynamics. Motion (intentional manoeuvre or translatory motion) may be designed to improve observability and quicker convergence of the estimators. As the navigation equations are nonlinear, gyro and accelerometer contain biases and observability is not very good for all alignment problems, there is immense scope to improve alignment procedure by applying advanced filtering technique. In the present work, we focus on the application of nonlinear risk sensitive filtering technique for the alignment problem.

1.4

Background in Solution Domain The dissertation is on the development of novel techniques for nonlinear risk sensitive

estimation problem and application of such filterings in target tracking and navigation. In this section a very brief background has been provided on state estimation techniques. For more appreciation, this background information has been provided in a simplistic non-mathematical way with few references. References for background work have been provided in “literature survey” in the next section of the thesis.

6

Chapter 1

1.4.1

Background on Filters and State Estimators

Many excellent textbooks are available [Anderson 1979] on state estimation. Here some important features on (non-robust) estimation are mentioned as background. •

State estimators require (i) state equation or process dynamics of the system, called the state model (ii) measurements equation or the measurement model. In real life problems estimation of state becomes difficult due to nonlinear system dynamics and measurement system.

•

The state dynamics and measurement model can be represented in continuous time or discrete time. Here in the thesis mainly the discrete mode of representation has been taken.

•

In a general way, the process dynamics and measurement model can be written as State Equation: x k +1 = f k ( x k , u k , wk )

(1.1)

Measurement Equation: y k = hk ( x k , v k )

(1.2)

Here x k is the states of the system, u k is the known input, wk is random unknown input known as process noise, y k is the measurement and vk is measurement noise. •

In the special simplified case, the noise may be additive. The above two equations can be written as x k +1 = f k ( x k , u k ) + wk (1.3)

y k = hk ( x k ) + v k

(1.4) From now on, we shall only consider the discrete additive noise situation. The noise

wk and vk may or may not be uncorrelated and may not be white Gaussian sequence. •

To date, no general closed form solution for nonlinear problems is available. Most filtering solutions to such problems are suboptimal or approximate.

•

If the noises are not white, a method is available to whiten the noises and after that standard Kalman filter can be applicable. Similarly if the noises are correlated then also the Kalman filter can be applied with some modification in algorithm.

•

The process noise has great significance in real life cases. It represents the unknown and random nature or the modelling error of the process dynamics. In particular target tracking scenario process noise may signify the unknown maneuver due to pilot or due to pre program stored on onboard computer of the target. If the plant is substantially deterministic (means that having no modelling error or unknown excitatory) then that process noise part can be set as zero. In simplified cases the process noise is assumed as zero mean white Gaussian noise, its covariance generally expressed with the symbol Q.

7

Chapter 1

•

The sensor measurements are corrupted with the sensor noises. Sensor noise characteristics can be found out from the experimental data. Generally the sensor noise is assumed as zero mean white Gaussian noise sequence. The covariance matrix of the noise is generally symbolically expressed with R.

•

To run the filter, the state variables need to be initialised with the prior knowledge. Also initial error covariance of the states needs to be assigned. More the confidence designer has with initialisation less will be the error covariance and vice versa. More accurate is the initialisation quicker is the convergence.

•

It is worthy to mention here the statistical significance of covariance matrix for multidimensional Gaussian distribution. The diagonal elements of the covariance matrix represent the square of the standard deviation of the respective variable. The off diagonal term represents correlation coefficients between the corresponding variables. For Gaussian probability distribution more is the value of cross correlation coefficients more the bulginess of the probability density curve.

•

In real life application of the Kalman filtering the big challenge is to select Q and R. In real life scenarios, sometimes it happens that the noise covariance or the characteristics of the process and measurement noise remains unknown. In such a situation, estimation of noise covariance is necessary before implementation of estimation technique. That type of filtering where the noise covariances are also estimated online, is known as adaptive filtering [Mayer 1976, Noriega 1997].

1.5

Problem Statement and Scope The dissertation is on the development of novel techniques for nonlinear risk sensitive

estimation problem and application of such filterings in target tracking and navigation. Comparatively more emphasis has been given in formulation of novel filtering techniques rather than its aerospace application. In the present work some nonlinear advanced filtering methods, which has also been claimed to be robust under process uncertainty, have been proposed. Prior to this work and work done by the co-worker in the same laboratory [Srinivasan 2007] no good method was available in literature for nonlinear risk sensitive filtering. Initially an existing technique based on extended Kalman filter had been examined which is known as extended risk sensitive filter (ERSF). However it was noted that the ERSF contains all the limitation of EKF and does not perform well in the nonlinear estimation problem. This limitation motivated the author to develop novel techniques, which will provide better results than ERSF.

8

Chapter 1

The methods developed include some approximate algorithms where noise excitation had to be Gaussian. However, system nonlinearities eventually produce non-Gaussian probability density function for the evolving states. The Risk Sensitive Particle filter developed however does not require Gaussian assumption / restriction. For the comparative studies of the filters and to understand the properties of the estimators in a better way, simple target tracking scenarios have been considered. The target tracking examples considered in two dimensional scenario. It is assumed that the target is a non manoeuvring one. In this work, a problem on self alignment procedure has been formulated. The problem is in general nonlinear in nature and under the assumption of small misalignment angle the formulated problem becomes linear. Both the linear and nonlinear problem have been solved using risk sensitive filters. In literature Cramer Rao Lower Bound (CRLB) is well known for theoretically best results if any optimal filter exist. As the selection of filtering technique requires substantial experimentation, a part of the study involved investigation of whether CRLB can play a role in selecting sensors and sampling times without committing to a particular filtering algorithm. Some interesting results have come out of this investigation. It has been established that in the preliminary phase of design of the tracking systems different conflicts can be resolved with CRLB without going in detail design of estimator. Notable exclusions from the scope of this thesis are (i) Manoeuvring target tracking and (ii) H ∞ robust estimators.

1.6

The Approach and Methodology The general approach had been to develop new algorithms by extending existing algorithms,

thereby removing some of the shortcomings. The proposed filters were then tested by using test problems. For the evaluation of the performance of the filters, accuracy in estimation and computational load have been considered. To compare the accuracy between different filtering methods, Monte Carlo runs have been executed. It has been ensured that the number of Monte Carlo population size is adequate. Most of the cases the simulation has been carried out using Matlab software. Although in case of particle filter and grid based filter, the computation became most intense especially in the time of Monte Carlo simulation. In those scenarios, the simulations are carried out using executable code generated by C++ compiler. From C++ codes developed by the worker, The possibility of implementation to on board computer has also been explored.

9

Chapter 1

In some cases the results obtained from different tracking filters are also compared with CRLB to know the degree of optimality of the estimator. These are mostly done in the papers contributed by the present worker for the non risk sensitive type estimators. This is because the CRLB bound for the risk sensitive case is not available.

1.7

Contribution This section will specifically state the contribution of the present work in the background of

earlier work. The contribution made by the present worker may be broadly categorized as follows •

Fairly extensive literature survey on estimation methods and tracking and navigation problems.

•

Choice of test problems.

•

Study the unknown properties of risk sensitive estimator. In this study it has come out that apart from model uncertainty, risk sensitive filters perform better under un modelled bias in simple cases.

•

LMI based formation of risk sensitive filtering for Linear Problems.

•

Formulating novel risk sensitive filtering algorithms for nonlinear problem. Developed novel estimation methods are (i) central difference risk sensitive filter (CDRSF), (ii) Risk sensitive unscented Kalman filter (RSUKF), (iii) Adaptive grid risk sensitive filter (AGRSF), (iv) risk sensitive particle filter (RSPF) both prior and posterior version. The applicability of optimal proposal for RSPF and AGRSF has also been verified.

•

Formulating an approach for preliminary design of target tracking systems using CRLB.

•

Application and evaluation of nonlinear and risk sensitive filtering to tracking problems.

•

Application and evaluation of nonlinear and risk sensitive filtering to alignment of inertial navigation systems.

The first application of nonlinear risk sensitive filtering in BOT problem has been reported in the thesis submitted by Srinivasan [Srinivasan 2007] who worked closely with the present author in the same laboratory. Perhaps the work done by the present author and Srinivasan [Srinivasan 2007] is the first attempt to apply the risk sensitive filtering in target tracking scenario. In the early phase of design life cycle of the interceptor it is necessary to select the filtering strategy and feasibility of the demand made by the designer. It has been established that in this

10

Chapter 1

scenario CRLB helps designer to test feasibility of the mission without going in detailed estimator design. This concept was first reported in [Sadhu 2004a] and later extended by Srinivasan. In another application a problem for inertial navigation system alignment procedure has been formulated. Here risk sensitive filtering has been applied to self alignment problem for inertial navigation system. This can also be claimed as the first attempt to apply risk sensitive filtering in alignment problem. Different aspects of self alignment problem including observability, bias sensitivity have been discussed. The performance of risk sensitive filtering has been compared with their risk neutral counterpart under different situations. 1.7.1

Publication Generated from this Work •

Formulation of Risk sensitive unscented Kalman filter (RSUKF) has been accepted for publication in IET Control Theory and Applications, UK, 2008.

•

Formulation of central difference risk sensitive filter (CDRSF) has been published in IEEE Signal Processing Letters, 2007.

•

Developed Risk sensitive particle filter (RSPF) has been accepted for publication in Elsevier

Journal

of

Signal

Processing

and

available

online

since 1st October, 2008. It has also been presented in IEEE INDICON conference 2006. The publication has been cited and critically appreciated by

international researchers [Orguner 2007, 2008] •

Developed Adaptive grid risk sensitive filter (AGRSF), was published in (i) the proceedings

of

the

IEEE

Nonlinear

Statistical

Signal

Processing Workshop, Cambridge, UK, 2006 (ii) IEEE INDICON, 2005 and also communicated for publication to International journal of modelling and simulation.

•

An approach for preliminary design of guidance system for homing missiles by using the Cramer Rao Bound (CRB) has been presented in the proceedings of the IEEE

International

Symposium

on

Signal

Processing

and

Information Technology (ISSPIT), Rome, Italy, 2004. The work also

published in the Journal of the Institution of Engineers, India, 2006.

1.7.2

Credits to Co-Workers

Sharifuddin Mondal [Sadhu 2006] should be credited to initiate the work on target tracking by applying UKF for the BOT problem. Co-worker Srinivasan worked closely towards the

11

Chapter 1

development of CDRSF algorithm. He also contributed towards the adaptive grid filtering which is taken as the basis to develop AGRSF algorithm by the present author.

1.8

Organization and Content of the Thesis The thesis consists of ten chapters including this present one. Effort has been made to reduce

the interdependencies between different chapters. Apart from this chapter next two chapters can be considered as introductory chapters. Among them second chapter contains a detailed literature survey on different types of estimators as well as target tracking and navigation problem. In third chapter several test problems have been discussed which were used in successive chapters for comparing the accuracy of different proposed estimators. Different properties of risk sensitive Kalman filter (RSKF) and its alternative formulation have been discussed in chapter 4. In Next three chapters various novel nonlinear filtering techniques, which may be considered as main contribution of the thesis, have been described. In chapter 5 two types of sigma point risk sensitive filter named as central difference risk sensitive filter (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) have been developed. In next chapter another nonlinear risk sensitive filter based on grid adaptation method has been formulated. In chapter 7 one more variety of nonlinear risk sensitive filtering based on particle filtering technique has been proposed. Chapter 8 deals with Cramer Rao Lower Bound for bearing only target tracking problem. It describes how that bound could be used in preliminary design stage of a target tracking problem. In chapter 9 a self alignment navigation problem has been developed and risk sensitive filters have been applied to solve the problem. The last chapter is on Discussions, Conclusions and Scope for further work. The concluding part is followed by a list of literature cited. The Appendices provide flow charts and algorithms, which have been used in this work but not embedded within the earlier chapters.

12

2 Literature Survey 2.1

Introduction to Literature Survey There are two aspects of this thesis. One is to develop new improved nonlinear filtering

techniques for nominal as well as uncertain system dynamics. Another aspect is to apply the nonlinear improved filtering techniques to real life aerospace problem such as guidance, target tracking and navigation. So the surveyed literatures are broadly classified as problem domain and solution domain. Problem domain literature means the literature where applications have been discussed and solution domain literature means where the new improved filtering techniques by previous workers have been developed. Although the distinction between the two classes is not rigid as in many papers the developed methods have been applied to newly formulated real life problems. In this survey an attempt has been made to provide a summary of recent techniques for nonlinear filtering and their application into target tracking, guidance and navigation problems.

2.2

Problem Domain Literature Survey In this section we’ll discuss about the existing literature on aerospace problem with

emphasis on the topics of target tracking and Navigation. In target tracking different methodologies on ballistic target tracking and bearing only target tracking have been discussed. Under the heading of navigation we summarized previous works on alignment problem and GPS INS estimation data fusion. 2.2.1

Tracking of Maneuvering Target Prior to this work Lie and Jilcov [Li 2001a, Li 2001b, Li 2002, Li 2003, Li 2005] have

published comprehensive survey of various models and methods on Manoeuvring Target Tracking in five parts. In the first part [Li 2003] different models used for target modelling have been summarised. In the second part [Li 2001a] survey on ballistic models has been presented. Part III [Li 2001b] of this survey deals with measurement models. Survey on multiple model method has been discussed in part V [Li 2005]. The survey series is helpful for the beginner for a broad over view on the model and methods used for maneuvering target tracking. Bar Shalom also in his book [Bar Shalom 1993] discussed about the state estimation theory with application to target tracking. In that book basic concepts and different aspects of the estimators have been illustrated.

Chapter 2

2.2.1.1 Bearing Only Tracking Target tracking in aerospace is generally carried out with the data obtained from the seeker. Such seekers, depending on the sensors used may provide one or more of sight line angle (loosely speaking bearing), sight line rate (SLR) range and range rate. However a challenging tracking problem is no-named bearing only tracking (BOT). Though not favoured for aerospace tracking, this tracking has become a benchmark problem for testing nonlinear filters (estimators). BOT problem has attracted many researchers. In the brief discussion that follows, a subset of the literature is surveyed. A good starting point for the BOT literature is the monograph [Ristic 2004a] where the BOT problem for the 2D case has been formulated. The problem has been solved in various co ordinate system using different nonlinear filters such as UKF, PF, auxiliary particle filter (APF), IMM EKF. Simulation results obtained from different filters have been presented and compared with Cramer Rao lower bound. In [Song 1996] an observability analysis has been made for proportional navigation guidance using bearing only measurement. Based on this analysis a guidance law was proposed. Simulation results using modified gain extended Kalman filter (MGEKF) have also been provided to show that the proposed method produces small miss distance. Bearing only tracking in Cartesian and modified polar co ordinate system has been studied in [Peach 1995] using extended Kalman filter. A new approach named as Range parameterized tracking (RP) has been proposed. It is claimed that the RP tracker performs better than modified polar (MP) or Cartesian tracker in certain tracking scenario. Passive BOT tracking was also subject of interest in [Jauffret 1996]. Observability in such target-tracking scenario has been analysed. The advantage of such observability analysis is its simplicity over the analysis based on lie algebra. In the paper [Nardone 1997], it has been stated that the available pseudo linear solution suffers with poor observability condition. Here a practical pseudo linear solution, which does not suffer from the observability problem, has been proposed. Algorithm, performance and results are also presented in this paper. BOT problem has been extended in [Cadre 1998] for the source having maneuvering abilities. In this scenario source trajectory has been modeled using Markovian model. Applicability of dynamic programming where no prior information is available has been pointed out. Next the general framework of Markovian decision process has been provided for complete information case.

14

Chapter 2

In [Kronhamn 1998] the multi hypothesis Cartesian Kalman filter (MHCKF) has been applied to the problem of bearing only target tracking. MHCKF has been used adaptively to find an own ship motion trajectory. The simulation results compare the estimated range with the results from a fixed, two-leg own ship trajectory. In [Karlsson 2000] auxiliary particle filter (APF) has been used to track a maneuvering target. Here the truth motion of target is assumed to follow an “S” shaped curve. Simulation results show that APF gives better results compared to IMM filter. It is well known that convergence rate of the extended Kalman filter is poor and batch estimator cannot handle the maneuvering target. To rectify these shortcomings in [Kirubarajan 2001a] batch maximum likelihood probabilistic data association (ML-PDA) estimator has been combined with the recursive interacting multiple model (IMM) estimators. In an earlier publication [Aidala 1983] bearing only target tracking has been considered in modified polar co ordinate system. In [Nardone 1984] Modified gain extended Kalman filter has been developed and its stability has been studied. The filter is applied to solve bearing only measurement problem. The Monte Carlo simulation results for 3D BOT obtained from different filter (pseudo measurement filter, MGEKF, EKF) have been provided. The paper [Ho 2003] proposed an estimator based on the pseudo linear measurement equations, which can produce an unbiased solution for bearing only tracking and Doppler bearing tracking. Simulation results have been provided to illustrate the performance of the proposed estimator with respect to CRLB. In [Clark 2005] the novel Rayleigh shifted filter, has been applied to bearing only problem with clutter. Simulations indicate that it gives good accuracy, even in adverse circumstances. Currently some efficient nonlinear filters are available in the literature, researchers are investigating the applicability of such filters in the BOT problem. Such an attempt has been made in [Sadhu 2004b] where sigma point Kalman filter (SPKF) has been applied to 2D BOT problem. The SPKF is found to more robust against track loss criteria. In a similar paper [Sadhu 2006] the performances of iterated EKF, different version of SPKF, and EKF are studied in detail in terms of computational load, efficiency, track loss etc. It has been reported that the use of SPKF reduces the track loss. Finally it is recommended to use SPKF for BOT problem considering all aspects. BOT problem has also been solved in [Hou 2006] using joint sigma point Kalman filtering technique. It is reported that the algorithm can eliminate the effect of systematic error in state estimation as well as reduce linearization error. The simulation results are also provided.

15

Chapter 2

The problem of maneuvering target racking has been the subject of study in [Ristic 2003b]. The target dynamics has been modeled by using multiple switching models. Three tracking algorithms namely (i) IMM EKF (ii) IMM UKF (iii) IMM PF have been developed for BOT problem. The results obtained from three algorithms have been compared with theoretical lower bound. In [Lin 2002] a simple 2D BOT problem has been solved using EKF, pseudo measurement filter and particle filter. The simulation results show the RMS error obtained from different filter. It is reported that EKF diverges while the other two filters converge nearly the same manner. In this thesis this problem is reformulated and several systematic studies have been done using different types of estimators. In [Farina 1999] BOT tracking problem has also been solved using batch processing and recursive processing filter. Monte Carlo simulation is performed for straight line and accelerating targets. In recent years numerical integration based filter known as particle filter (PF) has been evolved for nonlinear estimation problem. The filter has also been applied for BOT problem by researchers. In [Ristic 2004b] different verities of particle filters such as (i) multiple model PF (MMPF), (ii) auxiliary MMPF (AUX-MMPF), and (iii) jump Markov system PF have been applied for nonlinear target tracking problem. The performance of these filters is compared with IMM-EKF, IMM-UKF and CRLB for three separate cases, namely (i) single-sensor case, (ii) multi sensor case, and (iii) tracking with hard constraints. In [Qu 2006] SMCEKF (sequential Monte Carlo EKF) and SMCUKF (sequential Monte Carlo UKF) have been used for BOT problem. It has been shown that the SMEKF and SMUKF provide better results compare to ordinary UKF and EKF.

2.2.1.2 Ballistic Object Tracking Tracking ballistic object is one of the important topics for application of estimators that are being studied during past three decades by researchers. The goal of the research is to track the ballistic object and destroy it before they hit the ground. The ballistic object may be the enemies’ missile or other terrestrial object like meteor that enter into the earth atmosphere. The interception may be carried out at exo atmospheric or endo atmospheric region. The methods and models for the both type of interception are different. Recently a survey has been made in [Li 2001a] where the authors reviewed more than 50 articles published over nearly three decades. Here the modelling issues in three phases of a ballistic target (namely boost, ballistic, reentry) have been summarized in various co ordinate

16

Chapter 2

systems. The review will be helpful for broad overview on ballistic target tracking for the beginners. The work on ballistic target tracking is mainly based on the modeling of the ballistic target and estimation of the position, velocity, acceleration, ballistic coefficient etc using different nonlinear filtering and compare the performance of the filters. Earlier when EKF was the only choice for nonlinear filtering, several variants of it have been applied to the said problem and compared. In [Mehera 1971] several variants of extended Kalman filter have been implemented in various co ordinate systems and compared based on estimation accuracy and computational time. In the monograph [Ristic 2004a] one chapter has been dedicated for tracking ballistic object on re entry phase. Here a simple vertically falling ballistic object has been considered. Extended Kalman filter (EKF), unscented Kalman filter (UKF) and particle filter (PF) have been used to estimate attitude, velocity and ballistic coefficient of the target. The simulation results obtained from the estimator have been compared with Cramer Rao lower bound (CRLB). Actually the chapter elaborately described the subject matter published in [Ristic 2003a]. The same problem of falling object has been considered in [Srinivasan 2006] where the problem is solved using EKF, unscaled square root UKF, central difference filter (CDF). The estimators are compared in terms of estimation accuracy and computational load. In [Yeddanapudi 1995] the ballistic target tracking problem has been considered using line of site (LOS) observation obtained from satellite. Gauss Newton iterative least square minimization algorithm has been applied for orbit determination. It is reported that the convergence of the algorithm depends on the initial guess. In [Sioris 1997] the tracking of incoming ballistic object has been presented using radar measurement. Theory of Extended Interval Kalman filter (EIKF) has been presented and the filter is applied to track ballistic target in uncertain environment. Simulation results show the filter can successfully track the incoming ballistic object. Paul Zarchan [Zarchan 2000] considers the tracking of ballistic missile with spiraling manoeuvre. It has been seen that the miss distance increases considerably in presence of spiraling manoeuvre. It has been also shown that using compensated wave guidance law with Kalman filter the miss distance can be reduced. In [Cardillo 1999] essential features necessary for a filter to be used for ballistic tracking have been discussed. A Kalman filter has been formulated for estimation of states where air drag is considered as one of the state. The measurements used for filter are azimuth, range and

17

Chapter 2

elevation. Simulation results show the filter can track both endo and exo atmospheric target for a wide verity of trajectory. In [Kirubarajan 2001b] probabilistic data association (PDA) algorithm with maximum likelihood (ML) estimation has been used to track the incoming ballistic missile using bearing only measurement. The Cramer Rao lower bound (CRLB) has also been simulated for that target. It is shown that the proposed estimator meets the CRLB, even for low observable condition. An interactive multiple model algorithm has been used in [Cooperman 2002] for tracking a tactical ballistic missile (TBM) using multiple sensor data. Tracking efficiency has been increased using multiple sensors in all three phases of the ballistic missile. Ballistic target tracking in re entry phase was also subject of study in [Farina 2002a]. The target has been tracked using EKF for both known and unknown ballistic coefficient case. The results are compared with CRLB. A similar problem has also been formulated in [Farina 2002b]. Newly available nonlinear filters such as UKF, PF have been applied to ballistic target tracking case. The filters have been compared with CRLB. In [Bruno 2004] the same problem has been solved using sequential importance resampling (SIR), particle filter (PF) and auxiliary particle filter (APF) along with CRLB. Extensive simulation results show that, the SIRPF performs nearly well with CRLB even in difficult scenario like an unknown, time-variant ballistic coefficient condition. Also in [Bruno 2005] nearly the same problem has been solved using density assisted particle filter along with posterior CRLB. Ballistic missile detection and tracking algorithm along with its kinetic model was also the subject matter of [Farina 2002c]. IMM tracking algorithm is applied to ballistic missile problem and Monte Carlo simulation results have been presented. P. Minvielle, [Minvielle 2005] lucidly described (i) ballistic dynamics during re entry phase (ii) radar scanning process (iii) the development of various nonlinear filtering techniques applied to ballistic target tracking scenario. The paper may be considered as a very good introduction to begin with in the field of ballistic target tracking. A ballistic target tracking problem has been solved in [Zhao 2006] using UKF, PF, and linear minimum mean square filter (LMMSF) and Gaussian PF. The authors concluded that LMMSE is preferred for ballistic missile target tracking scenario considering the tracking accuracy, robustness and computational load. Exo atmospheric ballistic target tracking problem has also been discussed in [Srinivasan 2005] using bearing only measurement.

18

Chapter 2

The tracking of re entry ballistic object has also been studied in [Angelavo 2003]. Sequential Monte Carlo based multiple model filter is used for dealing with high nonlinearity of the object dynamics. The performance of the filter has been evaluated by using RMSE. Predecessor M Srinivasan, from the same laboratory where the present author is working, in his thesis [Srinivasan 2007] also considered both the engagement on endo atmospheric and exo atmospheric for tactical ballistic missile. Advanced nonlinear filtering methods have been applied for both the problems and performances obtained from different methods have been compared. 2.2.2

Navigation

The term navigation had passed through several shades of meaning. Literally navigation means passing through waterways. This meaning has now been overshadowed by the advent of navigation instruments and aids. In the context of this survey, navigation means finding the current position (with velocities and attitudes) of a vehicle, robot or any moving body. A quick overview of terrestrial navigation concepts may be obtained from [Kayton 1988]. This study is confined to a subclass of navigation instrumentation system called strap down inertial navigation system (SDINS). This survey attempts to analyse the state of knowledge in application of filtering and state estimation techniques for SDINS (also called SINS) application. Main focus would be Aerospace navigation, occasionally there are digressions to space and terrestrial navigation. As SDINS is often used in conjunction with GPS, we shall provide some elementary references to the GPS literature as well. A navigation system consists of sensors, which generate signals and the processor, which determine position coordinates, velocity vector and attitude from such signals. For INS, the sensors are three gyros, which measure angular rates in three mutually orthogonal axes and a set of three accelerometers aligned with three gyro axes. These set of instruments along with their electronics are called Inertial Measuring Unit (IMU). Description of various inertial sensors may be obtained from standard textbooks like [Pitman 1962, Merhav 1996]. Conventional gyros are being replaced in some cases by fibre optic gyros and ring laser gyros [Merhav 1996]. Special low-cost, low accuracy MEMS IMU components [Rios2000, Randle1997, Godha 2006] are finding use in land vehicle, UAV and underwater manipulators. The signal obtained from the accelerometers and gyros need to be processed. The signal processing consists of noise filtering and integration. To obtain attitude information, the rate gyro outputs need to be integrated. For velocity information, attitude information and integral of accelerometer signal would be necessary. For position information one needs attitude information

19

Chapter 2

and integral of velocity signals. This double integration means that any bias in the IMU output may manifest as positional error, which increases with the square of elapsed time. Inertial navigation find wide applications in space missions, satellites and spacecrafts [Cheng 2004, Crassidis1997, Crassidis2003], surveying [Hewitson 2003], aircraft [Enge1990, Frykman 2003], weapon systems [Hyslop1990, Groves 2003, Moody 2003], ships [Scheninge1997, Banbrook 1970], UAV [Kim 2006, Kau2006], submarines [Karlsson 2003] land vehicles [Dissanayake 2001, Hong2003] and autonomous underwater vehicles [Huster2002]. The accuracy requirements for different applications may vary widely. For a planetary mission, one needs that the errors of navigational information should remain within acceptable limits for several months. For tactical applications, the duration of flights may be within minutes. Early INS systems were stabilised platform based. Accelerometer and gyro triads were mounted on a 3DOF gimbals. A pure inertially stabilised platform would maintain the attitude of the platform fixed with respect to the remote stars. This was achieved by a feedback system, which maintained the rate gyro outputs to null [Britting 1968]. Advantage about this system had been lower computational requirement which could be tackled by analog computers and simpler stabilisation electronics. The next version of platform based INS keeps the platform locally level and the north gyro pointing to the geographic north. This required moderate computation which was carried out by on board computer. This system was termed as “semi-analytic” [Britting 1968] platform. The truly analytic platform did not have any gimbals but the IMU was rigidly mounted to the body. The term strap-down possibly appears first in the same reference [Britting 1968]. In a follow-up paper [Britting 1969] an early error analysis for SDINS was reported. For constructional details of platform and strapdown INS the references [Pitman 1962, Lawrence 1998, Barbour2003] can be seen. Now a days global positioning system (GPS) signal is available from the dedicated satellites. GPS signal provides very accurate position (in terms of latitude and longitude). The position of a vehicle obtained from the INS may be corrected using the GPS data, which is free from drifts (which occurs in INS due to integration of bias). Earlier GPS was only used by the military aircraft but now a days as GPS receivers are readily available and it is extensively used by civil air craft, and many other application even by the automobiles. While the GPS system is operated and maintained by the US military, the Russians have their own equivalent called GLONASS (Global Navigation Satellite System). The European Community is planning their own version called the Gallelio.

20

Chapter 2

Before GPS was available other ground based systems such as LORAN (Long Range Navigation) which were in use, which have now been nearly discontinued. There were some effort going on to popularise the LORAN. In [Enge 1990] the authors have analysed the performance of services, which could be provided by a combined GPS/Loran receiver. In a similar paper [Nagle 1990] the authors proposed to implement combined service of the GPS and GLONASS. This paper includes a discussion of the integrity, coverage, availability and timing enhancements that are possible from the combined services. The monograph [Farrel 1998] presents detailed background material, case study relating to navigation and global Positioning system design and analysis. The book [Borre 1997 Part II & Part III] describes the science of geodesy and various aspects of GPS. Processing of GPS data has also been discussed in that monograph briefly. T Upadhyay et al in [Upadhyay 1993] described an experiment with an autonomous integrated spacecraft navigation system using onboard GPS and INS measurements. J. Lobo et al in [Lobo 1995] describe a prototype inertial navigation system, which may be used in land navigation. The complete system including sensors (both their characteristics and performance), their mechanical mount, pc card, cabling and micro controller has been described in detail. The paper [Tan 2001] examines whether angular motion of a rigid body can be computed using accelerometer only. If it can be done, the gyroscope free inertial navigation system can be designed. Here it has been shown that output equation relating to the linear and angular motions of a rigid body relative to a fixed inertial frame can also be derived if a condition is satisfied. In [Napolitano 2002] navigational algorithm of fibre optics gyroscope has been presented. It is pointed out that like all other type of gyroscope it can be used either as an autonomous system or it can be integrated with external information based on Kalman filtering to achieve the best performances. The technical report [Meyers 1988a] specifies the functional performance and specification required for INS simulator, which can be interfaced with the external computer system. Similar technical report [Meyers 1988b] provides functional specifications for a shipboard inertial navigation system (INS) simulator, which can be interfaced with the external computer system whose functional specifications are defined in [Meyers 1988c]. 2.2.2.1 Estimation Problems in Inertial Navigation Inertial Navigation had been a fertile application area for estimation techniques (like Kalman filter) as soon as such techniques were formulated. Many of the early application papers for Kalman Filters were from navigation [Barham1970, Banbrook1970, Batin1970, Bellantoni1970,

21

Chapter 2

Crocker1970, Halamandaris1970]. Estimation techniques are primarily employed in the following navigation problems: •

Alignment

•

Calibration

•

Fusion

Alignment problem and fusion problem in the context of navigation will be discussed in detail in subsequent sections. Calibration problem in navigation context means estimation of gyro and accelerometer bias, nonlinearity and scale factor errors. This problem is closely related to the error modelling and parameter estimation. Calibration is often carried out at the factory or base station and may take a long time. Deviations from calibrations may be modelled with small signal approximation and be carried out during alignment. To formulate alignment problem it is necessary to develop error model. So before going to alignment problem literature survey, it is better to review the work done on error model. 2.2.2.2 Literature on Error Models Error models of the SDINS are important for alignment as well as fusion. The degree of details and the choice of coordinate system may be different. Error models for SDINS have been reported since seventies [Krishnan 1970, Britting 1969]. One of the earliest but very readable treatments of gyro and accelerometer error and methods for compensation was given by [Erzberger 1967]. It is interesting that Kalman filter, which was very new at that time, was applied. In [Park 1995], covariance analysis for SINS system has been presented (i) using existing conventional SINS error model (ii) using modified SINS error model. In modified SINS error model attitude errors are transformed into new state variables, so called pseudo states. Here the concept of equivalency and difference between the two models have also been discussed. Conclusion has been drawn that the two models will give the same result. The analysis takes into account gyro compassing characteristics. [Nebot 1999] provides an error model and also analyses the impact of INS errors on trajectory. The paper [Scherzinger 1996] discussed psi angle error model for a strapdown inertial navigator whose platform azimuth is completely unknown. Error modelling for large initial misalignment has been given in [Hong 2003]. [Nassar 2003] provides a good introduction on INS stochastic error models. In this thesis, the stated objective is to improve SINS error modelling such that a major improvement of navigation parameter estimation could be obtained in different SDINS stand alone applications. Different approaches are investigated for improving inertial error modelling in SINS stand alone

22

Chapter 2

applications. The proposed SDINS error model contains deterministic as well as stochastic errors. Position, velocity and attitude bias errors are usually modelled as deterministic errors while the SINS sensor residual biases are often modelled as stochastic errors. The actual behaviour of SINS sensor errors is investigated by computing the autocorrelation sequence using long data records. Autoregressive (AR) process is introduced as an alternative approach in modelling SINS sensor residual biases. Different methods for the optimal determination of the AR model parameters are studied. A recent work on error modelling is [Eduardo 2006], where the effect of errors on trajectories are also analysed, like in [Nebot 1999]. In [Wu 2006] dual quaternion algebra has been adopted for representing the general displacement of a rigid body and to analyse error characteristics of the strap down INS. Two new error models (i) additive dual quaternion error (ADQE) model and (ii) multiplicative dual quaternion error (MDQE) model have been developed using quaternion algebra. The authors expected that future integrated navigation filter would be facilitated using those error models. 2.2.2.3 Alignment Problem Strap down INS needs to be aligned before they may be used for navigation. Alignment means finding the instrument axis system vectors with reference to a known coordinate axis system. Possible standard/reference axis systems are described in [Stimac2004]. It must be remembered that when the gyros in the platform are made inertially stable and properly aligned, all the gyros would sense zero rotational rates. If on the other hand, the platform is to be kept aligned with local North, East and Vertical axis, the platform must have an inertial rate corresponding to the rotation of the earth. At the equator the north axis would always point towards the north and the earth rate need to be applied only about the north axis. However, at any other latitude, components of earth rate must be added to both North and East axes. This process used to be called the gyro compassing. As would be seen that the terms for stabilised platform based INS had been borrowed even for SDINS alignment. The SDINS instruments are rigidly mounted to a frame. For alignment, one needs to know the relative angles between (and corresponding transformation) the IMU and the reference axis. As the INS is not physically rotated, SDINS alignment is sometimes called “alignment of analytical platform” [Britting 1968]. The earliest literature on the alignment of SDINS is possibly the Ph. D thesis of V. Krishnan in the University of Pennsylvania [Krishnan 1970]. Alignment is typically done in two phases, namely, (i) Coarse Alignment, (ii) Fine Alignment. In coarse alignment the INS axis (the analytic axis) is coarsely aligned [Ali 2004] with respect to the reference axis. Typically the misalignment errors are less than 6 degree that is 0.1 radian (100 mRad). The advantage of the coarse alignment is that after this phase, small angle approximations

23

Chapter 2

may be employed and the resultant equations are often linear. This allows application of linearized filters like EKF to be successfully employed for the fine alignment. However there are methods where such distinctions between coarse and fine alignments need not be made [Cheng 2004, Ali 2004]. When the IMU is of precision grade, SDINS may be aligned from the gravity vector and the earth rotation vector. Such procedures are often called self-alignment. Self-alignment is appropriate when the SDINS mounting platform is stationary. However if the mounting platform motion is small and the oscillation is zero mean, self-alignment is still possible [Ali 2004]. The other possibility of alignment is aided-alignment when help is taken from other instruments. Aided-alignment with the help of an already aligned (and possibly of better accuracy) INS (or INS-GPS) combination is often called transfer alignment [Ali 2004]. Transfer alignment is typically carried out for missiles where the reference INS is mounted on the mother vehicle. However, due to elastic motion of the platform and the missile, filtering needs to be employed. Apart from taking aids from another INS, aided-alignment may also be carried out with the help of GPS [Borre 1997]. When the mother aircraft is in flight the alignment is sometimes called “in flight alignment” [Hong 2003]. However there is a scope for misunderstanding. In flight alignment is generally transfer alignment but it could also be self alignment (rare). 2.2.2.3.1

Self Alignment:

Coarse

In earlier section it has been discussed that in self-alignment in SDINS is the process of alignment from the gravity vector and the earth rotation vector. A good starting point on alignment literature is the survey paper [Ali 2004]. It has been discussed earlier that self alignment can be done in two phases namely (i) coarse alignment (ii) fine alignment. In [Schimelevich 1996] two methods have been discussed for coarse alignment. In one method attitude was estimated using accelerometer and gyro measurements. Gyros are required for azimuth estimation and accelerometers are required for solving the levelling problem. In other method, called gyro compassing, the gyros are used for continuous tracking of Instrumental axes attitude and the accelerometer signals used for determining the DCM. Use of DCM linearises the equations but number of state variables increase. The filtering method used was simple averaging. Readers should be careful because this paper contains printing errors; again there are no references cited. Two coarse alignment methods have been provided in [Jiang 1998] for SINS using the measurements of the local gravity vector and earth rate. Drift, skew, and scale alignment errors associated with course alignment method are also evaluated.

24

Chapter 2

Coarse alignment technique has also been described in [Pham 1992] using traditional INS extended Kalman filter applied for helicopter application. Here common limitation of EKF arises from first order linearisation process have been pointed out. In gyro compassing process the inertial platform is aligned to the external reference frame. Gyro compassing error is affected due to gyro non orthogonality. In [Priel 1992] two-step gyrocompassing process is analyzed and the effect of gyro nonorthogonality on gyrocompassing has been discussed. A scheme to eliminate this error is presented here. This method can be used for high quality platform alignment. The paper [Um 2000] proposes an approach to estimate noise covariance using least square algorithm for a linear, time invariant stochastic system with constant but unknown bias. It is claimed that (i) the proposed method requires no a priori estimation of noise covariance, (ii) it provides consistent estimates, and (iii) it can also be applied when the relationship between bias states and other states is unknown. The method has been applied to strapdown inertial navigation system initial alignment scenario. Simulation results have been provided to show satisfactory performance of the proposed method. Chuanbin [Chuanbin 2004] describes an initial alignment scheme using modified Kalman filter, which takes care of the east gyro drift, which tends to affect simple alignment procedures. 2.2.2.3.2

Self Alignment:

Fine

In [Grewal 1988] a smoothing technique has been presented to calculate the relative azimuth of inertial measurement unit (IMU). It is reported that the algorithm has been implemented on an IBM computer and alignment data from an IMU simulator has been post processed. The smoothing results have also been presented in the paper. The paper [Grewal 1990] discusses about the calibration and alignment scheme for an inertial navigation system. Here the error model has been developed and dual extended Kalman filter is applied to estimate the gyro and accelerometer parameters. It may interestingly be noted that the direction cosine (which method is not generally used now a days due to singularity problem) differential equation and their solution have also been presented. Simulation data have been provided to demonstrate the accuracy and convergence of dual extended Kalman filter used for parameter estimation. In the article [Jiang 1992] an estimation algorithm for computing the misalignment angles is provided. Here also a systematic analysis of the observability of an inertial navigation system in ground alignment situation is presented. From simulation results it has been revealed that azimuth error can be entirely estimated from the estimates of levelling error and its error rate, without using gyro signals.

25

Chapter 2

In [Helmick 1993] an algorithm has been presented where two 3-D sensors have been aligned relatively using information of the targets that are being tracked by both sensors. The algorithm estimates sensor biases and sensor frame orientation errors for a tracking system. A Kalman filter has been used [Saab 1994] to investigate the calibration and alignment of a terrain vehicle navigation application problem when the system is at rest. Simulation results illustrate the performance of Kalman filter. SINS error model using tilt angle has been derived in [Chung 1996] and it is shown that this is equivalent to the conventional error model. The inter conversion of the tilt angle and quaternion errors have also been derived using DCM. Based on the error model the quaternions are computed and analysed during multi position alignment problem. In [Fang 1996] SINS error model for the stationary alignment has been introduced and observability of the SINS error model has been analyzed. Then, on the basis of SINS error model estimation of the azimuth error has been done for the initial alignment of SINS on stationary base. Simulation results show that estimation method is efficient and it greatly reduces the initial alignment time. In the article [Eduardo 2006] initial calibration and alignment algorithm for a six-degree of freedom inertial navigation unit have been presented. After the illustration of individual error models for the gyros and accelerometers full error model of SINS has also been presented. Finally, experimental result based on the initial alignment and calibration has been presented which shows the proposed algorithm is able to obtain position and velocity information. The work [Kong 1999, 2000] discusses a general nonlinear psi angle approach that does not require coarse alignment. In this approach three psi-angles are described using three states. Extended Kalman filter is used to integrate the data obtained from a low cost IMU with GPS. In the work [Zhao 2000] multiple sensor alignment problem has been formulated in an ECEF co ordinate system. Sensor biases are estimated using least square technique. Simulated and realtime radar data are used to evaluate the performance of the proposed algorithm. The performance of proposed algorithm has been compared with the standard one. A completely different approach has been used for fine initial alignment problem in [Yong 2002]. Here the dynamic programming theory has been applied to the problem of alignment of SINS. The authors discuss how to apply dynamic programming (optimal control) strategy taking the state as the feedback and how to find the strategy to achieve the alignment on the condition of the quadratic performance index. A patented method for alignment of seekers (mounted on missiles) has been disclosed in [Perkins 2003]

26

Chapter 2

The article [El-Sheimy 2004] describes the method to processes the gyro and accelerometer measurements with multiple levels of wavelet decomposition. The proposed method has been applied to a navigational system. The results show quite accurate alignment, fast convergence and reduced error covariance of three attitude angles. 2.2.2.3.3

Transfer and Aided Alignment

In many applications it is necessary for a vehicle to carry more than one IMU for various reasons. Command module IMU is called master IMU and other IMU is called slave IMU. It is necessary to align the slave IMU to the master IMU assuming master IMU was itself inertially fixed. This process is known as transfer alignment. Here brief review of some publication on transfer alignment has been presented. In [Hagan 1992] a transfer alignment method is presented using only accelerometer measurements. The benefits are claimed in this scheme are (i) freedom to choose IMU gyros solely based on in-flight considerations (ii) the ability to update the transfer alignment by simply making new measurements of gravity and processing the measurements. In this paper the developed algorithm, error analysis and laboratory results are presented. Results of various transfer alignment techniques using Kalman filter have been presented in [Ross 1994, Rogers 1996]. In that publication various transfer alignment issues such as the effect of wing flexure vibration on instrument error estimation and rapid alignment concepts have been addressed. The implementation of Kalman filter in inertial guided weapons is also included. In [Rogers1998] the two models based on the computer frame and perturbation error using position observation are formulated in transfer alignment Kalman filter algorithm. Performances are compared between the two INS error models in terms of (i) filter convergence rates, (ii) IMU navigation solution heading compared to the reference IMU heading, and (iii) post-alignment issues. Between the models the perturbation error model shows the better result. Transfer alignment for a weapon has been discussed in [Groves 2003]. In [Lim 2001] an error compensation method is proposed to reduce the alignment errors in transfer alignment problem of INS subjected to roll and pitch motion of the ship. In this method a linearised error model for the velocity and attitude matching transfer alignment has been derived by linearising the nonlinear measurement equation. The simulation results demonstrate the effectiveness of the present method in azimuth alignment. A recent US patent [Buchler 2006] has been awarded for transfer alignment. Gao [Gao 2007] used velocity matching algorithm for transfer alignment problem. Robust H ∞ filter and Kalman filter have been used for estimation of error angles. It is claimed that H ∞

filter converges faster than Kalman filter. Readers should be careful because there are errors in

27

Chapter 2

navigation equations and some simulation parameters are missing in that paper. Also in simulation result it is not clear what kind of plot has been shown. In [Zaho 2005] transfer alignment for ship has been presented. Transfer alignment has been achieved based on angular velocity mismatch. Kalman filter has been used to estimate misalignment angle. It has been claimed that the method is effective and alignment can be achieved preciously. Hao [Hao 2006] applied unscented Kalman filter in transfer alignment problem. UKF may be advantageous for large initial heading error uncertainty. 2.2.2.3.4

In Flight Alignment

As discussed earlier initial alignment is performed when the vehicle is at rest. But many missile systems require accurate alignment of the on board INS during flight in order to achieve accurate target tracking. So in-flight alignment (IFA) has been developed to estimate the navigational error while inertial sensors are on moving base. In this case the alignment process is performed after launching using accelerometer, gyro data and the data provided from external source during flight. In flight alignment for weapon systems may be transfer alignment with special methods to take care of elastic vibrations. It may also be performed with the help of other aids like Doppler radar or laser based auxiliary equipment. Here brief review of some publication on in flight alignment has been presented. In the case of SDINS attitude errors, the uncertainty caused by linearization of the system degrades the performance of the filter. Robust filters are often applied to overcome this problem. In [Meskin 1992] an error model has been formulated that appropriately describes the INS behaviour during IFA. Observability analysis has been made for vertically damped and undamped INS during IFA. The analysis demonstrates the observability enhancement of IFA. Simulation results have also been presented which confirm the analytical results. Rapid transfer alignment algorithm using Kalman filter for inertially guided air launched munitions has been presented in [Shortelle 1998]. In this alignment procedure it is required to execute only a brief wing rock manoeuvre. The simulation results demonstrate the rapid convergence of the estimator. It has been discussed earlier that psi angle model has been used in initial alignment. Similarly in [Yu 1999] psi angle model and the equivalent tilt (ET) model have been developed and applied to IFA in presence of large attitude errors. To obtain equivalent velocity and attitude error models, the quaternion error is divided into (i) additive quaternion error and (ii) multiplicative quaternion error. The rotation vector error is introduced as a new variable. Exact relationships between these attitude errors are also derived. Proposed error model has been validated for IFA with large

28

Chapter 2

attitude errors. [Brown 2002] describes an in-flight GPS aided alignment using kinematic carrier phase information and a conventional Kalman filter. A robust extended filter for in flight alignment has recently been described in [Yu 2003]. Various velocity aided in-flight alignment methods are proposed by combining the robust filter H 2 / H ∞ with the error models. Simulation results demonstrate that the proposed filter effectively improves the performance. In this paper, a robust filter and various error models for the uncertainty are presented. The analytical characteristics of the proposed filter are also investigated. The results show that the filter does not require the statistical property of the system disturbance and that the region of the estimation error depends on a freedom parameter in the worst case. Then, the uncertainty of the SDINS is derived. Depending on the choice of the reference frame and the attitude error state, several error models are presented. Jaacov [Jaacov 2003] in his patent describes a scheme for in flight alignment when the aircraft measurements get delayed. A complete departure of alignment technique, which involves in-flight and transfer alignment is proposed in a US patent [Kau 2006], where recursive inversion of matrices have been applied. The scenario is a ballistic missile where the INS of the reentry vehicle is aligned with the INS of the launch vehicle before these are separated. 2.2.2.3.5

GPS Based Alignment

In [Park 1998] the SINS error model for stationary and in-flight alignment has been described and it is shown that by employing GPS data the observability and performance of the alignment process can be improved. Covariance analysis result has been presented to analyse the observability and performance of GPS INS system. In [Hide 2003] Adaptive Kalman filtering technique has been used for GPS INS integration for dynamic alignment of the inertial sensor errors in a real life marine application. Results show that the integration significantly improves the dynamic alignment speed and overall navigation accuracy. 2.2.2.4 GPS INS Estimation and Data Fusion The data available from GPS can be integrated with INS system of the vehicle using estimators to obtained higher accuracy in position, attitude and other states of the system. This specific application for estimation technique started nearly three decades ago and attracted many researches. As an estimator mainly Kalman filter and its extended version is being used due to its simplicity and time efficiency. Although this filter has many limitations and now a days other efficient nonlinear filters and high-speed computers are available to designer. These motivate the designers to apply other nonlinear filters such as UKF, PF, to the navigation system. It should be

29

Chapter 2

pointed out here that no robust filter such as risk sensitive filter, H ∞ filter has been applied to navigation system to find out the estimators performance under model mismatch and large initial condition mismatch cases. Here review of the literature related to GPS INS estimation and data fusion is briefly summarized. 2.2.2.4.1

GPS INS Estimation Using Kalman Filter

Here the literature related to GPS INS estimation and data fusion using Kalman filter and its extended version are reviewed. Newton [Newton 1990] compares four filters (least square filter, Kalman filter, two Kalman polar filters) used to smooth the angle only data received by a sensor on a moving platform. In the paper [Poor 1992] the algorithm to estimate the statistical parameters such as system covariance matrix, bias, drift parameters has been presented. Estimation of these parameters are necessary to estimate the position better. Kalman filter’s robustness with respect to adverse environment and non-favorable input condition has been enhanced in [Karatsinides 1994] by successfully identifying that situation. Two robustification techniques such as (i) handling of GPS position bias (ii) proper identification of measurement noise have been discussed. In [Doraiswami 1996] navigational states of a Vehicle are estimated using Kalman filter from the range measurements obtained using beacons (transmitter receivers). The paper [Um 2000] presents least square algorithm based noise covariance estimation technique for a linear, timeinvariant, stochastic system with constant but unknown bias states. It is claimed that the algorithm requires no a priori estimation of noise covariance, and can also be applied when the relationship between bias states and other states is unknown. The proposed method has been successfully applied to strap down inertial navigation system. A modified Kalman filter is used [Song 2000] for filtering of target glint in homing engagement of a ship-to-ship missile. The proposed filter has decoupled range and angle channels so that it enjoys computational efficiency. The proposed algorithm has been tested by a series of simulation runs and its superior performance compared with the other filter structure has been reported. Honghui and Moore [Moore 2002] suggest processing of GPS INS nonlinearties prior to Kalman filtering. The preprocessed INS equations act as process dynamics where as GPS preprocessed data are used as measurement inputs. The advantage of this process is that KF algorithm can be implemented to achieve significant computational saving. Y. H Kim et al [Kim 2003] compare the results obtained from covariance analysis with the results of Monte Carlo simulation for the performance of spacecraft attitude control systems.

30

Chapter 2

In [Portas 2004] four techniques such as estimation with blocks of consecutive, correlated measurement difference, least square and Kalman filter are applied and compared for correcting radar biases for air traffic control (ATC) applications. The comparison is based on accuracy and cost efficiency and implementation issues. An optimal and efficient algorithm has been proposed in [Crassidis 2000] for attitude and position determination from line of sight measurement. It has been claimed that the new algorithm is optimal, non iterative and computationally efficient. The attitude and position error covariance matrix is shown to be equivalent to the Cramer Rao lower bound, which established that the proposed method is optimal. Hanlon et al [Hanlon 2000] propose estimation in the scenario where the true system model is unknown. In this scheme multiple model adaptive estimation (MMAE) which is nothing but a bank of Kalman filter has been used. Each filter assumes a different internal model. Hypothesis testing algorithm analyze the residual from this bank of Kalman filter and determine which Kalman filter model will exactly match with the true model. In [Chatterji 1997] the position of a commercial aircraft with respect to the runway during night landing and takeoff is determined by Kalman filter based fusion of multi sensor data obtained from different source. The data source may be (i) Known structure of the airport lights, (ii) video images acquired by an onboard video camera, (iii) position estimates from an onboard GPS, and (iv) data from an attitude indicator. The paper [Omedo 1998] provides performance of GPS aided inertial navigation system (GAINS) for missile systems made from Raytheon Systems Company. In a case study the paper discusses about the situation where a missile is fired from a ship to intercept an incoming target. The missile and the target are tracked by a ship-based radar system. A Kalman filter is implemented on the interceptors to integrate the data obtained from on-board inertial navigation system, GPS, and ship-uplinked radar. Simulation results show that the accuracy is improved by using both radar and GPS measurements compare to radar only measurement. The paper [Sukkarieh 1999] combined Global Positioning System (GPS) data and an inertial measurement unit (IMU) data for autonomous land vehicle applications. Detection of possible faults both before and during the fusion process has also been discussed. Results of the fusion process are also presented. In [Jang 2000] Kalman filter has been used to estimate the covariance in response to the changing dynamics in GPS navigation. It had been pointed out there that this method can also be applied for fault detection.

31

Chapter 2

Different design and implementation aspect of inertial navigation system, inertial measurement unit (IMU) and GPS have been discussed in [Walcho 2000]. It has been shown that the combination of the sensors data will provide good estimates of position at each instance. The paper [Randle 1997] describes the data fusion of GPS/inertial navigation system (GPSI) with a low cost inertial measurement unit (IMU) using Kalman filter algorithm. Case study shows that the IMU can be successfully integrated with GPSI system using Kalman filtering which improves the system performance. In [Dissanayake 2001], an algorithm and simulation results for IMU GPS inertial navigation system have been provided for land vehicles using information filter. The real life implementation has also been discussed through a multiple observation inertial aiding algorithm. In [Kim 2005], LOS, gyro measurements and dynamic models are coupled using extended Kalman filter to determine relative attitude, position and gyro biases. The quaternion is used to describe the relative kinematics. Some important quaternion identity and quaternion linearisation technique have been derived which may be helpful for beginner. Simulation results show that the estimator satisfactorily determines relative position and attitude. The paper [Roumeliotis 1997] provides interesting discussions on the navigation on the Mars where GPS data is not available. Here the measurement from the Sun sensor has been fused with INS using EKF to obtain the higher accuracy. The estimation accuracy exponentially decreases with decrease in frequency of the sun sensor data. In [Roumeliotis 1999], filter combines the attitude rate information from the gyros and the absolute orientation measurements through error state form of the Kalman filter to obtain attitude estimation. Sukkarieh [Sukkarieh 2000] in his thesis develops a low cost, high integrity inertial navigation system using global navigation satellite system (GNSS) along with Kalman filter for use in autonomous land vehicle. Benlin [Xu 1996] has proposed a new self-learning navigation filter based on probability space and non-Newtonian dynamics. To make the new filter flexible and responsive to a navigation environment, some parameters have been left for filter to choose their values. The proposed filter has been tested with both simulated and real time navigation data and compared with the Kalman filter. The paper [Moon 1999] loosely integrates GPS and INS data and it has been shown that the integrated system provides more accurate navigation solution than the inertial or the GPS-alone. In [Hyslop 1990] the integration of GPS/INS for standoff land attack missile (SLAM) has been discussed in detail.

32

Chapter 2

In [You 2001] vision and gyroscope sensor data are fused with EKF to improve tracking stability and robustness. The paper [Vik 1999] presents an observer based on nonlinear control theoretic approach to estimate gyro biases, scale factor errors and misalignments using GPS measurements. The Kalman filter theory is presented in a very lucid manner in [Levy 1997]. The applicability of Kalman filter in GPS/INS navigation system and GPS only navigation system has also been discussed. 2.2.2.4.2

GPS INS Estimation Using Post Kalman Filter

As discussed earlier availability of efficient nonlinear filtering technique and high speed computers are encouraging the designers to design post Kalman nonlinear filters for GPS INS data fusion. Mainly UKF and PF have been reported in the literature to integrate the GPS INS data. Crassidis [Crassidis 2005] discussed different reference frame and their inter conversion which may be helpful for beginner. UKF and EKF have been used to integrate the INS and GPS to obtain position, velocity, attitude, gyro and accelerometer bias and scale factor estimation. Robustness of UKF with compare to EKF has been reported. In [Vandyke 2005], a similar paper unscented Kalman filter (UKF) has been used to solve the spacecraft dual estimation problem with greater accuracy than that obtained from extended Kalman filter (EKF). In [Crassidis 2005] a spacecraft attitude is estimated using UKF. The filter formulation based on standard attitude measurements using a gyro-based model and attitude dynamics is described using quaternion. Simulation results indicate higher robustness and faster convergence compare to extended Kalman filter under large initial condition. Now a days with the increase of computational power of on board computer particle filter is now being considered for GPS INS data fusion problem. In [Dezert 1999] PDAF (probability data association filter) has been applied for improving the accuracy of autonomous strap down inertial navigation systems (SINS). The proposed method has been applied to terrain aided navigation (TAN) system. Simulation results significantly show improvements can be achieved using PDAF. In [Boberg 2002] PF and EKF have been applied to integrate the data obtained from GPS and INS. The results obtained from two estimators have been compared. In the paper [Iremus 2004], Rao-Blackwellization and particle filtering have been applied for an INS/GPS scenario. Simulation results compare the variance of the estimates with posterior Cramer-Rao bound. Spacecraft attitude has been estimated using particle filter in [Cheng 2004]. The filter formulation is based on the star camera measurements using attitude dynamics-based model for attitude

33

Chapter 2

propagation. It should be pointed out that Rodrigues parameters are used for attitude parameterization. It is reported that PF converges even for large attitude initial error. In [Crassidis 1997], a real-time estimation algorithm called predictive filter algorithm is derived for spacecraft attitude estimation without the utilization of gyro measurements. Predictive filter algorithm is combination of KF and MME algorithm and it can be used for non Gaussian noise sequence. The algorithm indicates that the proposed filter estimates the attitude of a spacecraft quite accurately. A constrained filtering method is developed in [Chiang 2002] to avoid the problem due to singularity of covariance matrix in classical filtering methods. The proposed estimator has been applied for attitude estimation problem.

2.3

Literature Survey Solution Domain

In this section a brief literature survey on the nonlinear filtering techniques has been presented. Here the survey highlights about the previous attempts to improve the efficiency of nonlinear filtering methods both robust and non-robust cases. On the light of this survey it can also be justified the necessity of the novel filtering algorithms which are proposed in the later chapters. For linear Gaussian cases Kalman filter gives the optimal estimation of states. For nonlinear and or non Gaussian cases no general optimal solution exists in the literature till date. The very first attempt for nonlinear filtering has been made using extended Kalman filtering techniques and its different variants. These methods are already subject matter in many books [Brown 1996, Grewal 2001]. So the literature available on Kalman filter and extended Kalman filter and their variants have not been discussed in this chapter. Many techniques for nonlinear filtering other than extended Kalman filtering have been proposed in literature. These techniques as a whole are known as post Kalman filtering techniques. Survey presented in this chapter takes up recent filters such as (i) unscented Kalman filter (UKF), (ii) pseudomeasurement, (iii) particle filter, (iv) mixed Gaussian, (iv) adaptive grid etc. Some of the post Kalman filtering such as particle filter, Gaussian sum filter and UKF have been described briefly in the book by Ristic [Ristic 2004a]. Another aspect of filtering is to estimate the states under uncertain plant dynamics. Those types of filters are known as robust filtering techniques. Literature available on that has also been reviewed and summarize here. Finally the literature on theoretical lower bound (known as CRLB), which can be achieved by any unbiased optimal estimator has been reviewed.

34

Chapter 2

2.3.1 Linear Regression Kalman Filter (LRKF) Techniques For nonlinear Gaussian model it is common practice to apply simple extended Kalman filtering techniques for estimation of states. The EKF has two major disadvantages namely (i) the filter uses 1st order linearisation (ii) the computation of Jacobian. To avoid these disadvantages some methods of filtering such as unscented Kalman filter (UKF), central difference filter (CDF), divided different filter (DDF) etc are proposed. These methods as a whole are known as linear regression Kalman filter. Like EKF, in LRKF the probability density function are also approximated as Gaussian one.

2.3.1.1 Unscented Kalman Filter (UKF) The UKF is first proposed by Julier and Uhalmar [Juiler 1996] and after that many applications of this filter have been made in the field of target tracking, navigation etc. The literatures on application of UKF in the field of target tracking and navigation have been summarized in previous section. Here we review the literature related to theoretical development of unscented Kalman filter, or sigma point filter and their variants. In the paper [Juiler 1996] the authors proposed a new algorithm for nonlinear filtering where a general method for predicting mean and covariance has been proposed by a nonlinear transformation. Using this transformation a new filter has been proposed. The transformation is known as unscented transformation and the filter is known as unscented Kalman filter (UKF). After that a series of paper have been published by the same authors, which established the new filtering theory in a solid foundation. In a similar paper [Juiler 1997] Julier and Uhlmann have described the unscented transformation and algorithm of the newly proposed filter named as UKF. An application of this filter to a real life system has been presented. In [Juiler 2000] the algorithm of UKF developed previously has been presented and discussed. The performance of EKF and UKF has been compared for a test system. In appendix (i) properties of the higher moments of the sigma points, (ii) justification for approximating a pdf by a set of samples, which match the moment, (iii) modified form of the algorithm have been presented. In general for an nth order system the number of sigma points required for computation are 2n+1. In [Juiler 2002a] it has been shown that the number of sigma point can be reduced to (n+1). The major advantage of this reduction is the reduction of computational load of the filter. In [Juiler 2002b] the generalization of the unscented transformation has been made which is known as scaled unscented transformation. It is claimed that the method calculates the mean and covariance with an accuracy of second order with out calculation of Jacobian or Hessian. In

35

Chapter 2

[Juiler 2004] the advantage of UKF over EKF has been discussed. The motivation, development and implementation of unscented transformation have also been discussed. After UKF has been proposed, the filter is being extensively used in various field of application. Most of the applications related to the thesis have been discussed in the previous section. [Wan 2000a] lucidly illustrated the concept of sigma points using attractive figures. The estimator has also been used in dual estimation problem. In [Wan 2000b] the authors advocate the use the UKF as a better substitution of EKF at a comparable level of complexity. In [Merwe2001a, Merwe2001b] the square root unscented Kalman filter (SRUKF) has been proposed and the filter has been applied to a robot arm problem. The simulation results show that the SRUKF show better results than EKF and UKF. In the paper [Briers 2003], Rao Blackwellisation technique has been introduced to calculate the tractable integrals found in the unscented Kalman filtering technique. The newly developed filter has been named as Rao Blackwellised UKF filter. The advantage of this new variant of UKF has been stated as (i) reduction in quasi Monte Carlo variance (ii) decrease in computational load. Wu [Wu 2005] discuss the augmented and non-augmented unscented transformation for nonlinear system with additive noise. Based on the two types of transformation augmented and non-augmented UKF has been derived. It is shown that the basic difference between them is that the augmented UKF draws a sigma set only once while the non-augmented UKF has to redraw a new set of sigma points after the time update to incorporate the effect of additive process noise. An example has been provided and the performances of these two filters have been compared.

2.3.1.2 Central Difference Filter (CDF) and Divided Difference Filter (DDF) In the paper [Lefebvre 2002] linearization process using statistical linear regression has been discussed through selected points as used in CDF/DDF/UKF. It is shown that UKF is a special case of LRKF. Looking the UKF as a special case of LRKF the performance of estimator can be well understood and the adaptation of the estimator can be made where the original form does not give good result. The linearization process based on square root factorisation of the output covariance matrices has been proposed in [Schei 1997]. The advantage for this type of linearization technique has been stated as (i) no Jacobian or Hessian matrix need not to be computed; (ii) the proposed linearization process is more accurate. In [Niels 2000] state estimators for nonlinear systems have been derived based on polynomial approximations obtained rather than using usual Taylor series approximation. Using this

36

Chapter 2

technique two state estimators DD1 and DD2 (DD means Divided difference) have been derived. It is shown that under certain assumptions the estimators perform better than estimators based on Taylor series approximations. A vertical falling body problem has been solved using EKF, DD1, and DD2 filter. It was reported that among them DD2 filter gives the best performance. A conference paper containing similar material has appeared in [Schel 1995]. Gaussian filter and mixed Gaussian Bayesian filter have been formulated in [Ito 2000]. Based on this formulation two filters namely Gauss Hermite filter (GHF) and central difference filter (CDF) have been developed. The GHF, UKF, CDF, EKF have been implemented for highly nonlinear system. The simulation results show that the CDF and GHF perform better than UKF. Interestingly in that publication UKF is called as Julier Uhlmann filter. In the paper [Merwe 2003], limitations of EKF have been highlighted. The different type of LRKF such as UKF, SRUKF, CDKF, SRCDKF, and particle filter with different type of proposal have been presented. The filters are applied to state, parameter and dual estimation problem.

2.3.2 Pseudo Measurement Filter In this type of filter nonlinear measurement equation is manipulated to a linear like structure measurement equation. It is not always possible to transform a nonlinear measurement equation into a linear one. If the linear manipulation is possible the modified measurement is known as pseudo measurement and the estimator known as pseudo measurement estimator. Here literature related to pseudomeasurement filter (PMF) has been summarised. The pseudolinear filter has been developed and applied to bearing only tracking in [Aidala 1982]. In [Song 1985] the pseudo measurement has been derived for the homing missile problem applied to bearing only measurement. Simulation result has been provided to compare the performance of PMF with EKF and Modified gain EKF. In [Song 1988] pseudomeasurement filter has been proposed for target tracking problem. The system dynamics is linear and measurement is nonlinear. The nonlinear measurement is manipulated as a linear one and PMF has been applied. PMF, EKF, MGKF, and conventional sub optimal Kalman filter (CSKF) have been applied. Among them the authors advocate to use the PMF as all the filters performed similarly but computational time is less for PMF. In [Tahk 1988] MGKF and PMF have been applied for filtering problem with kinematics constrain. RMSE for the two filters have been provided for position and velocity estimation. In [Lin 2002] PMF has been applied for 2D BOT problem. The filter performance has been compared with PF and EKF. It has been reported that among the three filters PMF performs as well as PF. The use of PMF has been preferred due to less computational load with respect to PF.

37

Chapter 2

Also in [Srinivasan 2005] PMF has been successfully applied for exo atmospheric IR tracking of ballistic object. UKF and EKF have also been applied to this problem and the performance of PMF is compared in terms of robustness, accuracy and computational load. Considering all three aspects the authors suggest the use of PMF in that particular problem. The algebraic manipulation has been provided to obtain linear measurement equation from nonlinear measurement equation in [Doraiswami 1996].

2.3.3 Approximate Grid Based Filter In grid-based methods, the posterior density function has been approximated in state space using grids and weights associated with each grid point. The grids should be sufficiently dense to get a good approximation of the posterior density function. Approximate grid based filters also called “point mass filters”. Hidden Markov model (HMM) filter is an application of such approximate grid based method. In [Rabiner 1989], the theoretical aspect of HMM in statistical modelling has been reviewed. It has also been shown how the HMM can be applied to practical problems. In a similar paper [Rabiner 1986] the theory HMM has been discussed lucidly. Grid based method has been discussed in [Arulampalam 2002] where emphasis has been given on implementation. The methodology has been used for speech processing [Xianya 1990] and image processing [Veldhuizen 1998]. The point mass filter has been applied to nonlinear navigation problem in [Bergman 1997]. The performance of the filter in terrain aided navigation system has been compared with CRLB. RMSE has been provided to show that the grid based point mass filter’s performance is close to achievable lower bound, which can be obtained by ideal optimal filter.

2.3.3.1

Adaptive Grid Filter

One disadvantage of approximate grid method is its high computational load. As the dimension of the state space increases the computational load increases dramatically. Another disadvantage is that the grids are placed in a predefined point and therefore cannot be partitioned unevenly to give greater resolution in high probability density region. To avoid this technique adaptive grid based filter has been introduced where the grid points can be adapted in each recursion. In [Bergman 1999 b] a truncation operation has been made which is equivalent to the grid adaptation technique. In truncation process the new grid point has been introduced in high probability region removing grid points in low probability areas. A new grid adaptation technique has been proposed in [Simandl 2002] where minimum but sufficient number of grid points necessary for estimation has been found out. It is claimed that the

38

Chapter 2

new algorithm reduces the computational load without loss of accuracy in the estimation process. A comparison has been made between proposed algorithm and the standard point-mass technique using a numerical example. In [Li 1996] the term “adaptive grid” has been used in the context of multiple model (MM) estimation. The main idea of the algorithm is to start with course grid points and the grids are adjusted according to current estimate and measurement residual.

2.3.4 Particle Filter Recently a more general numerical integration based approach has become very popular for state estimation in nonlinear non Gaussian system. The method numerically generates the probability density function of the state using some points called as particle. The method has been proposed by Gordon et al in 1993 [Gordon 1993]. The method is known as particle filter or Sequential Monte Carlo filter or Condensation filter or bootstrap filter. In the monograph [Doucet 2001 a] the theoretical development and application in various fields have been discussed. As soon as the method is proposed a large number of researchers have been attracted. As a result a huge literature exists on its theoretical development, variant and application to different field in science and technology. The paper [Carpenter 1999] may be considered as a good starting point on particle filtering. Here the authors presented the basic sequential importance resampling (SIR) algorithm in the back ground of Bayesian estimation. An example of BOT problem has been presented where the particle filter has been applied successfully. In the book [Ristic 2004a] a tutorial on particle filtering has been presented. Various resampling scheme choice of importance density function has been discussed in detail. Various variant of particle filter available in literature are summarised and discussed briefly. In the tutorial paper [Arulampalam 2002], algorithms of different numerical nonlinear filtering methods have been presented. The main emphasis has been given on the particle filtering methods and its different variants. Detail algorithm of SIR particle filter with different resampling scheme has also been presented. The algorithms of different variants of particle filter such as auxiliary particle filter, likelihood particle filter, and regularised particle filter have been summarised. The filters are applied to a very simple nonlinear Gaussian 1D problem. The performances of different filters have been compared. The paper should be very helpful for beginners and as well as designers. In [Doucet 2000], reason for resampling has been described. The possibility of existing optimal proposal has been addressed. It has been shown that for linear Gaussian process the optimal

39

Chapter 2

proposal density function exists. For nonlinear case similar proposal can also be used after linearising the process dynamics, although the proposal will no longer be optimal one. Rao Blackwellisation process for sequential importance sampling has also been discussed. Similar Discussion with elaborated algorithm has been made in the technical report [Doucet 1998] by the same author. In [Andriseu 2001], the Rao Blackwellised particle filter method has been applied to more complex hybrid models. The model can be augmented with artificial variables that enable to apply the said technique. Also in [Orton 2002, Hue 2002a, Hue 2002b] the developed theory of particle filtering has successfully been implemented for multiple target tracking. In [Doucet 2001 b], particle filtering and smoothing algorithm have been presented for Jump Markov linear system. It is claimed that the method can parallely be computed for real life application problem. In a similar publication by the same group [Andrieu 2003], again the particle filter for Jump Markov system has been discussed. The methodology has been applied to estimate the states from time varying auto regressive process. Applicability of the methodology to complex multi target tracking scenario has been investigated. In [Crisan 2002], some useful convergence results for particle filtering methods have been summarized for practitioners. The efficiency of the particle filter greatly depends upon the clever choice of the proposal. Using appropriate proposal, the higher efficiency can be achieved using small number of particle thus decreasing computational load. As discussed earlier for nonlinear system no optimal proposal exists. So the researchers propose different functions as a proposal density function. The simplest choice of proposal is the prior probability density function. In another approach [Merwe 2000], the proposal is assumed to be Gaussian with the mean and covariance obtained from other recursive nonlinear filter such as EKF and UKF. These types of particle filters with KF and EKF proposals are known as extended Kalman particle filter and unscented particle filter (UPF) respectively. The proposed UPF has successfully been applied to target tracking problem in [Rui 2001]. To decrease computational load of PF posterior pdf can be approximated by a Gaussian pdf and the particle can be drawn from that pdf for the next iteration [Kotecha 2003] thus avoiding the resampling step. This variant of PF is named as Gaussian sum PF. In [Huang 2004] a hybrid importance function has been proposed for particle filtering which encompasses the advantages of both the posterior and prior importance functions. It is shown through simulation that the PF with hybrid proposal density function will provide better results than PF with prior proposal. The paper [Pitt 1999] introduces an auxiliary variable to particle filtering problem to make it robust to an outlier, although the robustness of the proposed filter has not been clearly stated or exemplified in that paper. The proposed method is known as Auxiliary particle filter (APF). R.

40

Chapter 2

Karlsson at al [Karlsson 2000] applied the APF to a nonlinear target tracking problem immediately after it has been proposed by Pitt and Shepard. Very recently new interpretation of APF has been provided in [Johansen 2008]. Some times in the real application case, the marginal probability density of the filter is our only interest to know. This requirement motivated the authors [Klaas 2005] to design an algorithm, which provides marginal filtering distribution. The idea has also been extended to auxiliary particle filter to formulate marginal auxiliary particle filter. The computational cost has been reduced in this new variant of particle filter.

2.3.5 Robust Filtering Kalman filter (KF) gives the optimal estimate for the linear Gaussian cases when system model and noise covariance are perfectly known. The performance of the Kalman filter degrades rapidly for uncertain noisy system dynamics. These drawbacks prompted several researchers to formulate various robust estimators. In robust estimation, two approaches are used. One is to robustify the Kalman filter while the second approach is to derive robust estimation algorithms based on different types of cost functions. The Risk sensitive (RS) filter and H ∞ filter belong to the second category. As the thesis is focused on risk sensitive filter the literature available on it has been surveyed in detail while literature relating to robust Kalman filter and robust H ∞ filter has been surveyed very briefly.

2.3.6 Robust H2 and

H ∞ Filter

Robust Kalman filter or H2 filter has been designed for Gaussian linear system that will guarantee a minimum bound on the estimation error variance in the presence of system uncertainty. The H ∞ filter minimizes the worst case energy gain from the noise input to the estimation error. So H ∞ filter is a worst case design method that assumes no a priori knowledge of the noise statistics. While guarantying the worst case performance of the filter, it sacrifices the average performance. There are two approaches namely (i) Riccati equation based approach (ii) LMI based approach for solving both type of estimation problem.

2.3.6.1

Riccati Equation Based Approach

For a robust H2 filtering design general technique is to minimise the upper bound of the error covariance matrix. The paper [Peterson 1991] investigates the problem of state estimation in presence of plant uncertainties. In a theorem it has been stated that a stable quadratic state

41

Chapter 2

estimator can be obtained by solving a Riccati equation. Although the proof of the theorem has been omitted in that paper. Robust Kalman filter has been derived in [Xie 1994] for a discrete time system where norm bound parameter uncertainty is present. The cost function is derived as the error variance of the filter lies with in a bound for all admissible uncertainties. In [Zhu 2002] a similar cost function has been used for optimisation. The problem of both finite and infinite horizon Kalman filter for time varying norm bound parameter uncertainty has been discussed. The uncertainty may present in system matrix or output matrix or in both. A Riccati equation based robust filter has been derived for that type of uncertainties. Similar type of uncertainty has been considered in [Theodor 1996]. In that paper a priori filter for linear uncertain system that guaranteed a bound on error covariance matrix has been proposed. Here also both finite and infinite horizon cases have been considered. Similar type of filter for the similar type of system has been discussed in [Souza 1999] where the system is subjected to a measurable input. In [Saab 2004] an algorithm has been proposed where the states of a nonlinear system have been estimated to yield bound error covariance. It has also been shown that the upper bound of the error tends to zero when the state noise covariance tends to zero. A numerical example with nonlinearity has been presented in the paper. In a recent publication [Dong 2006] a robust Kalman filter has been derived so that upper bound of the error covariance matrix can be minimized. The system has been considered as a discrete one and the noises are considered as unknown covariance white and additive. Probably in [Nagpal 1991, Grimble 1990] the theory of H ∞ filtering has been proposed. In [Grimble 1990] a polynomial approach to the solution of the L2 linear optimal filtering problem has been discussed. Computational algorithm for the solution of H ∞ estimation has also been described. The difference between H ∞ and L2 filter has been illustrated in frequency domain. In [Nagpal 1991], system has been considered in state space domain. The necessary and sufficient condition for existence of H ∞ estimators for finite and infinite horizon cases has been derived for (i) known initial conditions and unknown plant and measurement noise (ii) both unknown initial condition and noises. H ∞ smoother has also been presented in the paper. Robust H ∞ filter has been presented in [Xie 1991] for a class of linear periodic system subjected to norm bound time varying parameter uncertainty. The filter guaranteed quadratic stability and H ∞ performance index.

In a tutorial [Shaked 1992] the different aspects of linear time varying H ∞ estimator for continuous and discrete time case have been considered. Basic preliminaries have been provided where the meaning of H ∞ cost function has been discussed.

42

Chapter 2

In the paper [Fujitat 1993] a necessary and sufficient condition for a finite horizon H ∞ filtering problem of linear discrete system has been derived. The filter has been applied to 3-D shape recognition. Theory of linear estimation in Krein space has been developed in [Hassibi 1996]. It has been shown that H ∞ filtering, quadratic game theory, risk sensitive control and estimation can be considered as the special case in Krein space estimation theory. H ∞ filter for continuous time process and sampled measurement has been considered in [Mirkin

1995] for known and unknown initial condition. Kalman filter, H2 filter and H ∞ filter have been presented in continuous time case in [Rawicz 2003]. The filters are applied to target tracking and the performances both in transient and steady state have been considered. Different design consideration of robust parameter has also been discussed.

2.3.6.2 LMI Based Approach In another approach H2 and H ∞ filtering problem can be solved using linear matrix inequality (LMI) approach. In the book [Boyd 1994] various common problems arise in the control theory has been reduced such that they can be solved using convex optimization problem. The book may be useful to learn the application of LMI in solution of real life problem. Using robust stability criterion a LMI based robust H2 filter has been proposed in [Boyd 1994] for LTI discrete time system for polytopic uncertainty. Some numerical examples have been solved to show the superiority of the method. In [Minyue 1999], a LMI based new technique has been provided to design a robust Kalman filter for norm bound uncertainty. An optimized scaling parameter has been introduced to improve the design methods. Robust H ∞ filtering problem has been solved in [Minyue 1999] for both continuous and discrete time system using LMI technique. It has been claimed that the method may be useful due to recent advancement in convex optimization. In [Gerome 1999] design problems in discrete-time systems subjected to parameter uncertainty have been solved for H2 and H ∞ filtering using LMI approach. Comparison between H2 and H ∞ filter has been provided there. In the paper [Shaked 2001] two methods for filter designing have been proposed for discrete time system with polytopic uncertainty. The first approach is a stationary robust filter designing method based on the relaxation of quadratic stability requirement. The second approach is solution of a SDP subject to LMI. The results are compared with different available robust

43

Chapter 2

estimator. Similar design methodology has been adopted in [Xie 2003] where the designed filter was claimed to be less conservative due to presence of free parameters for tuning. The new result merges with older one when the value of free parameter is set to zero. The similar concept has been discussed in [Xiea 2004] more elaborately. In [Hu 2003], H2 filter has been designed using two new H2 measures for uncertain linear system. The optimization of the cost function has been made using constrains in LMI form. An example to validate the method has also been presented. Observer based control-using LMI for uncertain linear system has been considered in [Lien 2004]. Few examples have been provided. The verification of results provided in the paper may be good exercise for the beginner. In [Feng 2005], H2 and H ∞ filters have been designed for piecewise discrete time linear system by solving a set of LMI. Two examples have been provided to demonstrate the advantage of the proposed algorithm. Now the researchers are trying to extend the robust H ∞ filter for nonlinear problem. Such an attempt has been made in [Coutinho 2003] where linear H ∞ filter has been discussed for a particular class of nonlinear system. Here LMI condition for H ∞ performance analysis has been derived. Based on this derivation an LMI based method for filter design has been proposed. In another recent publication [Zhang 2005] few methods of robust filtering techniques have been discussed for “affine nonlinear stochastic system” with external disturbance dependent noise. Mixed H2 / H ∞ filtering analysis has also been developed. Finally H ∞ filtering design has been considered using LMI toolbox for that type of nonlinear system.

2.3.7 Risk Sensitive (RS) Filtering The risk sensitive estimation originates from the risk sensitive control and supposed to be more robust compare to non-robust estimators. The risk sensitive cost function includes an accumulated exponential error cost. Probably the first publication on risk sensitive estimator reported in literature is from Speyer et al [Speyer 1992]. In this publication the risk sensitive cost that is considered as the exponential of a quadratic sum of the estimation error has been minimised to obtain a new type robust estimator named as risk sensitive estimator. It has been pointed out that the estimator is linear but it is not a conditional mean estimator such as Kalman filter. In that publication the correlation between the process and measurement noise has also been incorporated in deriving the filter equations.

44

Chapter 2

In another publication [Banavar 1994] the exponential cost criterion problem has been solved through quadratic kernel optimisation method. The scalar term appeared in the cost function has been named as “risk sensitive factor”. The physical meanings of the risk sensitive factor were also discussed there. In [Collings 1994], the risk sensitive filtering using information state approach has been developed. Results are presented to show the relationship between risk sensitive control and risk sensitive filtering. Simulation results have been provided to show the advantages obtained using risk sensitive control. In a similar paper [Collings 1996] the authors discuss the above concepts more elaborately using block diagrams. More examples and more elaborated simulation results have been provided to demonstrate the advantage of the proposed method. In [Dey 1995], the problem of risk sensitive filtering and smoothing has been solved for discrete time Hidden Markov Model (HMM) using reference probability model. The authors mentioned the term “risk sensitive filtering” and “Risk neutral filtering” and discuss the connection between them. The theory of change of measure has been discussed and applied to risk sensitive estimation problem. The same problem has also been addressed in [Dey 1997]. A brief discussion on the connection between the risk sensitive estimation problem and L2, H ∞ estimation problem has been discussed. An attempt has been made for stability and convergence analysis. In [Elliott 1996] the risk sensitive maximum likelihood estimation for HMM has been discussed. An algorithm has been formulated which shows the risk sensitive estimation is more robust compare to risk neutral estimation. It has been said that more researches are necessary to understand the behaviour of the estimator as well as risk sensitive parameter. In a similar publication [Moore 1997] risk sensitive filtering, smoothing and prediction problem have been studied for both continuous and discrete time for general finite dimensional signal models and HMM models. It has been shown that the risk sensitive filtering is generalisation of standard risk neutral filtering. R. N. Banavar in [Banavar 1998] well summarises the risk sensitive Kalman filter (RSKF) algorithm. It also precisely established the relation between the risk sensitive filtering and H ∞ filtering. Various properties of risk sensitive estimator were also summarised and the filter was compared with well known Kalman filter. Bound of the risk sensitive parameter has also been derived. In [Ford 1999] RSKF has been used for state and parameter estimation problem. Simulation example has been provided in the report. No better results than Kalman filter has been reported but it has been argued that in some situation risk sensitive estimation technique provide better result for the system with uncertainties.

45

Chapter 2

In the paper [Boel 2002] a more generalise cost function for risk sensitive filtering (RSF) has been discussed. It has been shown that for linear Gaussian system closed form results can be achieved. H ∞ filtering problem has also been discussed in this paper. A 2D example has been provided where RSF gives better results than Kalman filter. In [Lo 2002] the existence and uniqueness issues of risk sensitive estimation have been investigated. It has been shown that the optimal risk-sensitive estimates will be non unique. A risk sensitive filtering scheme has been proposed in [Ramezani 2002] for the estimation of partially observed Markov chains and structural results and behaviours of the filter bank have been provided. The relationship between risk sensitivity and information has been studied in [Ramezani 2004] via simulation. The authors introduce product estimator as a generalization of maximum a posteriori probability (MAP) estimator for hidden Markov models. Risk sensitive filtering is formulated for indirectly observed Markov state process in [Malcolm 2005]. The Markov process observed through Poisson process and Brownian motion has been considered. Computer simulation results are given which demonstrate the benefits of risk sensitive estimator.

2.3.7.1 Nonlinear Risk sensitive Filter Linear risk sensitive filter has been modified in [Jayakumar 1998] so that it can be used for nonlinear systems. Actually simple EKF like linearisation technique has been used and the filter was called as extended risk sensitive filter (ERSF). The ERSF gives the same result as EKF when the risk sensitive factor equals to zero. The ERSF has been applied to estimate the motion parameters from images. The performance has been compared with EKF. The result shows that ERSF converges in presence of high initial uncertainty whereas EKF diverges. In [Dey 2003] A Risk sensitive filter has been derived for a class of discrete time finite dimensional nonlinear plant. It is obtained by adjusting standard exponential cost index. The smoothing has also been described for that special type of plant. The proposed algorithm has been successfully applied to 1D nonlinear problem. The present author proposed an adaptive grid based [Bhaumik 2005b] and particle filter based nonlinear risk sensitive filtering [Sadhu 2005, Bhaumik 2006a, Bhaumik 2006b, Sadhu 2008], centre difference risk sensitive filter (CDRSF) [Sadhu 2007] and Unscented risk sensitive filter (RSUKF) [Bhaumik 2008]. These methods will be discussed in the appropriate chapter of the thesis.

46

Chapter 2

2.3.8 Cramer Rao Lower Bound (CRLB) CRLB is the least mean square error that can be achieved by any optimal unbiased estimator. As for linear Gaussian system Kalman filter is the optimal estimator and the result from Kalman filter algorithm matches with CRLB. For nonlinear systems as no optimal estimator exists, it can be used to gauge the performance of nonlinear filters. Some literature called it as Cramer Rao Bound (CRB). The concept of theoretical lower bound in the context of state estimation was first introduced by Taylor [Taylor 1979]. In that paper theoretical lower bound expression was derived for continuous time nonlinear deterministic system with discrete nonlinear measurement in additive Gaussian noise case. In [Kerr 1989] the utility, motivation and limitation of CRLB have been reviewed. The literature previous to this publication has been extensively reviewed. The results and discussion presented in this paper may be helpful for practitioner. In [Doreschuk 1995] Cramer Rao Bound for nonlinear auto regressive process with nonlinear observation sequence has been derived. The noises considered here are additive Gaussian sequence. In [Tichavsky 1998] Posterior Cramer Rao bound algorithm has been derived for nonlinear discrete time system and Gaussian measurement noise sequence. The derived algorithm becomes very useful for the practitioner. The papers [Taylor 1979, Kerr 1989, Tichavsky 1998] complete the development of the theory of CRLB for both continuous and discrete time systems, as a consequence it draws the immediate attention of the designer. Lot of researches have been done to apply CRLB methodology on bearing only target tracking [Rao 1998, Kutluyil 2005], ballistic target tracking and navigation [Bergman 1999 a] problems. These have been discussed in previous section in details and are not mentioned here. The papers [Farina 2002a, Farina 2002b] formulate the algorithm of CRLB precisely in a way, which is more convenient to designers. The developed algorithm has been applied to ballistic target tracking case and results obtained from the nonlinear filter have been compared with CRLB. In the paper [Hernandez 2004] two variant of Cramer Rao bounds for nonlinear filtering have been compared. The first bound has been called the information reduction factor, and the second bound is called as enumeration method. It has been reported that the second method is more accurate but computational load is also high compare to first method. It has also been stated that the two methods converge as the time increases. In literature CRLB is known to be a weaker lower bound. Recently a tighter lower bound has been proposed for practical problems, which is known as Bhattacharya lower bound. In [Reece 2005] the Bhattacharya lower bound has been discussed. It has also been reported that the

47

Chapter 2

Bhattacharya lower bound is as conservative as CRLB. Discussion on the new type of lower bound is beyond the scope of this thesis.

2.4

Discussion and Conclusion

A fairly elaborate literature survey on the topics related to the areas of interest of the thesis has been carried out. There are two major aspects of the thesis namely (i) advanced nonlinear filtering (ii) tracking and navigation of aerospace system. This survey covered more recent literature on advanced nonlinear filter like (i)

Linear regression Kalman filter such as UKF, central difference filter divided difference filter.

(ii)

Particle filters

(iii)

Mixed Gaussian filter

(iv)

Robust filter such as risk sensitive filter and robust H2 and H ∞ filter

(v)

Cramer Rao Lower Bound

Literature survey has also been done on target tracking and navigation problem. In target tracking prior works on (i) ballistic target tracking, (ii) Bearing only target tracking have been summarized. Detailed literature survey on navigation has also been done. In navigation prior work on (i) GPS INS data fusion (ii) initial, in-flight and transfer alignment problem have been summarized. The literature survey had been a continuous process which started around 2004 and was frozen a few months before completion of this dissertation. During the tenure of this work, the author contributed to a number of publications. As is customary, such publications are not reviewed (in detail). The literature survey, in its initial phase had helped the present worker to appreciate the concerned areas and to find out the technology gaps for aerospace problem in the area of nonlinear estimation where original contributions can be made. This chapter is expected to be helpful likewise to the newcomers in this field. In the later phase of the survey, it helped to place the work carried out by the worker in the broader perspective of the knowledge corpus contributed by other workers.

48

3 Test Problems 3.1

Introduction

In the later chapters different methods for advance filtering will be proposed. The proposed methods have been applied to simple linear and nonlinear problems chosen carefully for validation, and comparison purpose. In this chapter those simple test problems are summarized. In this thesis the problems described here are considered as benchmark for demonstrating advantages and disadvantages of proposed filtering technique. In this chapter the following test problems are considered.

3.2

•

First order linear system (L1)

•

Second order linear system (L2)

•

First order nonlinear system (N1-1 & N1-2)

•

Second order bearing only tracking (BOT) problem

First Order Linear System (L1)

Truth model of the system is taken as xk +1 = ( F + ∆F ) xk + bias + Bwk

(3.1)

Where the filter model is x k +1 = Fx k + Bwk , where wk ~ N (0,Q)

(3.2)

The measurement model is y k = Hx k + v k , where vk ~ N (0,R)

(3.3.)

To test the performance of robustness of proposed filters against unmodeled bias and uncertainty this test problem is used. Generally the values of H and B are taken as unity. Truth is initialized with the random number generated from the normal distribution of zero mean and given covariance. Filter state is initialized with zero.

3.3

Second Order Linear (L2)

(i) A simple second order linear problem has been chosen as a test problem from earlier publication [Xie 1994]. The system structure is same as described in equations (3.1), (3.2) and 0 − 0.5 . The measurement matrix and 1

(3.3). The state transition matrix has been chosen as F = 1 other parameters are chosen as follows.

− 6 0 0 B = H = [−100 10] wk ~ N (0,1) v k ~ N (0,1) ∆F = , | δ |≤ 0.3 1 0 δ

(ii) A rather contrived two-dimensional model has been chosen from earlier publications [Boel 2002], for testing and validation of different improved filters discussed later. The system had nearly decoupled state variables but with nearly identical eigenvalues, which shows poor

Chapter3

controllability and observability. The system matrix, uncertainty matrix, noises and measurement matrix

have

been

chosen

as

0.99 − 0.01 F = , and 0.99 0

0 δ ∆F = , | δ |≤ 4 . 0 0

H = [1 −1] .

wk ~ N (0,1) v k ~ N (0,1) ; B is chosen as unity matrix.

For both the cases truth is initialised with a random number generated from the zero mean and 1

0

covariance and filter states are initialised with zero. 0 25

3.4

First Order Nonlinear Problem (N1-1)

We consider the following single dimensional process and measurement sequence as a test problem. The problem was appeared in [Ito 2000] then it is revisited with a different measurement equation in [Sadhu 2007]. xk +1 = xk + ∆tf ( xk ) + wk

(3.4)

yk = ∆txk (1 + 0.5 xk ) + vk

(3.5)

Where f ( x) = 5 x(1 − x 2 ) and wk ~ N (0, b 2 ∆t )

v k ~ N (0, d 2 ∆t )

The problem is very interesting in nature. It should be noted that the process equation has three equilibrium points, 0, -1 and +1. Among them equilibrium at 0 is unstable and other two equilibriums are stable. So the truth state will stabilize either around –1 point or on around +1 point. We use the time elapsed ∆t = 0.01 second, the initial condition of the truth x0 = −0.2 , initial condition of the filter xˆ 0|0 = 0.8, b=0.5 and d=0.1. We shall consider the time from 0 to 0.8 second. The value of initial error covariance is taken as 2 ( P0|0 = 2 ). Figure 3.1 and 3.2 show the evolution of states and corresponding measurement values for the case when state variable stabilizes at –1 equilibrium point. Similarly the figure 3.3 and 3.4 show the evolution of state variable, which stabilized near about +1 equilibrium point, as well as measurement. It is observed that state variable shows more tendencies to stabilize around –1 point rather than to stabilize around +1 equilibrium point.

50

Chapter3

0 0

50

100

150

Truth value of state

-0.2

200

Truth value of state

-0.4 -0.6 -0.8 -1 -1.2 -1.4 step

Fig 3.1 Truth value for state stabilizes around the equilibrium point -1 0.02

0.01

Measurement

0

-0.01

-0.02

-0.03

-0.04 -1.4

-1.2

-1

-0.8 -0.6 State variable

-0.4

-0.2

0

Fig 3.2: Measurement for state when stabilizes around –1 equilibrium point

3.5

Another First Order Nonlinear System (N1-2)

Here we consider another first order system with nonlinear process and linear measurement. xk +1 = f ( xk ) + wk where wk ~ N (0, Q k )

(3.6)

y k = Hxk + v k , where vk ~ N (0, Rk ) .

(3.7)

The process and measurement noise covariance are taken as unity, i.e., Qk = 1, Rk = 1 . The process nonlinear equation has been taken as f ( x k ) = 2 − 0.001x k sgn ( x k ) , H=1, x0 = 0

51

Chapter3

1.6 Truth value of state Truth value of state

1.2

0.8

0.4

0 0

50

100

150

200

-0.4 Step

Fig 3.3 Truth values for state stabilizes around the equilibrium point +1 0.05 0.04

0.03

Measurement

0.02

0.01 0

-0.01

-0.02

-0.03 -0.2

0

0.2

0.4 0.6 S tate variable

0.8

1

1.2

Fig 3.4: Measurement for state stabilizes around +1 equilibrium point

3.6

2nd Order Bearing Only Tracking (BOT) Problem (non-linear) Bearing only tracking (BOT) is an important class of target tracking problems, which has

many civil and military applications [Bar Shalom 1993]. The problem has been dealt by a number of workers [Sadhu 2004a, Lin 2002] using filtering techniques ranging from extended Kalman filter (EKF) the unscented Kalman Filter (UKF), to the recently introduced particle filters (PF). Availability of substantial computing power in inexpensive and physically small VLSI components has encouraged exploration of simulation based techniques such as particle filtering. Bearing-only sensors are generally passive, comparatively inexpensive, consume less power, and more robust compared to active sensors. The problem is to estimate the coordinates of

52

Chapter3

the target using the bearing measurement only from a moving platform whose coordinates at each time step are (at least approximately) known. The problem is very important for test purpose due to its poor observability and high nonlinearity and more over less dimensionality. Keeping these in mind this problem was also considered as test problem and it is described in more detail in the next section.

3.6.1

BOT Problem Formulation The earliest version of this particular BOT problem occurs in Bar-Shalom [Bar Shalom

1993] and is later re-formulated by Lin et al [Lin 2002]. A brief overview of the problem formulation is being presented in this section. Bearing only tracking requires either (i) multiple tracking stations with known coordinates (where it is a generalised triangulation problem) or (ii) a moving platform with known velocities, on which the tracker is mounted. The non-linear bearing only target-tracking problem used in [Lin 2002] is of the second type. The problem may be visualised by a situation, where a low flying aircraft is tracking an ammunition train or an enemy ship. Though the numerical data provided in [Lin 2002] is possibly more suitable for an under water scenario, we continue to use the low flying aircraft vocabulary for appreciation of the problem. The tracking is done using angle of depression measurement from the airborne platform moving at a nearly fixed altitude. The target is assumed to move in a straight line (assumed to be the X-axis) in the horizontal plane (fig-3.5), with a near constant velocity (perturbed by the process noises in position and velocity). The platform, in order to track the target, attempts to move in a course parallel to that of the target with a near constant velocity in the same vertical plane. Imperfections of platform motion are modelled by noises in X and Y directions. The average (neglecting the noisy imperfections) motions of the platform are known with negligible errors. The bearing measurement for the target, however, is noisy. The measurement is processed to estimate the velocity and position of the target in an earth fixed coordinate system. The observer model is non-linear and both process and measurement noises are Gaussian. Note that the platform motion noise would appear as a measurement noise. The BOT problem considered has two components, namely, the target kinematics and the tracking platform kinematics as shown in fig 3.5. The target is moving in X direction with the following discrete state space relations X k +1 = Fk X k + G K w

(3.8)

53

Chapter3

Fig 3.5 Tracking platform kinematics.

x1, k T 2 / 2 1 T Fk = where x1,k is the position along X-axis in meters, and G k = 0 1 T x2, k

With X k =

x 2,k is the velocity in m/sec,

wk is independent zero mean Gaussian white acceleration noise

sequence with variance q. The sampling time is denoted as T, which has a nominal value of 1 sec. The (unknown) true initial condition and the known noise variance are

80 X 0 = q = 0.01 m 2 / s 4 1

The target motion noise covariance matrix may be computed from above as T 2 / 2 2 Q k = G k G Tk q = T /2 T q T

[

]

T 4 / 4 T 3 / 2 = 3 q 2 T / 2 T

(3.9)

In figure 3.6 and 3.7 truth position and velocity of the tracking object have been plotted for a representative run.

3.6.2

Measurement Model I

In this measurement model, bearing angle is the only measurement. The tracking platform motion may be described by the following discrete time equations: x p , k = x p , k + ∆x p , k y p , k = y p , k + ∆y p , k

k=0,1,…,nstep

(3.10)

k=0,1,…,nstep

(3.11)

54

Chapter3

where x p ,k and y p ,k are the (known) average platform position co-ordinates and ∆x p ,k and ∆y p ,k are the mutually independent zero mean Gaussian white noise sequences with variances rx =1 m2 and ry =1 m2, respectively. The mean positions of the platform are: x p , k = 4kT and y p ,k =20. The measurement equation (in bearing coordinate) is given as z m,k = z k + v s , k

(3.12)

zk = h[ x p , k , y p, k , x1, k ] = tan −1

y p,k

(3.13)

x1, k − x p , k

z m, k is the bearing angle between the x axis and the line of sight (LOS) from the sensor to the

target and v s , k is the zero mean Gaussian white measurement noise sequence with variance ( rs = (3 0 ) 2 ), assumed to be independent of the sensor platform perturbations and sampling interval. The random component of platform motion is thus seen to induce additional measurement error, which is non-additive and already embedded. The above effect can be approximated as additive noise by expanding the non-linear measurement equations as zm , k = h[ x p , k , y p , k , x1, k ] + vs , k ≅ h[ x p , k , y p , k , x1, k ] + v k

(3.14)

Where v k is the equivalent additive measurement noise (with variance R k ) given approximately by small perturbation theory as vk ≈

[ y p ∆x p + {x1, k − x p , k }∆y p ] [ x1, k − x p , k ]2 + y p , k

2

(3.15)

+ vs , k

R k is calculated considering ∆x p , k , ∆y p , k and v k are mutually independent

[ ]

2 2 R k = E vk 2 = y p , k rx + [ x1, k − x p , k ]2 ry + rs 2 2

{[ x1, k − x p , k ] + y p , k }

In fig 3.8 the measurement has been plotted for each time instance with the true position. It can be seen that as the time precedes the tracking platform approaches the tracking object the bearing angle increases.

55

Chapter3

Truth value of position (m)

96 92 Truth 88 84 80 76 0

5

10

15

20

Time(sec)

Fig 3.6 Truth value of position for a particular run

Truth value of velocity(m/s)

1.2 Truth

0.8

0.4

0 0

5

10

15

20

Time(sec)

Fig 3.7 Truth value of velocity for a particular run

3.6.3

Measurement Model II

In this model, the LOS rate measurement is also available along with the LOS. Though in reality, the LOS and the LOS rate measurement noises may be strongly correlated, we assume a diagonal

R matrix with the element R2,2 being made a variable for a systematic study.

56

Chapter3

Fig 3.8 Measurement versus position versus time

3.6.4

Measurement Model III

In this model, it is assumed that the target is being tracked using a high PRF RF seeker. Typically that type of seeker gives range and range rate signals as the output. Also the effect of eclipsing is present. In this model range and range rate signals are not considered but the effect of eclipsing [Zrnic 2001] has been taken as shown qualitatively in fig 3.9. The continuous line in that figure shows the S/N ratio for eclipsed signal where as the dotted line shows S/N ratio for the signal without eclipsing. The consequent R k is shown in fig 3.10.

50 45 40 35 SNR

30 25 20 15 10 5 0 0

5

10

Time (sec)

15

Fig 3.9 Signal to noise ratio (with and without eclipsing)

57

20

Chapter3

1000 100

log (R)

10 1 0.1

0

5

10

15

20

0.01 0.001 0.0001

Time (sec)

Fig 3.10 Measurement error covariance with eclipsing

3.6.5

Filter Initialisation

Traditionally, tracking filters are initialised from first two measurements. The latest bearing measurement defines the initial position estimate and the difference of two bearing measurements provides the estimation of initial velocity. The initial position estimate thus obtained may be shown to have a covariance of P11, 0 = rx +

ry 2

tan z

+

yp

2

4

sin z

(3.16)

rs

The traditional way of initialising velocity estimation would create unduly large variance due to the large measurement error covariance in the present problem. Lin et al [Lin 2002] attempted to reduce such large errors by using prior knowledge about the target motion. As per [Lin 2002] the initial velocity estimation is selected as xˆ2,0 = 0 and associated variance as terms P12, 0 and P21,0 are taken as zero.

58

P22 , 0 = 1 .

The off diagonal

4 Risk Sensitive Estimator 4.1

Introduction Traditionally Kalman filtering technique relies on the assumption that the system is

accurately known. Performance of Kalman filter degrades if there is uncertainty in system model. To cope up with this disadvantage different robust techniques such as H ∞ filtering, risk sensitive filtering, have been proposed in literature. In this thesis risk sensitive filtering has been chosen as the subject of study and enhancement. Initially risk sensitive estimator originates from risk sensitive control law. The concept of risk sensitive control had been applied to estimation problem to obtain the robust risk sensitive estimator. The risk sensitive estimator (RSE) derived earlier used exponential square error type cost function. Later a more general form of cost function [Boel 2002] has been used compare to the earlier publications [Dey 1997, Collings 1994, Moore 1997, Lo 2002]. The RSE is expected to have enhanced robustness and is closely linked with H ∞ type of robust estimators [Boel 2002]. While the properties and motivations for risk sensitive control law are fairly well known, despite a number of worthy publications, the same cannot be said about risk sensitive estimators. From the dearth of publication (with the notable exception of [Jayakumar 1998]) it may arguably be said that the properties of RSE are not appreciated to the extent of extracting design pragmatics for practical problems. Objective of the present chapter is to attempt a mitigation of the above shortcoming. In this chapter robustness properties of RSE have been studied and some unknown properties have been reported. In this study it has been revealed that the risk sensitive filter performs well compare to its risk neutral counterpart in presence of unmodelled bias and inaccurate knowledge of process noise. The new properties have been illustrated with the help of simple example. More over available literature on RSE tend to use different conventions and symbols, which could be confusing to a beginner. To add to the problem, there are some printing mistakes in some important publications [Boel 2002, Speyer 1992]. In this chapter all possible forms of risk sensitive Kalman filter available in the literature have been categorized. Errors appeared in earlier publication have been corrected. An alternative linear matrix inequality (LMI) based formulation of RSE has been proposed. The concept of risk sensitive estimation (RSE) is applicable to linear as well as nonlinear problems. It is well known that the closed form solution of nonlinear recursive risk sensitive estimation formula can only be obtained for linear Gaussian cases. For nonlinear system an EKF

Chapter 4

like framework (which was called as extended risk sensitive filter) has been given in [Jaykumar 1998]. However all the well known limitations of EKF including smoothness requirement and noise Gaussianity are applicable to extended risk sensitive filter (ERSF). To mitigate those limitations several methods will be proposed in next three upcoming chapters. Toward this goal recursive solution for nonlinear risk sensitive estimation is necessary to know as a starting point. For prior estimation, nonlinear recursive solution provided in [Boel 2002] has been adopted throughout the thesis. For posterior estimation a recursive solution of risk sensitive estimation has been derived in this chapter in the form of a theorem. The proof of the theorem has been done using inductive method. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem [Boel 2002, Dey 1997, 2001].

4.2 Formulation of Risk Sensitive Estimation Problem Consider a general (non-linear) signal model consisting of equations for the state ( x k ∈ R n ) and measurement y k ∈ R p with additive, uncorrelated noise wk ∈ R n , v k ∈ R p of known statistics at the instance k ={0,1,2,3…}. x k +1 = f ( x k ) + w k (4.1) yk = g (xk ) + vk (4.2) The vectors f ( x k ) and g ( x k ) are general (without any assumption of smoothness) nonlinear function of x k and k . The generalized cost function for risk-sensitive estimator at any instance k can be defined as [Boel 2002]. k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp(µ 1 ∑ ρ 1 (φ ( x i ) − Φ 2 2 i k i =0

(4.3)

The constant parameters µ1 and µ 2 are called risk-sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted

ˆ , and ς is a parameter. Functions ρ (.) and ρ (.) are both strictly convex, continuous and by Φ i 1 2 bounded from below, attaining global minima at 0. Here, the variables to be estimated are the state variables themselves ( Φ ( x) = xˆ ), and both the convex functions ρ1(.) and ρ 2(.) are known quadratic functions of vectors i.e., ρ j (α ) = α T α for j=1,2… We denote the optimal estimate as xˆi and accordingly we change the notation for the information state.

60

Chapter 4

k −1

J RS (ς , k ) = E[exp(µ1 ∑ ( x i − xˆ i ) T ( xi − xˆ i ) + µ 2 ( x k − ς ) T ( x k − ς ))] i =0

(4.4)

The initial state x0 is independent of the processes noise mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk-sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by

µ 2 . The current optimum estimate xˆ k is obtained by finding the optimum value of ς , which minimises J RS (ς , k ) , i.e. xˆ k = arg min J RS ( ς , k )

(4.5)

ς

The constant parameters µ1 and µ 2 are called risk sensitive parameters.

4.3 Recursive Solution of Risk Sensitive Estimation 4.3.1

Posterior Solution The recursive solution of RSE may be realized generally in a two-step process. First, a

recursive relation of an information state [Boel 2002, Dey 1997] is formulated, which can be updated in each time step. In each time step, the optimal estimate is then obtained by extremizing the cost of another function involving the information state.

Theorem 4.1 The posterior solution of the RSE problem may be obtained from the following recursive relations: +∞

xˆ k +1|k +1 = arg min ∫ exp[µ 2 ( xk +1 − ς ) T ( xk +1 − ς )]α k +1 dxk +1 ς

(4.6)

−∞

Where +∞

α k +1 = p( yk +1 | xk +1 ) ∫ exp[µ1 (xk − xˆ k )T (xk − xˆk )]α k p(xk +1 | xk )dxk

(4.7)

−∞

Proof of Theorem 4.1 Starting from the chain rule, of conditional density [Ristics 2004 a], it is straightforward to show that p( x0 , x1 , x 2 ,...x n | y 0 , y1 , y 2 ,... y n ) = p ( y n | x n ) p ( x n | x n −1 ) p( x0 , x1 , x 2 ,...x n −1 | y 0 , y1 , y 2 ,... y n −1 ) We derive the relations for the posterior estimation case first. The first posterior estimate xˆ1 is obtained as follows: from (4.6)

Define α 0|0 = p ( x 0|0 ) = p ( x 0 | y 0 ),

xˆ 0 = mean( p ( x 0|0 ))

xˆ1|1 = argminJ RS (ς ,1) = argminE[exp(µ1(x0 − xˆ0 )T (x0 − xˆ0 ) + µ2 (x1 − ς )T (x1 − ς )]p( y1 | x1)

ς

ς

61

Chapter 4

+∞ +∞

= argmin ∫ ∫ exp[µ1(x0 − xˆ0 )T (x0 − xˆ0 )]exp[µ2 (x1 − ς )T (x1 − ς )]p( y1 | x1) p(x1 | x0 ) p(x0 | y0 )dx0dx1

ς

−∞ −∞

by chain rule. Now let +∞

α 1 = p ( y 1 | x 1 ) ∫ exp[ µ 1 ( x 0 − xˆ 0 ) T ( x 0 − xˆ 0 )]α 0 p ( x 1 | x 0 )dx 0 −∞

+∞ then, xˆ1|1 = arg min ∫ exp[ µ 2 ( x1 − ς ) T ( x1 − ς )]α 1 dx 1 ς −∞

similarly xˆ2 can be obtained as +∞

xˆ 2|2 = arg min ∫ exp[ µ 2 ( x 2 − ς ) T ( x 2 − ς )]α 2 dx 2 ς

−∞

where +∞

α 2 = p( y 2 | x 2 ) ∫ exp[ µ 1 ( x1 − xˆ1 ) T ( x1 − xˆ1 )]α 1 p( x 2 | x1 )dx1 −∞

Proceeding in this manner, by the method of induction one can conclude that +∞

xˆ k +1|k +1 = arg ςmin ∫ exp[ µ 2 ( x k +1 − ς ) T ( x k +1 − ς )]α k +1 dx k +1 −∞

Notes: 1) Let the probability distributions of wk , v k be given respectively by p w (.), p v (.) . The two probability density terms in equation (4.7) can then be expressed as p ( y k +1 | x k +1 ) = p v ( y k +1 − g ( x k +1 )) p ( x k +1 | x k ) = p w ( x k +1 − f ( x k ))

2) Here we provide an inductive method for deriving the expression for the information state. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem [Boel 2002, Dey 1997, 2001].

3) The estimation equation is specifically applicable for posterior estimation where the estimation is performed after receiving the measurement. Similar relations can be obtained for prior estimates, also known as delayed measurement [Banavar 1998] as is used in [Boel 2002].

4.3.2

Prior Solution

The solution to the risk-sensitive estimator (RSE) problem for a priori estimation may be obtained from the following recursive relation [Boel 2002] using an information state α k (x k ) and probability density functions p( xk +1 | xk ) and p( y k | xk ) . α k (xk ) = ∫ p( xk | xk −1 ) p ( y k −1 | xk −1 ) exp( µ1 ( xk −1 − xˆ k −1|k −2 )T ( xk −1 − xˆ k −1|k − 2 ))α k −1 (xk −1 )dxk −1

Optimal estimate can be obtained from

62

(4.8)

Chapter 4

+∞

xˆ k|k −1 = arg min ∫ exp( µ 2 ( xk − ς )T ( xk − ς ))α k ( xk )dxk ς ∈R

−∞

(4.9)

Both the posterior and prior recursive solution of risk sensitive estimation has been used in the later chapters to develop risk sensitive particle filter and adaptive grid risk sensitive filter.

4.4

Risk Sensitive Estimation for Linear Gaussian Problem

The linear Gaussian system model can be described as: x k +1 = Fx k + Bwk ,

wk ~ N (0,Q)

y k = Hx k + v k ,

vk ~ N (0,R)

For the linear Gaussian risk sensitive estimation closed recursive Kalman filter like form is available. It can easily be derived. For example substituting the Gaussian probability density function in equation (4.6) and integrating, the risk sensitive Kalman filter formula for posterior estimation can readily be achieved. We call such filter as risk sensitive Kalman filter (RSKF). The multiplicity of forms in earlier literature [Boel 2002, Dey 1997, Spyere 1992, and Collings 1994] arises because one may choose (a) the posterior or prior estimation expressions and derive them using (b) posterior or prior covariance to obtain the estimates. Thus there are, at least, four possible forms. Other subtle differences like using information filter (inverse of covariance) [Collings 1994] are also possible. It is to be noted that as the exact solution is available for linear gaussian case the term µ 2 will not appear in the solution.

4.4.1

Posterior Risk Sensitive Kalman Filter

The prior error covariance Pk +1 k is given as Pk +1 k = Fk [ Pk−k1 −1 + H kT Rk−1H k − µ1I ]−1 FkT + BQk BT

(4.10)

The posterior state estimate at ( k + 1 )th step is given as xˆk +1 k +1 = Fk xˆ k k + ( Pk−+11 k + H kT+1 Rk−+11 H k +1 ) −1 H kT+1 Rk−+11 ( y k +1 − H k +1 Fk xˆk k ) = Fk xˆ k k + K k +1 ( yk +1 − H k +1 Fk xˆ k k )

(4.11)

Where the filter gain is defined as K k +1 = [ Pk−+11 k + H kT+1 Rk−+11 H k +1 ]−1 H kT+1 Rk−+11 The filter exists if and only if [ Pk−k1−1 + H kT Rk−1 H k − µ1 I ] > 0 The above expressions are equivalent (except the choice of symbols) to those used by earlier workers [Banavar 1998]. It should be noted that here posterior estimation formula uses prior covariance. Also formula for posterior risk sensitive state estimation form is available in [Dey 2001] where posterior error covariance has been used.

63

Chapter 4

xˆk +1 k +1 = Fk xˆk k + Pk +1 k +1 H k +1 Rk−+11 ( yk +1 − H k +1 Fk xˆk k )

(4.12)

where Pk−+11 k +1 = [ Fk ( Pk−k1 − µ1 I ) −1 FkT + BQk BT ]−1 + H kT Rk−1 H k

(4.13)

The state estimate equation (4.12) can be re-written in the following form xˆk +1 k +1 = Fk xˆ k k + K k +1 ( yk +1 − H k Fk xˆ k k )

(4.14)

where K k +1 = Pk +1 k +1 H k +1Rk−+11

(4.15)

is the filter gain. Note that again the relations are valid only for sufficiently small risk sensitivity factor satisfying Pk−k1 − µ1 I > 0 . In the case where µ1 = 0 , it can be seen that the equations (4.12) and (4.13) are

exactly same with the more popular form of linear Kalman filter. Matrix inversion lemma may be used to obtain the expression in desired form.

4.4.2

Prior Risk Sensitive Kalman Filter

The prior estimate is given as xˆk +1 k = Fk xˆk k −1 + Fk K k ( yk − Hxˆk k −1 )

(4.16)

With filter gain defined as K k = [ Pk−k1−1 − µ1 I ]−1 H kT ( Rk + H k [ Pk−k1−1 − µ1 I ]−1 ) −1 Prior error covariance is given by Pk +1 k = Fk [ Pk−k1−1 + H kT Rk−1 H k − µ1 I ]−1 FkT + BQk BT

(4.17)

The above relations exist provided Pk−k1−1 − µ1I > 0 ⇒ µ1 I < Pk k −1 The above expressions are simplified forms and equivalent to those used by earlier workers [Norgaard 2000]. Matrix inversion lemmas can be applied to get the desire form. It should also be noted that for risk neutral case the risk parameter becomes zero and the RSKF reduces to well known Kalman filter algorithm. The choice of risk parameter is bounded so that it satisfies the above inequality. In [Boel 2002] another form of prior to prior state update formula for RSKF has been provided in page 454. The formula given there is for unity variance process and measurement noise sequence. The prior state estimation formula according to Boel is (using Boel’s notation) Σ k +1 = A(C T C + Σ −k 1 − 2µ1 I ) −1 AT + I

(4.18)

xˆk +1 = A(C T C + Σ k−1 − 2µ1 I ) −1 (C T yk +1 + Σ k−1 xˆ k )

(4.19)

64

Chapter 4

The A matrix is state transition matrix which is equivalent to our F matrix. C is the observation matrix which is equivalent to our H matrix. The term Σ is error covariance that is equivalent to P matrix in our notation. The recursive equation for error covariance is correct. If unity covariance process noise is not considered the unity matrix will be replaced by corresponding covariance matrix. But the prior state estimation formula stated in Boel (4.19) is not correct. The correct form in Boel’s notation will be xˆk +1|k = A(C T C + Σ k−1+1|k − 2 µ1I ) −1 (C T yk +1 + Σ k−1+1|k xˆk |k −1 − 2 µ1 xˆk |k −1 )

(4.20)

The above formula is for unity measurement error covariance. For any measurement noise covariance ( Rk ) the formula will be (in conventional notation) xˆ k +1|k = F ( H T R −1 H + Pk−+11|k − µ 1 I ) −1 ( H T y k +1 + Pk−+11|k xˆ k |k −1 − µ 1 xˆ k |k −1 )

(4.21)

4.5 LMI Based Formulation of RSKF A wide variety of problems arising in system theory can be reduced to a few convex optimization problems involving linear matrix inequality (LMI). These problems can be numerically solved very efficiently using interior point method. Also softwares are available which can readily solve the LMI problem. So there is increase tendency to express a problem in LMI form and solve it. The monograph [Boyd 1994] may be referred as a good starting point to learn LMI for beginner. In the field of robust estimation there are two approaches to solve the problem. The first approach is Riccati equation based approach [Xie 1994, Nagpal 1991] and second is LMI based approach [Li 1997, Geromel 2000, Shaked 2001, Xie 2003]. The later approach has gained popularity since the development of interior point algorithm by Karmarkar in 1984. The LMI based approach has been applied to solve the robust H2 [Geromel 2000, Shaked 1997] and H ∞ [Geromel 2000] problem. But the method is not available in literature where risk sensitive filter has been solved using LMI. Shaked [Shaked 2001] formulated LMI based Kalman filtering for unity variance Gaussian process and measurement noise sequence. In this section LMI based Kalman filter will be reformulated for arbitrary process and measurement noise covariance. An alternative Linear Matrix Inequality (LMI) based risk sensitive Kalman filter will also be formulated for both posterior and prior type of estimation. The alternative formulation will provide a platform for testing the validity of available filtering algorithm.

4.5.1

LMI Based Kalman Filter Formulation

LMI based Kalman filter is first formulated by Shaked et al [Shaked 2001]. That formulation [Shaked 2001] was for unity process and measurement noise covariance and the formula contains obvious errors. In that work [Shaked 2001] Fk K k has been called as the Kalman gain, which is

65

Chapter 4

not conventional. Here LMI based Kalman filter has been reformulated with arbitrary process and measurement noise covariance. In two step Kalman filter algorithm the recursive equation of the error covariance is given by Pk +1|k = ( Fk − Fk K k H k ) Pk|k −1 ( Fk − Fk K k H k )T + Fk K k Rk ( Fk K k )T + BQk BT

(4.22)

and the posteriori estimate is given by xˆk|k = Fk xˆ k −1|k −1 + K k ( yk − H k Fk xˆk −1|k −1 )

(4.23)

The Kalman filter minimizes error covariance. That is equivalent to minimization of trace of the error covariance matrix at each instance. So this can alternatively be stated using semi definite programming (SDP) as follows. Pk +1|k and K k are the solution of the following SDP

min

trace( Pk +1|k )

Pk +1|k , K k

Subjected to − Pk +1|k ( F − F K H ) T k k k k BT ( Fk K k )T

Fk − Fk K k H k −1 k |k −1

B

−P

0

0

− Qk

0

0

−1

Fk K k 0 ≤0 0 −1 − Rk

(4.24)

Pk +1|k > 0

(4.25)

Pk +1|k − PkT+1|k = 0

(4.26)

Solving the above three inequalities with the cost criteria, which minimizes the trace of error covariance matrix, Kalman gain (Kk) and error covariance matrix can be obtained. The inequality describe in (4.24) and can easily be reduced in the form of (4.22) by using Schur complements. Softwares are readily available to solve the inequality. In Matlab LMI toolbox is available. In Scilab also similar toolbox is available. Using Kk obtained from the solutions of above LMI, posterior estimation can be done using equation (4.23). Prior estimate can also easily be obtained from the relation xˆk +1|k = Fk xˆ k |k . 4.5.2

LMI Based Risk Sensitive Kalman Filter Formulation

In the previous section it has been discussed that risk sensitive estimator minimizes exponential squared error type cost function. So risk sensitive cost function mentioned in the equation (4.4) can be stated as min exp[ µ1trace( Pk +1|k )] . For any other convex form of Kernel similar cost function can also be realized. Using Schur’s inequality it can readily be shown that the solution of equation (4.10) is equivalent to the solution of the following SDP for Pk +1|k .

66

Chapter 4

exp[ µ1trace( Pk +1|k )]

min Pk +1|k

Subjected to − Pk +1|k T Fk BT

B + H k Rk H k − µ1I ] 0 ≤0 0 − Qk−1 Fk

−1 k |k −1

− [P

T

−1

(4.27)

Pk +1|k > 0

(4.28)

Pk +1|k − PkT+1|k = 0

(4.29)

From the above LMI formulation, error covariance Pk +1|k can be obtained for each step. From Pk +1|k both posterior and prior risk sensitive Kalman filter can be realized using equation (4.11)

and (4.16). It should be noted that for RSKF the condition for filter to exist should be checked at each step as discussed in previous section. The SDP problem stated above is solved using both Scilab and Matlab software and it is verified that the obtained solution i.e. Pk +1|k is numerically same as obtained from the solution risk sensitive Riccati equation. In non-linear cases the method can be extended to obtain LMI based solution of extended risk sensitive filter (ERSF). For that measurement matrix (Hk) and state transition matrix (Fk) can be obtained after linearising the state and measurement equation. The values can be used in equation (4.24) and (4.27) to obtain error covariance. Also in that case equation (4.11) and (4.16) need to be changed accordingly to obtain the estimated value of state variable.

4.6 Robustness Properties of RSKF Essential general properties of RSE have been well compiled in [Banavar 1998]. We shall concentrate only on two distinct but interconnected aspects of the properties, namely, robustness and the pragmatics for choosing risk sensitive parameters. We draw upon the already known properties, examine and reinterpret them as also attempt to extend them. At this moment it is near impossible to comment on properties for non-linear and non Gaussian signal models. However, in the tradition of the predecessors, we use the properties of Linear Gaussian problems to gain insight and conjecture that some of the properties would also be applicable to ‘mildly’ non-linear non-Gaussian cases. The extended KF like linearisation, used in [Jaykumar 1998] also indicate that some properties in linear cases may be extended to non-linear cases.

67

Chapter 4

4.6.1

Robustness Properties

The connection between H ∞ filter and the RSE is well known [Boel 2002]. In H ∞ filter the objective is to minimize the cost ( J H ∞ ) such that J H ∞ < γ 2 . In case of Kalman filter γ → ∞ . Whereas in case of RSE the risk sensitive parameter functions as µ1 is proportional to 1 / γ 2 as stated in [Boel 2002], with say, µ 2 = 1 . This is the primary reason why the RSE is considered to be robust. However, the different aspects of the robustness, namely, with respect to the presence of un-modeled bias, error/inaccuracy in state matrix and the same for process would be carefully examined. Also of importance is to find the temporal aspects that is how long does it take for the RSE to recognize the modeling error and to compensate for them. The properties are enumerated as given below: (i) As the risk sensitive cost function memorizes the past errors incurred by the estimator, intuitively it is expected that the RSE would learn from past mistakes and perform better. At the same time, learning requires time and the effect of learning can only be seen over time and not instantaneously. One may therefore expect that the performance of RSE to improve over time as compared to risk neutral filter. (ii) The above also means that RSE would be more effective against sustained perturbations rather than quickly varying disturbances in the model. Thus, in the presence of an un-modelled bias, the RSE is expected to track with lesser errors than a risk-neutral filter. That it is indeed so, is exemplified in the next section. As a corollary, one would not expect the RSE to effectively reject un-modelled, quickly varying, disturbances. (iii) It should be pointed out that the RS cost function assigns cost for each point in state space and therefore penalizes the regions in state space, which, in the past, more often resulted in large error covariance. (iv) The RS cost function computes the cost associated with points in state space based on its knowledge of the nominal model and also on the particular realization of the measurement sequence. As such, RS cost function cannot directly infer perturbations and changes in the signal model; it has to depend on measurement. For a signal model with lower measurement noise (covariance), the RSE is expected to perform better. (v) Inspection of equation (4.8) reveals that the effect of increasing µ1 tantamount to increasing the process noise covariance as well as the estimation error covariance. This enhances confidence in measurement and reduces the same in the assumed dynamics of the process leading to higher feed back gain of the estimator- effectively increasing the filtering bandwidth.

68

Chapter 4

(vi) In practical application of KF, it is customary to use a larger value of process noise covariance (Q) than the nominal to avoid the performance penalty if the true Q is larger than the nominal or to take care of modelling errors in plant dynamics. The above property shows why such artificial measures are unnecessary for RSKF as the risk sensitive parameter automatically does that. Further, increase in Q, generally makes the estimators faster, however, RS estimators, in general, takes a longer time to settle. The effect of increasing Q and increasing the risk factor therefore does not have one to one correspondence. 4.6.2

Pragmatics Related to RS Parameter

From the discussion of previous paragraphs it may be easily inferred that enhancing the value of µ1 increases robustness and bandwidth of the filter and therefore is desirable. However, there are

limits beyond which one cannot increase the risk sensitive parameter. The estimation in risk sensitive cases is possible over a horizon of length N where 0 ≤ k ≤ N if

and only if the following conditions are satisfied: ( Pk−|k1 − 2 µ1 I ) > 0 for 0 ≤ k ≤ N . This is

discussed in [Boel 2002, Banavar 1998] and is also evident from inspecting equation (4.7) and equation (4.8). In general there is no guarantee beforehand that these conditions will be satisfied for each step. Apart from the above, larger values of risk sensitive parameters often gives rise to overflow or reduced accuracy. The present worker found that the following simple heuristic is useful to assign the first guess value for the risk sensitive parameter. This can be intuitively justified from the convergence criteria as above.

µ 1 ≤ 1 /( 2 || P ||) where ||P|| is the largest eigenvalue of P, taken over all the values of k. The later can be estimated from knowledge of the physics of the problem or from a run of KF or EKF.

4.7 Numerical Experiment and Illustration for RSKF 4.7.1

Sensitivity to State Model Error The figure 4.1 and 4.2 are the root mean square error (RMSE) of the state variables

obtained from ordinary KF and risk sensitive KF (RSKF) for 100 Monte Carlo run for the problem ‘2nd order linear (L2)’ discussed in section 3.3, case (i). The values of state transition matrix, measurement matrix, noise characteristics and input are mentioned in section 3.3, case (i). The perturbation term is taken as δ = 0.07 . Risk sensitive parameter has been chosen as

µ 1 =0.004. It should be noted from the figure that accuracy in estimation obtained from RSKF is better compared to KF in presence of perturbation. It should also be noted that RSKF estimation is much better than KF in the later time steps compared to the first few time steps. This is as

69

Chapter 4

expected according to our previous discussion. It is seen from the figures that even for such a small δ the RMSE calculated for the state variable from KF algorithm diverges but the RSKF does not. The figure 4.3 illustrates the variation of the RMSE of first state variable at 100th step for both KF and RSKF as the uncertainty parameter δ ranges in value from –2 to 2 for a fixed value of

µ 1 =0.004. The graph shows that for very low value of δ , the Kalman filter achieves a lower terminal RMSE value than RSKF. For the larger value of δ the RSKF has the lower terminal RMSE value than KF. It is also seen from the graph that in this particular case the variation with

δ is not symmetric in the positive and negative regions. The nature of variation of terminal RMSE with δ for second state variable shows similar trend and not shown here. 4.7.2

Sensitivity to Unmodelled Bias If any unmodelled bias is present in the system, the integrator like effect is observed on the

truth value of the state variable. It is illustrated in figure 4.4 using simple first order linear system (L1) discussed in section 3.2. For that problem the state transition matrix is taken as F=0.99, ∆F is zero and bias is taken as 0.2. The standard deviation of the process and the measurement noise is 0.1 and 2.5 respectively. µ1 has been taken as 0.085. From the figure 4.4 it can be seen that ordinary Kalman filter fails to track the true value of the states whereas RS filter acts comparatively better.

RMSE of 1st state variable

1.2 KF RSKF

1

RMSE

0.8 0.6 0.4 0.2 0

0

20

40

60

80

Step

Fig 4.1

RMSE of first state variable for KF and RSKF

70

100

Chapter 4

RMSE of 2nd state variable

6 KF RSKF

RMSE

5.5 5

4.5 4 3.5 0

20

40

step

60

80

100

Fig 4.2 RMSE of 2nd state variable for KF and RSKF Terminal value of RMSE

2.5 RSKF KF RMSE of 1st state

2 1.5 1 0.5 0 -0.2

-0.1

0

0.1

delta

0.2

Fig 4.3 Terminal RMSE of 1st state variable for KF and RSKF for different δ

This property seems to be very important for risk sensitive filtering application point of view. To examine whether this property exists also for multidimensional case an experiment has been done with a two dimensional linear model in the form of x k +1 = Fx k + wk + bias y k = Hx k + v k 0 − 0.5 , H = [1 − 0.5] , wk ~ N (0,1) vk ~ N (0,1). The bias vector is taken as 1 1

Where F =

bias = [0.2 0.2]T . Truth states are initialised with a random number generated from the zero mean

and unity covariance and filter is initialised with zero values for the both states. In figure 4.5 truth

71

Chapter 4

value and estimated value of first state variable obtained from RSKF and KF have been plotted. The figure shows that the estimated values obtained from the RSKF are comparatively close to truth than the estimated values obtained from Kalman filter. The same nature of the estimators is reflected in RMSE plot shown in figure 4.6 for 100 MC run. From that figure it can be seen that the RMSE plot for RSKF is lower than KF. It has been discussed in section 4.6.1 that the RSKF learns from past history and the effect of learning is evident after some time has elapsed. Figure 4.6 supports the above argument as RMSE of RSKF becomes significantly lower than ordinary KF after some times not from the beginning of estimation. Estimated and truth value of state

Truth and estimated value

8

TRUE KF RSKF

6

4

2

0 0

10

20

30

40

50

-2 step

Fig 4.4: The truth and estimated value of the state with unknown bias Truth and es tim ated value of 1s t s tate 30 Truth KF RS K F

25

20

state

15

10

5

0

-5

0

10

20

30

40

50 s tep

60

70

80

90

100

Fig 4.5 Truth and estimated value of 1st state variable with unknown bias for 2D case

72

Chapter 4

RM S E for K F and RS K F 9 RM S E of RS K F RM S E of K F

8 7

RMSE

6 5 4 3 2 1

0

10

20

30

40

50 s tep

60

70

80

90

100

Fig 4.6 RMSE for 1st state with unknown bias 2D case

4.7.3

Sensitivity to Noise Modeling Error

From the closed form equation of the risk sensitive filter for linear case it is clear that incorporating the risk sensitive parameter increases the effective prior covariance of the filter. So if the system is modelled wrongly by using a smaller process noise covariance, the risk sensitive filter estimates better than the risk neutral filter. For this experiment we have taken a first order linear problem (L1) discussed in section 3.2 with F=0.99. Filter’s process noise covariance and measurement noise covariance are taken as 0.01 and unity respectively whereas the true process noise covariance is taken as 4 times that assumed for filter. The RMSE obtained from the RSKF with µ1 =0.3, and KF are shown in the figure 4.7. The simulated result supports the concept discussed above. RMSE for Q mismatch 1.2 KF

1

RSKF RMSE

0.8 0.6 0.4 0.2 0 0

20

40

step 60

80

Fig 4.7:RMSE for RSKF and KF for Q mismatch

73

100

Chapter 4

4.8

Discussion and Conclusion In this chapter robustness properties and model sensitivity of RSE have been studied.

Some unknown properties of the risk sensitive estimator such as sensitivity to unknown bias, sensitivity to noise modeling error have been illustrated with the help of simple linear example. Different forms of risk sensitive filtering appeared in the literature have been categorized using conventional notation. Errors appeared in earlier publications have been corrected here. The role of risk sensitive parameter and simple guideline to choose such parameter have been provided. An alternative linear matrix inequality (LMI) based formulation of RSE has been proposed. A recursive posterior solution of nonlinear risk sensitive filtering has been derived in the form of a theorem. The proof of the theorem has been done using inductive method. The induction gives a direct and intuitive derivation, without the use of Measure change, Radon-Nikodyme derivatives and Girsanov’s theorem. In the upcoming chapter various methods will be developed for nonlinear risk sensitive estimation problem. Among them posterior risk sensitive particle filter and adaptive grid risk sensitive filter will be developed based on the proposed theorem.

74

5 5.1

Risk Sensitive LRKF

Introduction

In the previous chapter basic background of risk sensitive estimation, different formulation and robustness properties have been discussed. In this chapter two different type of risk sensitive filtering for nonlinear system based on linear regression technique are proposed. Basically based on central difference filtering and unscented Kalman filtering technique the risk sensitive version of both (named as central difference risk sensitive filter and risk sensitive unscented Kalman filter respectively) are proposed in this chapter. The primary motivation in formulation of the sigma point based risk sensitive filtering was the numerical efficiency and better approximation of nonlinear function than extended risk sensitive filter (ERSF). It may be noted that central difference filter (CDF) and unscented Kalman filter (UKF) are both the member of the family called sigma point filtering [Merwe 2001a]. Sigma points are generated for both the filters based on cholesky decomposition of error covariance with different tuning parameter. In CDF [Ito 2000] central difference approximation of first and second order increment has been calculated from which mean and covariance of the distribution are approximated. On the other hand in UKF weights are assigned against each sigma points. From sigma points and corresponding weights the mean and covariance of the distribution are approximated. It should be pointed out here that before this work of present author and coworker [Bhaumik 2008, Sadhu 2007, Srinivasan 2007], the only technique available for nonlinear risk sensitive filtering was the extended risk sensitive filter (ERSF) [Jayakumar 1998]. The method is numerically efficient but employs the small signal linearization technique of the extended Kalman filter (EKF) [Brown 1996]. The method captures the essence of risk sensitive filtering for plants with mild nonlinearities. However all shortcomings of the EKF [Brown 1996], including smoothness requirement for the functions and noise Gaussianity restriction are inherited in the ERSF. Hence, the ERSF fails to provide a good estimate and even diverges severely in nonlinear situations. In this chapter the following two different methods are proposed for risk sensitive filtering: (i)

Risk sensitive filter based on central difference approximation called central difference risk sensitive filtering (CDRSF).

(ii)

Risk sensitive filtering based on unscented transformation [Julier 1997, 2000] called risk sensitive unscented Kalman filter (RSUKF).

Chapter 5

The CDRSF has been developed by the present author jointly with Srinivasan under the guidance of his supervisors. The work has been published in journal [sadhu 2007] and also appears in the thesis submitted by Srinivasan [Srinivasan 2007]. Here the CDRSF has been presented very briefly for the sake of completeness of the dissertation. For detail please Srinvasan’s thesis [Srinivasan 2007] is referred. The work RSUKF has not appeared before in anywhere. The formulation has been communicated to “IET Control theory and application” [Bhaumik 2008] by the present author and it has been accepted for publication. The detail formulation of RSUKF, implementation and scope for improvement have been discussed in this chapter. The formulation has been applied to test problems and simulation results are compared with ERSF. 5.2

Risk Sensitive Filter

Consider the nonlinear plant described by the state and measurement equations as follows: x k +1 = φ ( x k ) + wk

(5.1)

yk = γ ( xk ) + vk

(5.2)

where x k ∈ R n denotes the state of the system, y k ∈ R p is the measurement at the instance k where k={0,1,2,3,…,N}, φ ( x k ) and γ ( x k ) are known nonlinear functions of xk and k. The process noise wk ∈ R n and measurement noise v k ∈ R p are assumed to be uncorrelated and normally distributed with covariance Q and R respectively. The following notations have been used for the probability density functions: f ( x k +1 | x k ) ∆

p X k +1 | X k (. | x k ) and g ( y k | x k ) ∆

pYk | X k (. | x k ) .

The objective is to estimate a known function Φ(x) of the state variables. The estimate is designated as Φˆ ( x) and its optimal value in the risk sensitive sense is denoted as Φˆ * ( x) which minimizes the cost function

(

)

(

) (

(

))

k −1 ˆ ,..., Φ ˆ = E exp µ ∑ ρ Φ(x ) − Φ ˆ * + µ ρ Φ(x ) − Φ ˆ* CΦ 1 k 1 i i 2 2 k k i =1 1

where µ1 ≥ 0 and µ 2 > 0 are two risk parameters. Functions ρ1 (.) and ρ 2 (.) are both strictly convex, continuous and bounded from below, attaining global minima at 0. Conventionally they represent squared deviation for the mean squared measure. In particular, the minimum risk sensitive Estimate (MRSE) is defined by

(

)

ˆ * = arg min C Φ ˆ * ,..., Φ ˆ * ,Φ ˆ .. Φ k 1 k −1 k

(5.3)

It can be shown that the MRSE satisfies the following recursion

(

(

))

σ k +1 (x k +1 ) = ∫ f (x k +1 | x k ) g ( y k | x k )exp µ 1 ρ 1 Φ (x k ) − Φˆ *k σ k (x k ) dx k

76

(5.4)

Chapter 5 ˆ * = arg min ∫ exp(µ ρ (Φ(x ) − α )) σ (x ) dx Φ k k k k k 2 2

(5.5)

α ∈R

where σ k (x k ) represents an information state [Boel 2002] and may be normalized and α is a parameter. Here, the variables to be estimated are the state variables themselves ( Φ( x) = x ), and both the convex functions ρ1(.) and ρ 2 (.) are known quadratic functions of vectors, of the form, ρ j (α ) = α T α for j=1, 2. In particular, for prior estimation, we denote the optimal estimate as xˆ k +1|k and accordingly we change the notation for the information state.

Hence, eqns (5.4) and (5.5) can be re-written as,

(

)

σ k +1|k (x k +1 ) = ∫ f (x k +1 | x k ) g ( y k | x k ) exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 ) σ k |k −1 (x k ) dx k T

(5.6)

where

(

)

T xˆk|k −1 = arg min ∫ exp µ 2 (xk − α ) (xk − α ) σ k|k −1 (xk ) dxk .

α∈R

5.3

CDRSF for Prior Estimation

5.3.1

Relations for Prior Estimation

(5.7)

The integrands (5.6) and (5.7) are rewritten as follows: σ k +1|k (x k +1 ) = ∫ f (x k +1 | x k ) σ k |k (x k ) dx k

(5.8)

where σ k |k (x k ) is the a posteriori estimate obtainable from the previous (kth) step using the following relation:

(

(

σ k +1|k +1 ( x k +1 ) = g ( y k +1 | x k +1 ) exp µ 1 x k +1 − xˆ k +1|k

) (x T

k +1

))

− xˆ k +1| k σ k +1|k ( x k +1 ) .

(5.9)

The optimal estimation is given by the expression

(

)

T xˆ k +1|k = arg min ∫ exp µ 2 (x k +1 − α ) ( x k +1 − α ) σ k +1|k (x k +1 ) dx k +1 .

α ∈R

(5.10)

Similar to CDF [Ito 2000], Gaussian approximations have been used in the proposed CDRSF to replace σ k (.) , which makes the integrations (5.8) and (5.9) tractable. Deterministic sigmapoints have been used to evaluate relation (5.8) as a Gaussian quadrature, and another set of sigma-points have been used to evaluate the convolution (5.9). Further, a Gaussian approximation would allow representation of the concerned distributions simply by the respective mean and covariance. To facilitate such approximation, σ k (.) must be normalized to qualify as a probability density function. When Gaussian approximation is used for σ k +1|k (.) , the optimal estimate is simply the mean value of the distribution [Bhaumik 2005b].

77

Chapter 5

5.3.2

Implementation of CDRSF for Prior Estimation

(i) Filter initialization: Initialise the filter with appropriate values of xˆ 0|0 and P0|0 . These are substituted as the initial value and covariance of the information state. (ii) Predictor step: the predicted state estimate and predicted error covariance are evaluated by approximating the nonlinear system dynamics using the central difference approximation approach and it is described as follows: •

Compute the factorization Pk |k = S T S using Cholesky decomposition as in [Ito 2000].

•

Choose a suitable scalar h and compute the central difference approximations of the first and second increments, respectively as a i = H i ,i =

φ (xˆ k |k + hS T ei ) − 2φ (xˆ k |k ) + φ (xˆ k |k − hS T ei ) h

2

φ (xˆ k |k + hS T ei ) − φ (xˆ k |k − hS T ei ) 2h

, and

, where ei is the unit vector in the ith

direction and 1 ≤ i ≤ n . •

Compute the mean and covariance as 1 i =1 2 n

1 i =1 2

n

n

σ k +1|k = xˆ k |k + ∑ H i ,i and Pk +1|k = Q + ∑ a i a i T + ∑ H i ,i H i ,i T , i =1

where Q is the process noise covariance. (iii) Optimization step: With Gaussian approximation, the optimal estimate is xˆ k +1|k = σ k +1|k . The mean of the distribution σ k +1|k (x k +1 ) are denoted by σ k +1|k (iv) Corrector step: From the available predicted state estimation and predicted error covariance the updated state estimate and updated estimate error covariance are evaluated using the central difference approximation of nonlinear measurement function and it is described as follows: The last two terms in equation (5.9) may be combined into an equivalent Gaussian distribution if we take the Gaussian approximation σ~ k +1|k (x k +1 ) in place of σ k +1|k (x k +1 ) . The combined term may

(

be expressed as N(mean, cov) where mean= xˆ k +1|k and cov= Pk++1|k = Pk +1|k −1 − 2µ1 I

)

−1

. Hence, the

corrector step would proceed as follows:

(

−1

)

−1

•

+ Compute Pk +1|k = Pk +1|k

•

Compute the factorization Pk++1|k = S T S using Cholesky decomposition as in [Ito 2000].

− 2 µ1 I

.

~ ~

78

Chapter 5

•

Compute the central difference increments along partial derivative approximations as ~ ~ γ xˆ k +1|k + hS T ei − γ xˆ k +1|k − hS T ei bi = ,and 2h

(

G i ,i =

•

) (

(

)

)

(

γ xˆ k +1|k + hS T ei − 2γ (xˆ k +1|k ) + γ xˆ k +1|k − hS T ei ~

h

~

2

) , where 1 ≤ i ≤ n .

T + Compute xˆ k +1|k +1 = xˆ k +1|k + Lk +1 ( y k +1 − z k +1 ) and Pk +1|k +1 = Pk++1|k − L+k +1 Pxz ,

~ n n 1 T where Pxz = S T (b1 ,..., bn ) , Pzz = ∑ bi bi T + ∑ G i,i Gi,i T , i =1

i =1

2

L+k +1 = Pxz (R + Pzz ) , and z k +1 = γ (xˆ k +1|k ) + ∑ Gi ,i . 1 i =1 2 n

−1

Recursion: Estimates for the subsequent steps may be computed by repeating the predictor, optimization and corrector steps as above for the required number of time steps.

Note (i)

CDRSF and CDF have the same computational complexity as is evident from the algorithms.

(ii)

The posterior form of the CDRSF has been illustrated in [Srinivasan 2007]. It is to be noted that unlike ERSF, there is no need for calculating Jacobian in the process of implementing CDRSF.

(iii)

The variable h signifies the span of the sigma-points and may be chosen appropriately. For Gaussian distribution the recommended value of h = 3

[Ito2000]. For non Gaussian distribution no standard value can be assigned. In that situation the favorable value of h needs to be determined using trial. In this respect idea over the spread of distribution may help the designer. So for nonlinear situation the CDRSF has another more tuning parameter h apart from the risk sensitive factor. (iv)

The condition 2 µ1 Pk +1|k < I should be satisfied for Pk++1|k to be computable in the corrector step. Hence, the risk-sensitive factor µ1 should be assumed to be low enough to ensure convergence of the CDRSF. It is to be noted, however, that the ERSF too suffers from a similar drawback.

5.4

Formulation of Risk Sensitive Unscented Kalman Filter

A general, closed form evaluation of the integral in equation (5.6) is intractable and hence, the well-known unscented transformation (UT) based approach has been used to obtain the approximate solution of the said integral.

79

Chapter 5

However, the unscented transform is not directly applicable and additional steps and assumptions are needed. These are required for (i) obtaining the optimal RS estimate from eqn. (5.7), (ii) reformulating the RS recursive eqns. (5.6) and (5.7) and, (iii) modifying the covariance to take care of the risk factor. Firstly, the information state σ k (xk ) is assumed to be normalized so that it may be interpreted as a probability density distribution. This allows a probabilistic interpretation of the concerned equations and makes the treatment amenable to unscented transform. The algorithm, however, is so arranged that explicit normalization is avoided. In risk-neutral estimation, the mean squared error is to be minimized and the mean of the underlying probability distribution is the solution. The definition of the optimal RS estimate given in (5.7) does not automatically provide the mean as the optimal estimate. However, the mean is the optimal estimate under assumption of Gaussianity. We can further state that even when σ k (xk ) is non Gaussian but may be represented by a mixture of Gaussian densities of the form σ k (x k ) = ∑ wi Ν (mi , Pi ) , (where wi is a weight, mi and Pi are respectively the mean and n

i =1

covariance of the component densities and n is the number of such component densities) the expected value of the distribution σ k (xk ) is the optimal RS estimate, for at least the following cases: (i)

when the component densities have the same mean (but different covariances),

(ii)

when the distribution is composed of identical pairs of mixtures, where each of such identical pair may have components described in (i) above.

Note that the above two situations cover a wide variety of symmetric distributions, including multi-modal distributions. The unscented Kalman filter algorithm makes an implicit Gaussian random variable (GRV) assumption [Ito 2000] while maintaining the mean and covariance through nonlinear transformations. The particular method by which the risk-sensitivity is accommodated within the UKF formalism is described next.

The Prior Form of RSUKF

5.4.1

In order that the UT may be applied, the integrand (5.6) is first decomposed into the following expressions. The integrand (5.6) is rewritten as: σ

k + 1| k

(x k +1 ) =

∫ f ( x k + 1 | x k )σ

k |k

(x k )dx k

(5.11) 80

Chapter 5

(

)

Where σ k |k (x k ) = g ( y k | x k ) exp µ 1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) T

(5.12)

Let us define σ + k |k −1 ( x k ) as

(

)

σ k+|k −1 (x k ) = exp µ 1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) T

(5.13)

Then (5.12) can be re-written as σ k |k (x k ) = g ( y k | x k )σ k+|k −1 (x k )

(5.14)

The expression (5.11) provides the basis for time-update or predictor step equation. The optimal estimate is then obtained by using eqn. (5.7). The expression (5.13) defines the “Riskupdate” step. It is specific to risk-sensitive filtering and there is no analogue in the risk-neutral case. Expression (5.14) provides the modified measurement update or corrector step equation. The computing steps are broadly as follows: (i) Initialization of filter with σ 0|0 and P0|0 and calculation of the weights of the sigma points. (ii) The time update step or predictor step starts with the knowledge of σ k|k and Pk |k from the previous cycle (or from the initial conditions, for the first cycle) and computation of the corresponding sigma points. The step is identical to UKF as the governing relations are also the same, which involves transforming the sigma points and adding the effect of process noise. From the transformed sigma points the updated information state σ k +1|k (x k +1 ) specified by its mean and covariance σ k +1|k and Pk +1|k may be computed using the weighted sum technique of UT. (iii) The optimal estimate may be replaced by the mean of σ k +1|k (x k +1 ) under Gaussianity assumption, as discussed in the previous subsection.

(

)

(iv) In “Risk-update” step, the term exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 )σ k |k −1 (x k ) is computed by the T

Gaussian distribution assumption, whereby, only the covariance term is affected (the mean of both terms being the same and the resultant covariance is expressed as P + k +1| k = ( P −1k +1|k − 2 µI ) −1 . (v) As the mean and covariance undergo changes due to time update and risk update, it is desirable that the sigma points be recalculated on the basis of σ k +1|k (x k +1 ) and P + k +1|k . If the changes are not significant, this step may be skipped to save some computation. (vi) The measurement update step is identical in form to the standard UKF, yielding σ k|k and Pk |k

using the weighted sum technique of UT.

The specifics of the proposed algorithm are described in the section 5.4.3. 5.4.2

The Posterior Form of RSUKF

The posterior form of the risk sensitive estimate is given by

81

Chapter 5

(

)

T xˆ k |k = arg min ∫ exp µ 2 (x k − α ) ( x k − α ) σ k |k (x k ) dx k .

ζ ∈R n

where

(

(

σ k |k (x k ) = ∫ f ( x k | x k −1 ) g ( y k | x k ). exp µ1 x k −1 − xˆ k −1|k −1

(5.15)

) (x T

k −1

))

− xˆ k −1|k −1 σ k −1|k −1 (x k −1 ) dx k −1

(5.16)

In a manner analogous to the prior RS estimate, we introduce the distribution

(

(

σ k+−1|k −1 = exp µ 1 x k −1 − xˆ k −1|k −1

) (x T

k −1

))

− xˆ k −1|k −1 σ k −1|k −1 (x k −1 )

(5.17)

Hence, σ k | k −1 ( x k −1 ) = ∫ f ( x k | x k −1 ).σ k+−1|k −1 ( x k −1 )dx k −1

(5.18)

where, σ k |k (x k ) = g ( y k | x k ) σ k |k −1 (x k −1 ) .

(5.19)

Eqn (5.17) is defined as the risk sensitive update step, eqn. (5.18) corresponds to the predictor step, and eqn (5.19) corresponds to the corrector (measurement update) step. The filter algorithm for posterior estimation is not being included in this chapter but the steps for the posterior RSUKF proceed in a manner analogous to those for the prior form. However, in the posterior form, it is convenient to consider the risk-sensitive term in the prediction step, and consequently, appropriate changes must be incorporated in the filter algorithm in accordance with eqns. (5.17)-(5.19). 5.4.3

Risk-Sensitive Unscented Kalman Filter Algorithm for Prior Estimation

5.4.3.1 General RSUKF Algorithm for Nonlinear Systems

Step (i) Filter initialization: •

Initialize the filter with σ 0|0 = xˆ 0|0 and P0|0

•

Calculate weights

(5.20)

W0 = κ /(n + κ ) , Wi = 1 / 2(n + κ ) , Wi + n = 1 / 2(n + κ )

(5.21)

Step (ii) Predictor step: •

Assign sigma points with σ k |k and Pk |k as χ k = [σ k |k σ k |k + ( (n + κ ) Pk |k ) i σ k |k − ( (n + κ ) Pk |k ) i ]

(5.22)

where ( (n + κ ) Pk |k ) i is the ith column of the matrix square root of (n + κ ) Pxx , where the matrix square root A of P is of the form P=AAT, [Julier 2000]. •

Update sigma points using process dynamics

χ i ,k +1|k = φ ( χ i ,k ) 2n

•

Compute time updated mean and covariance σ k +1|k = ∑ Wi χ i ,k +1|k

•

Pk +1|k = ∑ Wi [ χ i, k +1|k − σ k +1|k ][χ i, k +1|k − σ k +1|k ]T + Q

i =0

2n

i =0

82

(5.23) (5.24) (5.25)

Chapter 5

Step (iii) Optimal RS estimate: •

With Gaussian approximation, the optimal estimate is xˆ k +1|k = σ k +1|k

(5.26)

Step (iv) Risk sensitive update •

The mean remains the same and the covariance is updated with

•

Pk++1|k = ( Pk−+11|k − 2µ1 I ) −1

(5.27)

Step (v) Re-compute Sigma points •

Sigma points are recomputed with mean as σ k +1|k (x k +1 ) and covariance as P + k +1|k .

Step (vi) Corrector step •

Projected measurement at each sigma point Υi ,k +1|k = γ ( χ i , k +1|k ) .

•

Mean of measurement yˆ k−+1 = ∑ Wi Υi ,k +1|k

•

Calculation of covariances

•

Pyk +1 y k +1 = ∑ Wi [Υi , k +1|k − yˆ k−+1 ][Υi , k +1|k − yˆ k−+1 ]T + R

•

Pxk +1 yk +1 = ∑ Wi [ χ i , k +1|k − xˆ k +1|k ][Υi , k +1|k − yˆ k−+1 ]T

(5.31)

•

Calculation of Kalman gain K k +1 = Pxk +1k yk +1 Py−k1+1k y k +1

(5.32)

•

Posterior state values x k +1|k +1 = xˆ k +1|k + K k +1 ( y k +1 − yˆ k−+1 )

(5.33)

•

Posterior error covariance matrix is given by Pk +1|k +1 = Pk++1|k − K k +1 Pyk +1 y k +1 K k +1T .

(5.34)

2n

(5.28) (5.29)

i=0

2n

i =0 2n

i =0

(5.30)

5.4.3.2 Special Cases

For a nonlinear signal model with either a linear plant or linear measurements, the RSUKF algorithm can be simplified, leading to significant reduction of computation time, especially for systems with high dimensions. This can be done by using the RSUKF algorithm for the step corresponding to the nonlinear equation and the Kalman filter algorithm for the step corresponding to the linear equation. However, there are specific considerations which must be taken into account in each case. Hence, the modifications to the general RSUKF algorithm (in section 5.4.3.1) in each of these two special cases are being stated below.

Nonlinear System and Linear Measurement In this special case, the algorithm may be modified as follows:

Steps (i) - (iv) would be the same as the general RSUKF algorithm stated in section 5.4.3.1. Step (v) is skipped. Step (vi) The corrector step would be the same as the corrector step of the Kalman filter.

83

Chapter 5

Linear System and Nonlinear Measurement For linear process equation and nonlinear measurement equation, the predictor step would be computed by using the Kalman filter algorithm and the remaining steps are the same as that in the general RSUKF algorithm stated in section 5.4.3.1. In particular,

Step (i) Filter initialization is done in the same way as the general RSUKF algorithm. Step (ii) The predictor step is computed in a manner similar to the predictor step of a Kalman filter.

Steps (iii) - (vi) are the same as the general RSUKF algorithm in section 5.4.3.1. 5.4.3.3 Scaled Risk Sensitive Unscented Kalman Filter (Scaled RSUKF) The unscented transformation (UT) may be done in “unscaled” way or in scaled way. In scaled UT the sigma points are drawn with scaled parameters. These sigma points capture the mean and covariance of a Gaussian random variable and when propagated through the true nonlinear system it captures the posterior mean and covariance accurately to the second (extendable to third) order Taylor series expansion for arbitrary nonlinearity. Based on “unscaled” transformation the risk sensitive unscented Kalman filter has been formulated and described in earlier section. Using scaled unscented transformation RSUKF can also be developed. Similar to scaled UKF, in scaled RSUKF some degrees of freedom are available in the choice of sigma point location and it can rectify the numerical problem associated with ordinary RSUKF. The algorithm of scaled RSUKF is as follows:

Step (i) Filter initialization: •

Initialize the filter with σ 0|0 = xˆ 0|0 and P0|0

•

Calculate weights W0( m) =

λ n+λ

W0(c ) =

λ n+λ

+ (1 − α 1 + β ) Wi ( m ) = Wi c =

(5.35)

λ

2

2( n + λ )

; i = 1,2,...,2n

(5.36)

Step (ii) – (vi) would be same as described in section 5.4.3.1 Notes (i)

In risk sensitive filtering, the risk-sensitive parameter provides a tool for design trade off between nominal and robust filtering performance.

(ii)

The algorithms presented here use a basic form of UKF proposed by Julier and Uhlmann [Julier 2000] and also scaled version of UKF. In some rare cases, RSUKF steps may yield a singular covariance matrix. The scaled version of unscented

84

Chapter 5

transform is recommended for that case. Another improved version of UKF called as square root UKF [Merwe 2001a] exists in literature. Risk sensitive formulation of that can also be done in a similar manner. In square root UKF square root of error covariance is propagated directly [Merwe 2001a] to avoid computational cost associated with Cholesky factorization. (iii)

The condition ( Pk−+11|k − 2µ1 I ) > 0 should be satisfied for positive covariance matrix. Hence, the risk sensitive factor µ1 should be assumed to be low enough to ensure convergence of the RSUKF. It is to be noted, however, that the ERSF and CDRSF too suffer from similar kind of restriction.

(iv)

Numerical values of n and κ have been chosen as discussed by Julier and Uhlmann [Julier 2000] where heuristically for Gaussian noise value for (n + κ ) was taken as 3.

(v)

The parameter (n + κ ) in RSUKF can be considered equivalent to parameter h in CDRSF. It can be noticed that in CDRSF the value of h for Gaussian distribution is

3 and for RSUKF the value of

(n + κ ) is also

3 for the same kind of

distribution. (vi)

The main motivation of introducing the scaled parameter is to move the sigma points far away or close to its mean positions. Here λ = α 1 2 (n + κ ) − n is scaling parameter where α 1 is constant to control spread of sigma point around the mean value of random variable which is to be estimated, κ is a secondary scaling parameter and β is a parameter used to incorporate the prior knowledge of the distribution of random vector x . Normally the value of α 1 is small positive value in the range of 1 ≤ α 1 ≤ 10 −4 and κ = 0 . For Gaussian distribution, β = 2 is optimal value. In the

expressions of weight computation, the superscript ‘ m ’ denotes mean and ‘ c ’ denotes covariance. (vii)

Though the risk sensitive particle filter (RSPF) and adaptive grid risk sensitive filter (AGRSF), described in nest two chapters, have better performance than the CDRSF and RSUKF, the computational load is very high for these two filters and these filters may not be suitable in many real time applications, where RSUKF will be a better candidate, especially for higher order systems.

(viii)

For getting more accurate result with a given number of particles, RSPF requires a good proposal density. So far use of ERSF was proposed for this role [Sadhu 2008]. Clearly RSUKF will be much more suitable.

85

Chapter 5

(ix)

Though we used the result that optimal RS estimate is the mean with a Gaussian distribution assumption, it may be shown that this is true for many other symmetrical distributions using the Gaussian mixture models.

5.5

Case Study

Srinivasan in his thesis studied central difference risk sensitive (CDRSF) for both first order and second order nonlinear system including bearing only tracking problem. So in this section the emphasis is given to study RSUKF for different test problems.

Case I Linear Gaussian System In this case one example has been taken where RSUKF has been applied to a single dimensional linear Gaussian model. This example has been considered only to validate the formulation of RSUKF with RSKF in linear Gaussian condition. The system here considered as discussed in section 3.2 (L1). The system matrix (F) and measurement matrix (H) are taken as 0.99 and unity respectively. The uncertainty factor ∆F = 0 and bias is taken as zero. Process and measurement noises are considered as Gaussian with zero mean unity variance. The true state has been initialized with zero whereas the filter state and error covariance have been initialized with unity. The risk factor ( µ1 ) has been taken as 0.0756 for this problem. Fig 5.1 shows that both the RSKF and RSUKF state estimation have same value for this linear problem as expected from theory. This validates the formulation of RSUKF.

Case II Nonlinear Single Dimensional Case We consider the single dimensional process and measurement sequence (N1-1) described in section 3.4. The problem first appeared in [Ito 2000] then it is revisited with a different measurement equation in [Sadhu 2007]. The state of the system has been estimated using the ERSF and RSUKF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. Tracking performance for extended risk sensitive filter (ERSF) and proposed risk sensitive unscented Kalman filter (RSUKF) have been shown for a typical run in figure 5.2. For this representative run, it has been observed that the ERSF fails to track the truth whereas the performance of the RSUKF is satisfactory. The performance of the two filters is compared first in terms of percentage of fail count, which has been defined as the number of cases out of 100 (in a population of 1000 Monte Carlo run) where a filter fails to track the truth i.e. the filter settles at the wrong equilibrium point. For ERSF the percentage fail count is about 26% whereas for RSUKF it is about 1%. The numbers indicate the improvement of estimation accuracy with the proposed filter in comparison to the traditional ERSF.

86

Chapter 5

Comparison of filter performance has also been carried out with Root Mean Square Error (RMSE). The RMSE of RSUKF and ERSF for 1000 Monte Carlo runs are compared in figure 5.3. The figure shows that RMSE of ERSF is high (due to a large population of track loss cases), whereas the RMSE of RSUKF settles at 0.2 units. Also the computational costs of CDRSF and RSUKF are comparable to that of ERSF.

4

RSKF RSUKF

Estimated value

3 2 1 0 0

-1

5

10

15

20

25

-2 -3 Step

Fig 5.1 Identical nature of RSUKF and RSKF for linear Gaussian system

Truth & estimated value

1.5

Truth RSUKF ERSF

1 0.5 0 0

20

40

60

80

-0.5 -1

-1.5

step

Fig 5.2 Performance of ERSF and RSUKF for a representative run

87

Chapter 5

1.2 1 RSUKF 0.8 RMSE

ERSF

0.6 0.4 0.2 0 0

50

100 Step

150

200

Fig 5.3 RMSE plot of ERSF, CDRSF and RSUKF for 1000 runs

Case III

BOT Problem

To demonstrate the multidimensional nonlinear estimation using unscented risk sensitive filtering (RSUKF), the proposed filter has been applied to bearing only tracking (BOT) problem formulated in section 3.6 with the measurement model I described in section 3.6.2. The CDRSF has already been applied to BOT problem and results are available in [Srinivasan 2007]. The RMSE of position and velocity obtained from RSUKF and UKF has been plotted in figure 5.4 and 5.5 respectively. It has been verified that for zero risk sensitive factor the RSUKF exactly matches with ordinary UKF. From simulation it is observed that the in our BOT problem the state estimation is very sensitive to risk sensitive parameter. Only for very small value of risk sensitive parameter the RSUKF exists. It is observed that if the risk sensitive parameter is taken more than 0.00007, the RSUKF does not exist because error covariance loses its positive definite nature in intermediate step. In our simulation the value of risk sensitive parameter is taken as 0.0000605. The superiority of RSUKF in comparison to UKF cannot be established from this problem. Also comparison of RSUKF and ERSF has been made for large initial error condition. The results have been shown in figure 5.6 and 5.7. The improvement of performance by using RSUKF over ERSF is evident from the figure 5.6. The RSUKF is much more robust with respect to large initial errors. In fact, a 1000 cycle MC run shows that there are over 25 cases of track loss in ERSF. The track loss with RSUKF for the particular value of the risk factor is about a seventh of this number of cases.

88

Chapter 5

RMSE of position for UKF & RSUKF 25 RMSE_UKF

20 RMSE (position)

RMSE_RSUKF

15

10

5

0 0

5

10 15 Time (sec)

20

25

Fig 5.4 RMSE of position obtained from UKF & RSUKF

5.6

Scope for Future Work (i) In this chapter RSUKF is realized based on UKF proposed by Julier and Uhlmann in [Julier 1997, 2000]. Scaled version of RSUKF has also been formulated. It is well known that the most computationally expensive operation in UKF corresponds to calculate the sigma points in each instant of time. For that cholesky factorization need to be calculated in each step. To avoid this, square root UKF [Merwe 2001a] has been proposed. In this method the matrix obtained after cholesky factorization will be propagated recursively. Similar formulation for risk sensitive estimation can be possible. This will minimize the computation cost. (ii)

Ito [Ito 2000] proposed Gauss Hermite filter (GHF) in 2000. Like central

difference filter, GHF is also based on quadrature rules. Development of risk sensitive version of the GHF remains under the scope of future work.

89

Chapter 5

RMSE of velocity for UKF & RSUKF 1.2 RMSE_UKF RMSE_RSUKF

RMSE(velocity)

1 0.8 0.6 0.4 0.2 0 0

5

10 Time(sec) 15

20

25

Fig 5.5 RMSE of velocity obtained from UKF & RSUKF Comparison of ERSF and RSUKF

Estimated and truth value of position

250

Truth RSUKF ERSF

200 150 100 50 0

0

5

10

15

20

Time (second)

Fig 5.6 Position estimation obtained from ERSF and RSUKF for large initial position error Comparison of RSEKF and RSUKF

Estimated and truth value of velocity

2

Truth RSUKF RSEKF

1

0 0

5

10

15

20

-1

-2

-3 Step(second)

Fig 5.7 Velocity estimation obtained from ERSF and RSUKF for large initial position error

90

Chapter 5

5.7

Discussion and Conclusion

In this chapter two risk sensitive filters, named as central difference risk sensitive filter, and unscented risk sensitive filter, based on sigma point approximation have been formulated. The CDRSF has been developed jointly with Srinivasan under the guidance of supervisors [Sadhu 2007] and it has been appeared in the thesis submitted by Srinivasan [Srinivsan 2007]. So in this chapter main emphasis has been given over RSUKF. For detail formulation of CDRSF reader may read Srinivasan’s thesis [Srinivasan 2007]. ERSF inherits the shortcomings of EKF where as RSUKF retains the advantage of UKF in handling nonlinear models. This may be attributed to the fact that the UKF considers higher order terms in Taylor series where as a simple EKF makes only a first order approximation. The developed RSUKF technique has been applied to a linear Gaussian system. The result exactly matches with RSKF. This validates the algorithm. More over the RSUKF has been applied to single dimensional nonlinear test problem. The result has been compared with ERSF. The results suggest the superiority of RSUKF over ERSF in terms of estimation accuracy. The computation time necessary for RSUKF is comparable to ERSF. More over RSUKF has been applied to BOT problem. Superiority of RSUKF over ERSF is obtained in terms of immunity to failure.

91

6 Adaptive Grid Risk Sensitive Filter 6.1

Introduction For state estimation in nonlinear risk sensitive problem, prior to this work, only extended

risk sensitive filter (ERSF) is available in literature. In the earlier chapters of the thesis other closed form solutions for nonlinear risk sensitive estimation such as central difference risk sensitive filter [Sadhu 2007] (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) [Bhaumik 2008] have been proposed. The above-mentioned estimators are computationally efficient but suffer from the limitation arises due to approximation made in linearization process. Due to computational efficiency these filters may be useful for onboard application in aerospace problem in near future. But it is necessary to compare the performance of those approximate filters with a benchmarking algorithm to quantify the compromises made for not providing high computational power on board and also for tuning purpose. In non-robust estimation scenario Cramer Rao bound is being used for such benchmarking process. In robust estimation, nonexistence of such lower bound makes it difficult to compare the filtering performance with respect to achievable performance. This situation motivates to develop algorithm for risk sensitive filtering, which is not limited by nonlinearity or non gaussianity of the problem. Towards this goal two types of nonlinear risk sensitive filters based on grid (or point mass) adaptation and particle filtering method have been proposed in this thesis. Among them in this chapter the method based on grid adaptation technique has been proposed and exemplified. The method is named as adaptive grid risk sensitive filtering (AGRSF) [Bhaumik 2005, Bhaumik 2006a, Bhaumik 2007]. The main disadvantage of the method is its high computational cost especially for higher dimensional system. Computational load increases in power with dimension of the system. The drawback is well known as curse of dimensionality. But in the absence of any computing resource and time constraints, the method can estimate as accurate as desired. So the proposed method can be used off line and the estimated value obtained from this filter can be used to compare the performance of linear regression based risk sensitive estimators. The ordinary grid based or point mass technique was first reported in early seventies (Bucy 1971, Sorenson 1974). Fundamentally the method approximates continuous state space by grid of isolated points. Values of conditional probabilities are calculated only at the grid points. The method suffers from many disadvantages like poor numerical efficiency and finite resolution. Non-availability of computer makes the method as non-implementable at that time. Later Bergman [Bergman1997, Bergman 1999 b] modified and reformulated the algorithm and

Chapter 6

implemented for terrain-aided navigation. In that case uniform grid has been used although some preliminary adaptation methods have been implemented. The concept of grid adaptation had also been tried in [Simandl2002, Sun2005]. A short tutorial on point mass filtering has been made by Chen [Chen2003] and Arulampalam et al [Arulampalam 2002]. Simandl in his report [Simandl 2006] discussed about a technique similar to grid adaptation in the name of floating grid and anticipative grid. He also discussed grid design technique for single dimensional as well as multidimensional system. In [Srinivasan 2007] methodologies for efficient determination of the point mass in state space and various effective ideas towards the grid adaptation (such as use of posterior proposal, axes transformation, efficient grid interval) have been reported. It has been realised that among the various grid adaptation methods available in the literature, techniques proposed in [Srinivasan 2007] is theoretically much superior, and easier to implement. Hence in the process of formulating AGRSF all the grid adaptation techniques proposed by Srinivasan [Srinivasan 2007] have been adopted for efficient utilisation of point masses. As discussed above risk sensitive estimation based on grid adaptation technique will be proposed in this chapter. The risk sensitive filtering has been developed from fundamental principles so that it becomes easy to understand debug and implement. It should also be noted that this is the first attempt to solve nonlinear risk sensitive estimation problem numerically using grid-based method. The sections in this chapter have been organised as follows. In the next section ordinary grid based solution of the risk sensitive estimator problem has been formulated. Immediately the weakness of ordinary grid based risk sensitive filter have been realised. So in the following section various grid adaptation techniques adopted in this work have been discussed in detail. After that search algorithm necessary for AGRSF has been discussed. The developed AGRSF has been applied to a single and two dimensional linear problem to validate the formulation of proposed filtering technique. Next the developed filter has been applied to single dimensional nonlinear problem and 2D bearing only tracking problem. Finally the chapter ends with conclusion where various advantages, disadvantages and scope for future work have been discussed.

93

Chapter 6

6.2 6.2.1

Grid Based Solution of Risk Sensitive Estimation Problem Risk Sensitive Estimation Problem Consider a general (nonlinear) signal model consisting of equations for the state ( xk ∈ R n )

and measurement yk ∈ R p with additive, uncorrelated noise wk ∈ R n , vk ∈ R p of known statistics at the instance k ={0,1,2,3…}. x k +1 = f ( x k ) + wk

(6.1)

y k = g ( xk ) + vk

(6.2)

The vectors f ( x k ) and g ( x k ) are general (without any assumption of smoothness) nonlinear function of x k and k . The generalized cost function for risk-sensitive estimator at any instance k can be defined as [Boel 2002]. k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp( µ1 ∑ ρ1 (φ ( xi ) − Φ i 2 2 k i =0

(6.3)

The constant parameters µ1 and µ 2 are called risk-sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted by ˆ , and ς is a parameter. Functions ρ (.) and ρ (.) are both strictly convex, continuous and Φ i 1 2

bounded from below, attaining global minima at 0. Here, the variables to be estimated are the state variables themselves ( φ ( x) = xˆ ), and both the convex functions ρ1(.) and ρ 2(.) are known quadratic functions of vectors i.e., ρ j (α ) = α T α for j=1,2,…. We denote the optimal estimate as xˆ i and accordingly we change the notation for the information state. k −1

J RS (ς , k ) = E[exp( µ1 ∑ ( x i − xˆ i ) T ( xi − xˆ i ) + µ 2 ( x k − ς ) T ( x k − ς ))] i =0

(6.4)

The initial state x 0 is independent of the noise processes mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by µ 2 .The current optimum estimate xˆ k is obtained by finding the optimum value of ς , which

minimises J RS (ς , k ) , i.e. xˆ k = arg min J RS (ς , k )

(6.5)

ς

The constant parameters µ1 and µ 2 are called risk sensitive parameters.

94

Chapter 6

6.2.2

Recursive Solution of Risk Sensitive Estimation The recursive solution of RSE may be realized generally in a two-step process. First, a

recursive relation of an information state [Boel 2002, Dey 1997] is formulated, which can be updated in each time step. In each time step, the optimal estimate is then obtained by extremizing the cost of another function involving the information state. The posterior estimate of state for nonlinear risk sensitive estimation can be realised by the following two recursive equations. +∞

xˆk +1|k +1 = arg min ∫ exp[µ2 ( xk +1 − ς )T ( xk +1 − ς )]αk +1dxk +1 ς

(6.6)

−∞

Where +∞

αk +1 = p( yk +1 | xk +1) ∫ exp[µ1(xk − xˆk )T (xk − xˆk )]αk p(xk +1 | xk )dxk

(6.7)

−∞

The proof of the above two recursive relations has been provided in chapter 4. 6.2.3

Grid Formulation

The integrations appeared in relations (6.6) and (6.7), may not, in general be evaluated in closed form for nonlinear or non Gaussian cases. A convenient numerical technique to evaluate this integral is point mass approximation, which has been used in grid based filter. 6.2.3.1

Representing Probability Densities as Point Mass on Grid Points

The problem is to approximate at time step k, a probability distribution pk(x) defined everywhere in state space denoted by x. Let it be known apriori that outside the region Γ bounded by x=X0 and x=Xf, the probability density is negligibly small. In the region of interest Γ, the state space is then subdivided into N grid points, (with not necessarily equal intervals). In particular, at time step k, let the nontrivial grid points (assumed to be located at the centroid of the hyper volume enclosed by intersecting adjacent grids) in state space be denoted as x kj for j= 1,2…N. These points are also called the support points. In the simplest case, consider a single state variable. From prior knowledge, one can set the minimum and maximum values and divide the interval into N grids. The central point between grid line zero and grid line 1, may be called as the grid point 1. Similarly grid point j will be in the centre of grid line j-1 and j. The j-th hyper volume in this case would be the line segment between the j-th and j-1th grid lines. In two dimensional case, state variables may be divided into N1 and N2 grid lines, creating N=N1 X N2 grid points. The area bounded by the adjacent grid lines will be the corresponding grid hyper volume (surface). The grid points need to be numbered systematically. The idea of grid and support points is shown in figure-6.1.

95

Chapter 6

For grid point x kj , let the probability mass be wkj and the corresponding grid hyper volume is q kj . N

The weight for each point mass is ϖ ki = q kj wkj . We also note that ∑ ϖ ki +1|k = 1 . The probability j =1

N

density may be approximated by point mass as p( x k ) ≈ ∑ϖ ki δ ( x k − x ki ) . i =1

Though the information state α k is not a probability density, it does represent a density. It may therefore be approximated likewise. Support points for α k are generally chosen to be the same as for p( x k ) . It may be seen easily that estimation is unaffected if α k is multiplied by a constant factor. It is often advantageous to normalize the distribution of α k . 6.2.3.2

The Grid Based RSE Algorithm N

The initial state variable is first discretized into grids and point mass as p 0|0 ( x 0 ) ≈ ∑ ϖ 0i δ ( x 0 − x 0i ) . i =1

N

The information state likewise may be discretized as α 0 (ς ) ≈ ∑ α δ (ς − x ) . The information state i =1

i 0

i 0

is updated from k-th to k+1-th step as: N

α ki +1 ( x ki +1 ) = p v ( y k +1 − g ( x ki +1 )) ∑ {exp[ µ1 ( x ki − xˆ k ) T ( x ki − xˆ k )]α kj ( x kj ) p w ( x ki +1 − f ( x kj ))} j =1

(6.8)

The convolution like summation expression comes from the theory of approximate grid based filter [Ristic 2004a] and it requires the bulk of computational effort. Unlike the expression given in [Boel 2002], the distribution pv (.) appears outside the summation and provides substantial computational economy. The alpha masses are then normalized so that these sum into unity. The objective function to be minimised may then be approximated for any arbitrary point ς as: N

J RS (ς , k ) ≈ ∑ exp[ µ 2 ( x kj − ς ) T ( x kj − ς )]α kj

(6.9)

j =1

The risk sensitive state estimate is obtained by determining the optimising solution for the objective function. Note that, compared to [Dey 1997], the objective function above is simpler and optimisation is easier. Note also the difference with [Boel 2002]. Out of several possible methods for determining the optimum, the following has been used in the present work and is therefore recommended. To obtain the optimum, solve for the zero value of the partial derivative of the objective function, i.e., N ∂ J RS (ς , k ) = 0 ⇒ ∑ ( x kj − ς )α kj exp[µ 2 ( x kj − ς ) T ( x kj − ς )]α kj = 0 j =1 ∂ς

Standard techniques like Newton-Raphson method may then be employed.

96

Chapter 6

Fig 6.1 Idea of Grid and supports

6.2.3.3 Weakness of Simple Grid Based Risk Sensitive Filter Ordinary grid based risk sensitive filtering contains all the weaknesses of simple grid based filtering. The weaknesses of simple non risk sensitive grid based filtering have been reported [Srinivasan 2007]. Here the author summarizes the limitations and weaknesses of simple grid based risk sensitive filtering.

(i)

The root cause of the weaknesses of conventional grid based filters is due to the use of fixed grid points or point masses in space. For example suppose the filter started with the fixed point masses, which covered the region of probability density function well. As time proceeds the probability density function may shift to some other region, which was not perceived earlier. As a result scarcity of point mass will occur to represent the probability density function. So to obtain the accuracy in filtering one should choose large span of grid points as well as small hyper volume of cell. Again to implement the grid-based filter one should evaluate the convolution like summation (equation 6.8) and search method. These require bulk of computational effort.

(ii)

Another well-known and cited weak ness is huge computational cost especially for higher dimensional system. This limitation is named as curse of dimensionality [Bergman1999]. Suppose Ni be the number of point mass chosen for i-th state variable. In such a case to represent nth order system number of grid n

points necessary are N = ∏ N i . With the increase of dimensionality the number i =1

97

Chapter 6

N tend to be very large. However it should be noted that Monte Carlo based estimation techniques such as particle filtering also having similar curse of dimensionality problem.

(iii)

It has been discussed later that to evaluate equation (6.6) search mechanism is necessary. The mechanism also contributes toward computational load of the filter. To get rid of search mechanism one can assume approximately the grid distribution as Gaussian and can satisfy with mean and covariance of the population.

Initially equal interval grids have been used to describe probability function in state space. Although the probability density function may have peaks in some region or may be multimodal in nature. For example, if the posterior probability density is of normal distribution, it has peak around mean and if we consider particle cloud for this distribution it is dense in one sigma region and density gradually decreases from there and become almost transparent towards three sigma region. So to represent that probability density more grid points are necessary where the particle cloud density is more and vice versa. So uniform interval grid points should be modified based on probability density function. From the discussion it is also obvious that the concept of fixed grid point should also be modified for efficient utilization of the points. It is therefore necessary to determine the bounds ([xmin xmax]) of the state variables at each time step before selecting the support points. The bounds may be obtained approximately from the mean and covariance by using the relations [xmin xmax]= mean ±3*Factor*sigma. Here sigma is the standard deviation and “Factor” is a tuning parameter, which stretched or squeezed the probability density function. Depending upon the guess of trail of the pdf that factor can be selected.

6.3

Grid Adaptation Techniques Based on our above discussion the grid method tends to be computationally more expensive

compared to particle filter [Sadhu 2005, Bhaumik 2006b, Sadhu 2008]. The numerical efficiency of grid adaptation technique depends on the choice of support points. If fixed support points are used (as is the case for ordinary grid based filters), either many such support points are redundant and contribute only to the numerical load and not the accuracy of estimates or else, if too coarse a grid is chosen, computation efficiency suffers. So it is clear that to increase the numerical efficiency, the number of grid points should be chosen such as to give best estimation of states. In this section techniques used for grid adaptation have been presented elaborately.

98

Chapter 6

6.3.1

Obtaining Approximate Boundaries of the Region of Interest To draw the point mass or grid point first we need to know the boundary or region of

interest. For a single dimensional case it can be obtained approximately from the mean and covariance by using the relations [xmin xmax]= mean ±3*Factor*sigma as discussed earlier. It should be noted that the region of interest at each time step, should be as small as possible, consistent with the accuracy requirement. From the proposal distribution, the boundaries of the region of interest may be obtained. This is done by utilizing the concept of sigma-contour (for 2-D problems), sigma-ellipsoid (for 3D) and sigma-hyper-volume (for n-D). We would use the name sigma-contour generically for all dimensions. The centroid of the sigma contour is always located at the mean value of the distribution. Let us assume only the zero mean situations. The more general case is given in the algorithm. The m-sigma contour (m=1,2,3, or any real number) for an n-Dimensional distribution with a covariance matrix P is defined as the locus of the points satisfying the n-Dimensional quadratic equation x T P −1 x = m 2 , in state space. Consider a 2-D example where P −1 =

β 11

β 12 , the locus β 22

β x 2 ] 11 β 21

β 12 [x x ]T = m 2 β 22 1 2

β 21

of

the

m-sigma

contour

is

given

by

the

equation

[x1

⇒ β 11 ( x1 ) 2 + ( β 21 + β12 ) xx2 + β 22 ( x 2 ) 2 = m 2

This is clearly an equation of ellipse. If the distribution is Gaussian-like with normal tails, a 3sigma contour will contain almost all the points with significant density. (For a heavier tailed distribution, a higher value of m may be necessary). The task is then converted to finding the extremities of the 3-sigma contour and putting a rectangular box of grid to fit the contour. The quantity m will henceforth be called as the span factor. Figure 6.1 shows one sigma and three sigma contours. Extremities (Xmax, Xmin, Ymax, Ymin) of the m-sigma ellipse can be used to box the region will also lead to wastage. The x1 , x 2 axis intercepts would then be ± m

1

β 11

,± m

1

β 22

, respectively.

If the axes of the ellipse do not coincide with the state variables, the knowledge about the intercepts is not so useful. It should be noted that the off diagonal elements of the covariance represent the correlation coefficients between the state variable. If the off diagonal terms of the covariance matrix or correlation between variables are zero then the ellipse becomes circle and the axis of the circle coincides with the axis of state variable. It should also be noted that as the

99

Chapter 6

value of correlation terms increases the electricity of the ellipse also increases. So for the cases where the axis of ellipse does not match with the axis of state variable, what we really need are the extremities. The extremities would be the point where the tangents to the contour will be parallel to the axes. However, it is better to postpone the computations of the extremities until the coordinates are properly aligned which will be discussed later. We note in this context that, if the axes of the ellipse do coincide with the state variables, the intercepts immediately become the semi-major/minor axes and the dimension of the enclosing box is immediately obtained. 6.3.2

Selecting Grid Intervals

Next step is to choose the number of division in each dimension. At each time step, the number of intervals in each direction should be chosen to be consistent with the span of the state variable and the required resolution. The first easy approach is to generate the grid uniformly. Consider first a single order example. Let the region of interest is known to be [xmin xmax] and the allowable maximum error is ε. The minimum number of uniform divisions (grids) would then be Nε =(xmax- xmin)/ε. The idea can be extended to multiple dimensions as well. In figure 6.1 it is easy to note that the grids have been generated uniformly. It has been argued that there would be enough support points in the core area where a solution is expected and adequate points to represent the density in the fringe/tail areas. Based on the prior knowledge the gross nature of the posterior distribution can be known. So based on the gross nature of the posterior distribution core area and fringe area can be chosen approximately. For Gaussian distribution the core area may be taken as the one-sigma boundary and the fringe area could be the three-sigma boundary around the estimated state, leaving out the core. The idea is to keep approximately 60% of the support points in the core area and the rest for the fringe. It should be pointed out here the concepts of core and fringe area is suitable for unimodal distribution. For bimodal or multimodal distribution which may arises due to nonlinearity in process dynamics such concept cannot be applied. The idea of generation of support point in fringe and core area has been illustrated in figure 6.2. Here a Gaussian distribution with zero mean and sigma variance (sigma=[1 0.35; 0.35 1]) has been considered. Here the core area has been defined as 1 sigma contour where the fringe area has been defined as 3-sigma contour. The figure shows the inadequacy of the one-sigma region as assumption for core. It is therefore more pragmatic to define the core area to be about 1.5 to 2 times equivalent sigma. For a given nonlinear model, some experimentation may be necessary for choosing core and fringe region.

100

Chapter 6

Fig 6.2 selections of grid intervals

As no rigid line of demarcation can be made between the core and fringe region another method has been tried where the inter grid distance gradually increases as the grid distance from the mean increases. In this method grid templates, which can be used for every time step to generate grid point, can be calculated using following simple Matlab code. The generated grid in this approach has been shown in figure 6.3. ======================================== %% Algorithm for Template generation %% Npart1 is the number of template to be generated for one state %%variable Npart1=15; Gaps=zeros(1,Npart1)+1;Ticks=zeros(1,Npart1+1);xsum=0; Ticks= -1:2/(Npart1):1; for i=1:Npart1; Gaps(i)=Gaps(i)+((fix(i-(Npart1+1)/2)/Npart1)^2)*8; xsum=xsum+Gaps(i); end %%%%Normalised gaps and ticks Gaps=Gaps/xsum; for j=1:Npart1;Ticks(j+1)=Ticks(j)+Gaps(j);end Ticks=Ticks-0.5; Ticks=2*Ticks; =========================================

101

Chapter 6

Fig 6.3 Alternative method for selection of grid intervals

6.3.3

Alignment of Axes

From the previous figures it can be noted that lot of grid points have been unutilised due to mismatch between axis of sigma contour of probability density function and axes of state variables. If the covariance of the probability density function contains no correlation terms (off diagonal terms) then the sigma contour becomes circle and its axes orientation is in the axis of state variable. But during simulation the correlation terms arise obvious way even if in the initial covariance the states are not correlated. So it is necessary to orient the grid points in the axis of sigma contour and this can be done quite easily. The distribution of grid points after alignment of the axis will be looking like the figure 6.4. Advantage of the axes alignment method can be realized by comparing the figure 6.3 and figure 6.4. As a continuation of the discussion from 6.3.1 it can be stated that axes of the ellipse/ellipsoid x T P −1 x = m 2 would be along the eigenvectors of the symmetric matrix P −1 . The extremities of the

ellipse would be again at ± m

1

β 11

,± m

1

β 22

, only, the diagonal elements βj jj would be the

corresponding eigenvalues. From this, the extent of the area / hypervolume of interest is obtained directly. To get the grid points, first the grid divisions are carried out along the eigen vectors (which are pure numbers). Intersections of grid lines (parallel to the eigenvectors) are the grid points and the grid points are then transformed back into the original state space coordinates.

102

Chapter 6

The idea of using eigenvectors as preferred grid orientation was also mentioned in [Sorenson1974]. The concerned matrix being positive definite and symmetric, the computational cost of eigenvalues and eigenvectors will be reduced. The computational cost may be further reduced by noting that the eigenvalues and eigenvectors of P −1 and P are related. P −1 v j = λ j v j ⇒ v j = λ j Pv j ⇒ Pv j = (

1

λj

) v j Thus, the eigenvalues of P −1 and P are inverse of

each other and the eigenvectors are the same. Thus these quantities for P −1 can be computed without inverting the P matrix. Srinivasan in his thesis [Srinivasan 2007] developed another method called triangular square root (cholesky) method for alignment of the axes. The method is based on Cholesky factorization.

The method has been described in detain in [Srinivasan 2007] and will not be discussed here. To obtaining approximate boundaries of the region of interest, selecting grid intervals and to orient the position of the grid, an approximate knowledge of the posterior density would be required. So proposal is necessary from which the grid points can be generated in an efficient way. Method by which the approximate distribution can be obtained will be discussed in the next section.

Fig 6.4 Alignment of axes to reduce wastage of grid

6.3.4

Determination of Approximate Posterior Distribution

As discussed in the previous section approximate knowledge of posterior probability density function is necessary to generate the grid points efficiently. This is the most difficult part of the AGRSF method. But it is not impossible task. Several options have been tried in this work and discussed below. Aligning with the nomenclature use in particle filtering, this approximate

103

Chapter 6

posterior probability density function can be named as proposal density (distribution) and here the approximate posterior probability density function obtained to place the grid points will be called by that name. (iv)

At first probability density of the previous step (prior distribution) has been tried to create the grid points. However, due to time update and measurement update, the probability density at the next step may become radically different. So use of prior density function as a proposal may leads to generate the grid point in some other region of state space. One choice to cope up with the problem is broadening the region of interest. But in this process with in a few steps the region of interest will grow and blow. Therefore this method was rejected.

(v)

The next option tried for proposal distribution is to use the time updated distribution and then eliminating the fringe areas and thus obtaining approximate boundaries of the region of interest. However, determining the region from the point mass distribution often proved to be computationally expensive and therefore this method was rejected.

(vi)

The next successful attempt was to use a posterior Gaussian distribution of state as a proposal for the next step. As Gaussian distribution requires only the mean and variance, this option was considered attractive. To compute the mean and the covariance approximately at each time step additional computation using closed form filters can be carried out using computationally efficient methods. The concept is illustrated in figure 6.5. Here Extended Kalman filter has been used to generate proposal for AGRSF. Once the approximate distribution has been known as Gaussian, the grid points can be created in efficient way by following the combination of above described methods.

(vii)

One may of course use more sophisticated estimation methods like sigma point Kalman filter, central difference Kalman filter or other filters. Such sophistication method has not been tried to generate proposal in the present work. It must be pointed out that the approximate knowledge of mean and covariance is used only to place the grid points. Small errors in computation of these quantities do not have any significant impact on the final result. Sophisticated, computational methods for these approximations therefore would not be justified.

(viii)

In the context of grid placement for AGRSF the proposal can also be generated from the simple risk sensitive filters such as extended risk sensitive filter (ERSF)

104

Chapter 6

or other sophisticated risk sensitive filters. But the method has not been tried in the present work and remains in the scope for future work. All the concepts discussed in this section entitled “grid adaptation technique”, have been implemented in Matlab environment and the figures presented above (figure 6.1 – 6.4) are all generated through Matlab programming.

Fig 6.5 Determination of mean and co variance from EKF

6.4

Search Algorithm

To obtain the estimated value of states the equation (6.6) need to be evaluated. In discrete domain N

the equation (6.6) can be written as xˆ k +1|k +1 ≈ arg min ∑ exp[ µ 2 ( x kj +1 − ς ) T ( x kj +1 − ς )]α kj+1 . ς

j =1

Determination of xˆ k +1|k +1 from this expression can be done using search mechanism in state space. To obtain or to search the appropriate value for states several mechanisms, tried in the thesis, have been described below.

(i)

At first simple search mechanism has been implemented for AGRSF. In this mechanism solution has been sought among the grid points. So ς has been considered as x kj+1 . So for each ς the summation has been evaluated for arbitrary fixed µ 2 and state value has been chosen corresponding to least value of summation.

(ii)

It is to be noted that the accuracy of the search mechanism depends on the resolution of ς value. Later for search, ς value has been taken as the grid points as well as the middle point of the grid volume. It should also be noted that as the resolution of ς is

105

Chapter 6

going to be increased the computational burden of the search algorithm is going to be increased. So designer should choose the trade off between the accuracy versus computational load.

(iii)

The combination of the above two methods has also been tried where at first approximate value of state has been searched by method (i). Next around this value the searched has been refined using method (ii). Using this scheme accurate value of states can be searched with computational efficiency.

It should be pointed out here AGRSF has been computationally loaded due to generation of grid points, assigning weight and update the grid points in state space. Again search mechanism increases its computational burden more. So search mechanism can be replaced by assuming normal probability density function and hence finding out the mean and variance.

6.5

Implementation of Risk Sensitive Adaptive Grid Filtering Algorithm The main components of the AGRSF algorithm have been discussed earlier in a little bit

scattered way. The whole algorithm of AGRSF is assembled here for the designer. At the end of this section some notes have been added which may be useful to understand and implement the algorithm properly. Grid Template Generation

1. Select N i , the number of grid intervals in each axis, adequate for typical resolution requirements. Total number of grid points N ≤ ( N i ) n , where n is the dimension of state space.

2. The radius of the m-sigma contour (hyper-sphere) needs to be specified. 3. Divide the diameter of length 2m into approximately N i intervals. The intervals will be symmetrical about the centre and spaced according to the non-uniform spacing strategy described earlier.

4. Systematically generate the grid points in the hyper-cube space. Let z i be a typical point, with coordinates ( z1i , z 2i , z 3i ,....z ij ,.., z ni ) . The support (Grid) points coordinate component z ij would be located at mid points of corresponding grid intervals.

5. The corresponding cell volumes q~ i are generated for each support points. 6. Normalize q~ i ’s to produce q i . 7. Store the N x n coordinate points in Z-plane as the grid template.

106

Chapter 6

8. Once the grid template is formed, at each time step, the grid points in original coordinate are obtained by

x i = Sz i . Where S is the lower triangular matrix square root for the

covariance matrix. Initialization

9. (i) Set initial condition (xˆ 0|0 , P0 ) , which is assumed or given. (ii) From the initial covariance P0 , the grid alignment transform S 0 is obtained using T

S 0 S 0 = P0 .

(iii) The initial support points x 0 i , i=1,2,3…N, are obtained from x 0 i = S 0 z i + xˆ 0|0 . (iv) The weights ϖ 0i , corresponding to x k i are computed from Pk , xˆ 0|0 . The weights can be calculated by multiplying the cell volume ( q i ) with probability density function ( wki ) for the grid point associated with that volume. The weights are then normalized. Recursion (to obtain estimates for k+1-th step) The estimate xˆ k |k , error covariance Pk , support points x k i , i=1,2,3, …N, pdf wki 10. An approximate value of the posterior mean ξ k +1 ≈ xˆ k +1|k +1 at k+1-th step is obtained, by a linearised approximation of the filter, say, EKF. Such filters use the mean xˆ k and ~

covariance Pk as starting point. The approximate posterior error covariance Pk +1 comes as a by-product. 11. The lower triangular matrix square root for approximate posterior covariance S k +1 is then ~

~

obtained from Pk +1 , using S k +1S k +1T = Pk +1 . 12. From the grid template, S k +1 and approximate posterior mean ξ k +1 , the new support points x ik +1 , i=1,2,3, …N are obtained from the relation x k +1 i = S k +1 z i + ξ k +1 . 13. In the time update step (convolution), the weights ϖ ki +1|k corresponding to x ik +1 are obtained from the old and new supports, old weight, the process noise distribution function and the relation N

ϖ ki +1|k = q i ∑ {ϖ ki ( x kj ) exp[ µ1 ( x ki − xˆ k ) T ( x ki − xˆ k ) p w ( x ki +1 − f ( x kj )} j =1

14. In the measurement update step, the weights ϖ ki +1|k corresponding to x ik +1 are obtained from, the relation ϖ ki +1|k +1 = ϖ ki +1|k × p v ( y k +1 − g ( x ki +1 ))

15. The set of weights ϖ ki +1|k +1 are then normalized.

107

Chapter 6

16. The expected value of the posterior estimate (optimum in minimum mean square sense is obtained from xˆ k +1|k +1 =

1 N i i ∑ ϖ k +1|k +1 x k +1 or it can be found out using search algorithm N i =1

described previously.

17. The updated posterior covariance matrix Pk +1 , may likewise be computed using the relation. Pk +1 =

1 N i i i T ∑ ϖ k +1|k +1 (x k +1 − xˆ k +1|k +1 )(x k +1 − xˆ k +1|k +1 ) . N i =1

18. Set k=k+1 and go back to step (10). 6.5.1

Notes on AGRSF Algorithm a. Notes on grid template i. Eigenvector method allows us setting up grid templates, which can be

used for every time step.

ii. The outer area or the boundary of the grid points has been selected by choosing the value of m. Choice of this span factor (m) may be obtained after preliminary experimentation. The default value is 4. iii. Often, for convenience the number of intervals for each axis is chosen to

be the same. The number of intervals in each axis N1, assuming (N1= N2=N3 etc) is chosen, from a rough idea of the range of the variables and the necessary resolution. The approximate number of grid points is then the product of N1, N2, N3 etc, i.e N = ( N 1 ) n . iv. It may be noted that only relative volumes of the grid cells are important. So after the proposed linear transformation the relative volumes of the grid cell will not be changed. To compute the relative volume of a particular (say i-th) cell the sides of the volume are determined from the grid intervals and multiplied to produce the absolute volume q~ i . All the q~ i ’s are then normalised to q i , either to sum to unity or with respect the central cell (at the origin) volume.

b. Prior estimate In this algorithm the estimated state value is posterior. Similar algorithm can be formulated for prior risk sensitive adaptive grid estimation using delayed measurement.

108

Chapter 6

c. Computational cost (i)

Computational cost for N 2 multiplication and function evaluation (necessary to evaluate equation 6.7) is high. The computational cost for this convolution depends on the number of grid points in state space. Efficient choice of grid points using proposed method could reduce the computation cost for this N 2 multiplication.

(ii)

Cost of measurement update, N number of multiplications and likelihood function evaluations, is not very high and also depends on support points. With the same number of support points, the cost is same as that for particle filter.

(iii)

The cost of either eigenvalue or eigenvector computation is not significant for N >> n . The computational cost is of the order of n 3 , with some savings because of symmetry.

(iv)

The computational cost due to search mechanism is high and approximately in the order of N 2 . Approximating the probability density function as Gaussian and thus calculating the mean instead of searching from the state space can save the computational cost.

(v)

Cost of covariance computation is equivalent to Nn 2 multiplication. It may be tempting to use the approximate covariance obtained from EKF and save this cost. This is not recommended, as the use of approximate covariance usually tends to more wasted points and eventual track loss in extreme case. Moreover, with the same number of support points, the cost is the same as that for a particle filter.

d. Different proposals It has been discussed earlier that proposal is necessary to draw the grid points in an efficient way. In this work the pdf obtained from EKF has been tried as a proposal density function. Other sophisticated filtering algorithm (UKF, CDKF) etc can be used to generate the proposal for AGRSF. Also closed form risk sensitive filters proposed in previous chapters, such as ERSF, RSUKF and CDRSF, can be used as a proposal. But these methods have not been tried in the present work and remains in the scope for future work.

109

Chapter 6

6.6 6.6.1

Example and Case Study Case Study I

First Order Linear System

The AGRSF has been validated against Risk Sensitive Kalman Filter, for linear Gaussian case. In first case we consider a single dimensional system as described in section 3.2. Here system parameters are taken as F = 0.99 , H = 1 . In the earlier chapter it has been realized that risk sensitive filter performs significantly well in presence of unmodelled bias. So to verify this property with AGRSF the same model has been taken into consideration. The bias is a constant, set equal to 0.2 and is assumed to be unknown to the filter. Figure 6.6 shows a typical run with a first order system described above for 20 steps. It may be seen that the AGRSF estimate with 50 grids is virtually coincident with the ideal risk sensitive estimate obtained by RSKF. The result validates our proposed AGRSF formulation. The improved estimation of risk sensitive filter compared to KF is also evident. The tendency of convergence of RSKF algorithm with the increase of the grid point is shown in figure 6.7, where the absolute errors between AGRSF and RSKF, with different number of grid points, have been plotted for the same example. In that figure the values against AGRSF indicate the number of grid points used in a particular run, for example AGRSF-5 indicates that 5 grid points has been used. It can be shown from the figure that with the increases of grid points the error of the AGRSF reduces significantly and also estimation value converges to RSKF. 5 TRUTH KALMAN RSKF AGRSF

4

Value

3

2

1

0 0

5

10

15

20

Steps

Fig 6.6 Comparison of AGRSF and KF for linear Systems

110

Chapter 6

0.7 AGRSF-5 AGRSF-10 AGRSF-15 AGRSF-20

absolute error

0.6 0.5 0.4 0.3 0.2 0.1 0 0

5

10

15

20

steps

Fig 6.7 Convergence of AGRSF with number of grid points.

6.6.2

Case Study II Second Order Linear System In this section an example has been provided where the AGRSF has been applied to two-

dimensional linear system. The result matches exactly with the RSKF for sufficient number of point mass. That validates the proposed AGRSF method. Like single dimension system, the error settling characteristics of AGRSF with increase number of grid points has been studied. The superiority of RSKF in presence of plant perturbation or noise characteristics mismatch has not been studied here because that has been studied in detail in chapter 4. Here a two dimensional linear system has been considered as described in section 3.3 case (ii). From the system and observation matrix it is clear that the system’s observability is poor. Here the process and measurement noises are taken as zero mean and unity covariance with proper dimensions. The Truth has been initialized as xtruth = [0 0]T and filter has been initialized with x filte = [1 1]T and with unity error covariance matrix. Srinivasan [Srinivasan 2007] in his thesis shows the effectiveness of adaptive grid filtering (AGF) with (i) variable distance support points, (ii) number of support points and (iii) orientation of axes along the eigenvectors. The grid adaptation techniques proposed by Srinivasan [Srinivasan 2007] in the context of simple adaptive grid filtering (AGF) have been implemented here for AGRSF. As discussed above the variable grid points have been used. To generate variable grid (support) points, the following Matlab command has been used to generate gaps between successive grid points in any axis. Gaps(i)=Gaps(i)+((fix(i-(Npart1+1)/2)/Npart1)^2)*8;

111

Chapter 6

(a)

The gaps, computed from the left hand extreme to the right hand extreme, are thereafter

normalized to have a span of +/- 1. This is then multiplied by a factor m (default value 4) so that +/- m sigma values in each direction are covered. The area of the grid squares and their locations has been shown in figure 6.8. Z-axis has been plotted in log after dividing by the minimum area. Here number of grid point has been taken as 16 X 16. The value of factor has been chosen as 5.5. Figure 6.9 shows the posterior probability density typically reconstructed by the AGRSF in intermediate steps. The axes represent the distance from the assumed mean and normalized by the square root of respective eigenvalues of error covariance matrix. The weights are also not normalized. It should be pointed out here that as the problem is linear Gaussian the posterior pdf will be Gaussian and the figure 6.9 also shows the Gaussian pdf for posterior distribution function. That fairly matches with the theoretical consideration of AGRSF and hence again validates the proposed AGRSF filtering.

0.1

10

-0.1

10

-0.3

10

-0.5

10

1 1

0.5 0.5

0 0 -0.5

-0.5 -1

-1

StateVariable2

State Variable1

Fig 6.8 Plot of initial support points (16 X 16) and their respective area

Next to validate the AGRSF results the estimated value obtained from the filters has been plotted with the RSKF for both the states in figure 6.10 and figure 6.11. The results show that for a typical value µ1 =0.0005 the RSKF and AGRSF with 25 X 25 support points exactly matches with each other. This gives the confidence that the proposed AGRSF is correct and can be successfully implemented and with this confidence we will apply the AGRSF in non-linear cases in the next sections. The error settling property of the AGRSF has been studied. As the number of grid points increase the estimation error with RSKF decrease. The result obtained from this study is similar to that of obtained in 1 D for both the states hence the plot has not been shown here.

112

Chapter 6

current weight

8 7

probability density function

6 5 4 3 2 1 0 5 4 3 2 1 0 -1 -2 -3 -4 -5

-5

-1

-2

-3

-4

0

2nd state variable

1

2

3

4

1st state variable

Fig 6.9 Posterior probability density function at intermediate step

10 Truth RSKF AGRSF

estimated value

8

6

4

2

0 1

6

11

16

steps

Fig 6.10 Truth and estimated value of first state variable 2 Truth RSKF AGRSF25 0 estimated value

0

5

10

15

20

-2

-4

-6 Steps

Fig 6.11 Truth and estimated value of first state variable

113

5

Chapter 6

6.6.3

Case Study III First Order Nonlinear System

We consider the following single dimensional process and measurement sequence. The problem has been appeared in Ito et al [Ito 2000] then it is revisited with a different measurement equation in Sadhu et al [Sadhu 2007]. The problem has been described in section 3.4. We have considered the time from 0 to 0.8. The state of the system is estimated using the ERSF and AGRSF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. For AGRSF, number of grid points taken is 100. The truth and estimated value of state obtained from the abovementioned filters is shown in figure 6.12 for a representative run. It is noted that for this particular run the ERSF fails to track the truth while AGRSF track the truth well. RMS errors of AGRSF, ERSF have been compared using 1000 Monte Carlo runs in figure 6.13. It can be noted that the RMSE of ERSF is quite high where as the RMSE of AGRSF convergences to a much lower value. Here the computational time taken by the AGRSF has been studied. The results and comparison with risk sensitive particle filter has been presented in next chapter. To understand more, the probability densities of the state (as evaluated from AGRSF) for first 20 steps have been plotted in figure 6.14. It has been seen from the plot that although initially we started with Gaussian distribution due to nonlinear nature of process model from the next step it became a bimodal distribution with the modes are at +1 and –1 respectively. Initially the higher peak of pdf is more near the equilibrium point +1 but as time passes and more measurements are processed the mode as well as mean of the pdf gradually shifted towards –1 equilibrium point which is the truth value of the state. So it can be said that the filter is converging. In figure 6.15 probability density of states has been plotted when sufficient number grid point points have not been taken. In figure 6.15 pdf has been plotted for only 15 grid points. It can be noticed from the figure 6.14 and 6.15 as the number of grid points decrease the detail description of the probability density function (here bimodal nature of the pdf) has been lost.

114

Chapter 6

1.5

truth & estimate

1 truth AGRSF ERSF

0.5 0 0

20

40

60

80

-0.5 -1 -1.5

step Fig 6.12 Truth & Estimated value for ERSF, RSPF and AGRSF

1.2 1

AGRSF ERSF

RMSE

0.8 0.6 0.4 0.2 0

0

20

40

60

step

80

Fig 6.13 RMSE of ERSF and AGRSF for 1000 MC run

0.04

Probability density

0.03

0.02

0.01

0 5

4

3

2

1

0

-1

-2

-3

-4

-5

0

State variable

2

4

6

8

10

12

14

16

Step

Fig 6.14 Plot of probability density of estimated states with time

115

18

20

Chapter 6

1

Probability density

0.8

0.6

0.4

0.2

0 5

4

3

2

1

0

-1

-2

-3

-4

-5

0

2

State Variable

4

6

8

10

12

14

16

18

20

Step

Fig 6.15 Plot of probability density of estimated states with time for grid starvation case

6.6.4

Case Study IV Second Order Nonlinear System (BOT Problem)

6.6.4.1 Problem Description To demonstrate the multidimensional nonlinear estimation using adaptive grid risk sensitive filtering (AGRSF), the proposed filter has been applied to bearing only tracking (BOT) problem formulated in section 3.6. To determine the pdf of the transition between two points in state space the covariance matrix should be invertible. As process noise covariance in BOT problem described in section 3.6.1 is noninvertible a slight modification has been made in process noise covariance matrix. Interesting to note that the particle filter does not have this type of limitation as samples are drawn from the pdf. In original problem described in section 3.6.1 has the 0.5

0.25 0.5 × 0.01 . Here we used the process noise 1

covariance matrix Q = × [0.5 1]× 0.01 = 1 0.5

covariance matrix whose diagonals remains unchanged where as off diagonal terms are taken as zero. The physical implication is that we assume there is no cross correlation term in the process noise and obviously matrix becomes invertible. So our modified process noise covariance matrix 0.25 0 × 0.01 . 1

is Q = 0 6.6.4.2

Understanding the Intermediate Steps in Simulation

The AGRSF has been simulated using the algorithm described earlier. Here the process model is linear and measurement model is nonlinear. So linearised measurement noise model (same as used in EKF) has been used. This linearization is necessary for computation of likelihood and generation of proposal. EKF has been used as a proposal to capture the approximate posterior probability distribution. Eigenvector method described earlier has been

116

Chapter 6

used to orient the axes and scaling. One beautiful thing with AGRSF is that probability density function of prior, posterior, and likelihood can be visualised at each step. Here some useful insight has been gain on the AGRSF algorithm by visualising density functions in multidimensional case through this 2D BOT problem. It is necessary to mention that in this problem the estimates are very sensitive to risk sensitive parameter. Here the value of risk sensitive is taken as 0.000001.

(b)

Initialization The filter has been initialized with a Gaussian distribution with a mean and

error covariance. For a typical run we take mean

x0 = [67.48 0]T and covariance as

333.92 0 P0 = . 1 0

(c)

Proposal density obtained from EKF After the initialization or the end of the previous

step the probability density for the posterior distribution has been approximately generated from the EKF algorithm. The generated grid points from the proposal density have been subject to the time update step.

(d)

Time update

After generating the distribution of point mass from the proposals the

grid points are subject to time update step, which has been described earlier. In the first step after the time update probability density (50 X 50 support points, span factor of 5.0 sigma) of states has been shown in figure 6.16. It should be noted here that for the figures 6.16 the axes represent the distance from the assumed mean and normalized by the square root of respective eigenvalues of the posterior error covariance matrix as obtained from the proposal. The weights are also not normalized.

Fig 6.16 BOT point mass distribution after time update

117

Chapter 6

(e)

Likelihood

Figure 6.17 shows a typical likelihood function using 50 X 50 support

points and with span factor 5.0. The pattern in likelihood function indicates that the likelihood is only function of the position (first state variable) and not velocity. Here X axis is for position and Y axis is for velocity. The plot agrees with the measurement equation. The centre line of the fold is the position corresponding to the bearing measurement. The likelihood function is necessary for measurement update of the prior distribution to obtain posterior distribution.

Fig 6.17 Likelihood distribution 50 X 50 supports

(f)

Posterior distribution

When the prior probability distribution, shown in figure

6.16, has been updated with the likelihood function, shown in figure 6.17, we obtained the posterior distribution as shown in figure 6.18. So from the figure it can be noted that as the likelihood probability density is more on the mean value of position the point masses located near that region have been weighted more where as other pointed masses contain less weight.

Fig 6.18 posterior distribution with 50 X 50 supports

(g)

Effect of span factor

To illustrate the effect of span factor posterior distribution has

been plotted with the same number of particles but with a span factor (m) of 3 instead of 5. The

118

Chapter 6

effect of span factor can be realized by comparing the figure 6.18 and 6.19. It can be noted that there are significant distributions at the edges of the figure, indicating that the span of the generated point masses are not adequate to represent the whole probability density function hence the accuracy must be degraded. Due to this reason the span factor has been chosen as 5.0 for that problem.

Fig 6.19 Posterior distribution with 50 X 50 supports and 3 span factor

(h)

Effect of inadequate number of support points

To figure out the drawback

associated with the small number of support point, the posterior density function has been plotted for 20 X 20 number of support points in figure 6.20. The figure has been compared with the posterior probability density function with 50 X 50 support points (figure 6.18). As the number of support points has been decreased we immediately loose the details about the probability density function. This will be reflected to the final value of the estimation sequence leading to lack of accuracy in estimation.

Fig 6.20 Posterior distribution with 20 X 20 support points

119

Chapter 6

During the numerical experiment it was found that to obtain results with moderate accuracy and error convergence, at least 30 X 30 supports are necessary. The reason for the large number of supports has been investigated and the main reason identified to be the large ratio of the initial error covariance matrix P0 to process noise covariance Q. In this particular case the ratio of the eigenvalues was about 30,000 in worst case. This can be explained as follows. For a given number of supports, larger P0 enforces a larger gap between supports. On the other hand, if the gaps are not small compared to the square root of Q, the prior weights will become either close to zero or erratic. 6.6.4.3

Simulation Results

The BOT problem has been solved using proposed AGRSF and ERSF. This shows the applicability of the AGRSF in multidimensional non-linear scenario. Although the application of AGRSF in higher dimensional system is limited by the curse of dimensionality problem discussed before. To implement AGRSF for BOT problem 35 X 35 support points have been chosen in state space. A span factor of 5 has been assigned. The risk sensitive factor is taken as 0.000001. The performance of AGRSF has been compared with ERSF. For a typical single run the truth and the estimated value of position and velocity obtained from AGRSF and ERSF have been plotted in figure 6.21 and 6.22. Also the absolute errors for position and velocity estimate have been plotted in figure 6.23 and 6.24 respectively. The error settling property of AGRSF with the increase of grid number has been studied in figure 6.25 and 6.26. Similar to single dimensional linear problem the absolute errors for both position and velocity have been plotted with the number of support points. The number of grid points used for simulation has been mentioned in legend. Estimted value and truth

95

Position (m)

90 85 Truth ERSF AGRSF

80 75 70 65 60 0

5

10 step

15

Fig 6.21 Truth and estimated value of position

120

20

Chapter 6

From the figure 6.25 and 6.26 it can be noticed that with the increase number of grid the absolute error value decreases consistently. This is exactly with the concept of AGRSF. As the number of grid points in two-dimensional space increase the points can better represent the probability density and hence estimation of the state becomes more accurate. So with the increase of number of grid points the error in AGRSF converges. Estimated and truth value

1.6

velocity (m/s)

1.2 0.8 0.4 Truth ERSF

0

AGRSF

0

5

10

15

20

-0.4 steps Fig 6.22 Truth and estimated value of velocity Absolute error for position

16

position error

12

ERSF AGRSF

8

4

0 0

4

8

steps

12

Fig 6.23 Absolute error of position

121

16

20

Chapter 6

Absolute Error for Velocity

1.2 ERSF AGRSF

Velocity Error

0.9

0.6

0.3

0 0

5

10 Steps

15

20

Fig 6.24 Absolute error of velocity

AGRSF5*5 AGRSF10*1 0 AGRSF15*1 5 AGRSF20*2 0

60

40

20

0 0

5

10

15

steps

20

Fig 6.25 Convergence of position error with number of grid points AGRSF5*5

4 absolute error of velocity

Absolute error of position

80

AGRSF10*10 AGRSF15*15

3

AGRSF20*20 AGRSF25*25

2

1

0 0

5

10

15

20

steps

Fig 6.26 Convergence of velocity error with number of grid points

122

Chapter 6

6.7

Discussion and Conclusion (i)

This chapter introduces a new class of risk sensitive filtering based on grid adaptation mechanism. The method is named as adaptive grid risk sensitive filter (AGRSF). The main disadvantage is the computational cost. To utilise the grid points more efficient way and hence to minimise the computational time significant effort has been spent trying different schemes. All the schemes tried have been described in full detail in this chapter.

(ii)

In this chapter the results obtained from the AGRSF have been compared with RSKF for linear Gaussian 1D and 2D system. The exact matching between these two estimates for single as well as multidimensional case validates the formulation of AGRSF.

(iii)

Approximate knowledge of the posterior probability density function is necessary to run AGRSF. If the posterior density is approximated with Gaussian then rough knowledge of mean and covariance is necessary. In this work the EKF has been used to get rough idea about posterior mean and covariance. This is analogous to the use of “proposal density” in particle filter. Instead of EKF some other filters such as UKF, CDF and risk sensitive version of them (RSUKF, CDRSF) can also be used as proposal. It should also be kept in mind that the small error in proposal has no significant impact on over all result. Although the more accurate be the proposal lesser the number of grid necessary. So the impact of other filters mentioned earlier as proposal remains under the scope of future work.

(iv)

Span factor m may be considered as another tuning parameter of the AGRSF. Choosing a larger value of span factor m can cover any deficiency arises due to approximation of covariance. Choosing a larger value of the span factor m can also cover the error in approximating the estimate (of the mean) in proposal density function. Again with the increasing in span factor the particle density in state space dilutes. Here the default value of span factor has been chosen as 4. If the mean and covariance generated from proposal is far away from the real one then increase of span factor gives better results. In such cases we need to think for better proposal.

(v)

To reduce support points, axis alignment method has been used. The computational cost of the alignment procedure is negligible but through this method grid points can be efficiently utilised hence efficiency in estimation is increased. Here the alignment method has been demonstrated with the help of figures.

123

Chapter 6

(vi)

The proposed AGRSF has been applied to BOT problem to demonstrate the applicability of AGRSF in multi dimensional case. The better result than ERSF has been reported. The probability density functions of prior, posterior, and likelihood in an intermediate step have been illustrated with figures.

(vii)

It has been noticed that the number of grid point necessary is 10 times less in compare to risk sensitive particle filtering discussed in the next chapter. This is not surprising, because the fact that weighted supports are more informative. But the updation method of the grid points takes much time resulting over all less computational efficiency compare to particle filtering.

Strength of the AGRSF (i)

The AGRSF algorithm is simple straight forward easy to understand debug and implement. Unlike the closed form risk sensitive filters, here the probability density function can be visualised at each step. Here the mean of the density function has been considered as the estimate. Whether other statistical parameter of the distribution such as mode or median can be considered as the estimated value is under debate.

(ii)

The AGRSF can be considered as the test bench to determine the accuracy of other risk sensitive filters such as CDRSF, RSUKF and ERSF.

(iv)

In this chapter it has been shown that for nonlinear problem AGRSF gives better result than ERSF.

(v)

Unlike risk sensitive particle filtering sampling or resampling algorithm is not necessary.

(vi)

In the absence of any computing resource and time constraints, the method can provide estimation as accurate as desired.

Weakness of the AGRSF (i)

The main disadvantage of AGRSF algorithm is its computational time. Despite the different schemes proposed for grid adaptation the computation time is very high compare to ERSF, CDRSF and RSUKF. Thus at present it is not possible to implement the method for on board application. Rather AGRSF can be used as the benchmarking algorithm to compare the performance of onboard filter on offline basis.

(ii)

It is found out that the computation efficiency of AGRSF is even less than RSPF. However, with the linear transformation used for grid alignment the AGRSF

124

Chapter 6

becomes little faster. (iii)

Another weakness of AGRSF is curse of dimensionality problem. The grid point necessary to represent the probability density increases in power with the dimension of the system. So from computation cost point of view it is very difficult to implement AGRSF for very high dimensional problem. Again for the system when the ratio of P/Q is large, large numbers of support points are necessary to describe probability density function.

Scope for further improvement (i)

In the present work only EKF has been used for generating proposal. The use of other risk neutral filter such as UKF, CDRSF and also use of risk sensitive filters such as ERSF, RSUKF, CDRSF need to be explored.

(ii)

In the present work the span factor is considered as the constant over the time. The adaptation of scan factor with time can also be incorporated. Diagnostics like variance of weights may be used for this purpose.

(iii)

One can also think about the variable number of grid points as the filter evolves with time. To capture large error initially more number of grid points is necessary and as the time progress the filter gradually converges then the less number of particle may be sufficient to represent the probability density.

125

7 7.1

Risk Sensitive Particle Filter

Introduction

The concept of risk sensitive criterion is applied to linear as well as nonlinear problems and solved to get recursive risk sensitive estimators [Dey 2001, Boel 2002, Jaykumar 1998, Moore 1997, Collings 1994]. Previous to this work, for nonlinear risk sensitive filtering only EKF like approach was available [Jaykumar 1998]. As the filter has been derived from risk sensitive cost criterion, from the birth it is known for robustness compared to risk neutral filters. However, the limitations of the EKF, including smoothness requirement for the functions and noise Gaussianity restriction, are well known and hence, the extended risk sensitive filter (ERSF) fails to take care of non-Gaussian or severe nonlinear problems. To mitigate these shortcomings in an earlier chapter of this thesis linear regression Kalman filter based risk sensitive estimators namely central difference risk sensitive filter (CDRSF) and risk sensitive unscented Kalman filter (RSUKF) have been proposed. The estimators are approximate one. For more accurate estimation adaptive grid based method has been formulated and described in the previous chapter. But the adaptive grid risk sensitive filtering (AGRSF) is computationally not efficient especially for high dimensional system due to curse of dimensionality problem. So we make an effort to develop computationally more efficient particle filter based risk sensitive estimation algorithm. This new kind of robust filter has been named as risk sensitive particle filter (RSPF). It is worthy to mention here that the name risk sensitive particle filter [Thrun 2002] appears earlier in the literature but in completely different context. In their work risk sensitive cost function is not minimized rather a special risk function has been proposed which measures how risky not tracking a special area in state space would be. Sampling density of ordinary particle has been modified so that more samples can be obtained in risky region. But the interpretation is completely different from risk sensitive estimation specially used in robust control context where one minimises the expected cumulative exponential estimation error. So from the risk sensitive estimation point of view no particle filter based risk sensitive estimator has been available in literature prior to this work although the name has been used in a completly different context. The method of risk sensitive particle filtering significantly extends the range of applications of risk sensitive techniques and can be used as a benchmark to quantify the performance of other approximate risk sensitive filters. The work has been presented in conferences as well as accepted for publication in journal publication [Sadhu 2005, Bhaumik 2007, Sadhu 2008] and the international community recognises that [Orgunar 2007, 2008].

Chapter 7

So in this chapter a particle filter based numerical method named as risk sensitive particle filtering (RSPF), which minimises risk sensitive cost criterion has been proposed for nonlinear non Gaussian system. Compared to AGRSF, which was described in previous chapter, in this formulation, no heuristic approach like grid adaptation is necessary and the proposed filter is computationally more efficient than AGRSF. In the process of formulation of risk sensitive particle filter both prior and posterior versions of the RSPF have been proposed. The proposed filtering technique has been applied to nonlinear Gaussian single dimensional problem as well as 2D bearing only tracking problem. Convergence of the proposed filtering technique has also been discussed through numerical examples. The comparison between AGRSF and RSPF has also been made. It has been realised that like particle filter the efficiency of posterior RSPF depends on choice of proposal. Optimal proposal for linear Gaussian measurement has been derived. For nonlinear Gaussian cases the linearised version of optimal proposal is also derived and implemented. The applicability of nonlinear risk sensitive filter such as ERSF [Jayakumar 1998], central difference risk sensitive filter (CDRSF) [Sadhu 2007] as a proposal for RSPF will also be discussed.

7.2

Formulation of Risk Sensitive Estimation Problem Though risk sensitive estimation problem formulation has already been described in earlier

chapters but it has been re-described here for the sake of completeness. Let us consider the following nonlinear additive non Gaussian evolution and observation equations xk +1 = f ( xk ) + wk

(7.1)

y k = h( xk ) + v k

(7.2)

where xk ∈ R n denotes the state and y k ∈ R p is the measurement at any instance k where k=0,1,2,3,…, nstep, wk ∈ R n , vk ∈ R p are additive, independent noises with known statistics. The

vectors f ( xk ) and h( xk ) are general (without any assumption of smoothness) nonlinear functions of x k and k . The generalized cost function for risk sensitive estimator at any instance k can be defined as [Boel 2002] k −1

ˆ ) + µ ρ (φ ( x ) − ς ))] J RS (ς , k ) = E[exp( µ1 ∑ ρ1 (φ ( xi ) − Φ 2 2 i k i =0

(7.3)

The constant parameters µ1 and µ 2 are called risk sensitive parameters and φ is a real valued measurable function on R n . The optimal estimate of the function φ at any instance i is denoted

127

Chapter 7

ˆ , and by Φ i

ς

is a parameter. Functions ρ 1 (.) and ρ 2 (.) are both strictly convex, continuous and

bounded from below, attaining global minima at 0. The initial state x0 is independent of the noise processes mentioned above and has a known probability density distribution p( x 0 ) = p 0 ( x 0 ) . The risk sensitive cost function as above includes an accumulated error cost up to time k, with a relative weight µ1 and the current cost weighted by ˆ , is obtained by finding the µ 2 . The optimal estimate of φ at the current instant, denoted by Φ k optimum value of ς which minimizes J RS (ς , k ) , i.e. Φˆ k = arg min J RS (ς , k ) ς∈R

7.3

(7.4)

Prior Risk Sensitive Particle Filter Formulation

In this section a prior formulation of risk sensitive particle filter for nonlinear problem has been summarized.

7.3.1

Solution of Risk Sensitive Estimation Problem for Prior Estimation

In this section, the solution of the risk sensitive filtering problem has been provided for prior estimation. The predicted risk sensitive estimate, which has been referred to as the one stage delay estimate by some authors [Banavar 1998, Jaykumar 1998], provides the a priori estimate of states. It is straightforward to show that both the apriori and aposteriori estimators reduce to respective risk sensitive Kalman filter (RSKF) for the linear Gaussian case. The solution to the risk sensitive estimator (RSE) problem for a priori estimation may be obtained from the following recursive relation [Boel 2002] using an information state α k (x k ) and probability density functions p( x k +1 | x k ) and p( y k | x k ) .

(

(

))

ˆ α k (x k ) = ∫ p( xk | x k −1 )p( y k −1 | xk −1 ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k − 2 α k −1 ( x k −1 )dx k −1

(7.5)

Optimal estimate can be obtained from +∞

ˆ Φ k |k −1 = arg min ∫ exp(µ 2 ρ 2 (φ ( x k ) − ς ))α k ( x k )dx k ς ∈R

−∞

128

(7.6)

Chapter 7

7.3.2

Probabilistic Interpretation

The positive measures {α k (.)} are not normalized but we can normalize them to obtain probability α k (x k ) , where α k (x k ) is given by the equation (7.5). ∫ α k (x k )dx k

distributions {α k (.)} defined by α k (x k ) =

It is also convenient to introduce the following intermediate measures β k (x k ) =

(

(

))

Where β k (x k ) = p( y k | x k ) exp µ 1 ρ 1 φ ( x k ) − Φˆ k |k −1 α k (x k )

β k (x k ) ∫ β k (x k )dx k

(7.7)

Using such notation, we can rewrite the risk sensitive filter as follows α k (x k ) = ∫ p(x k | x k −1 )β k −1 (x k −1 )dx k −1

(7.8)

+∞

and Φˆ k |k −1 = arg min ∫ exp(µ 2 ρ 2 (φ ( x k ) − ς ))α k (x k )dx k ς ∈R

Where β k (x k ) =

(7.9)

−∞

(

(

)) ))

ˆ p( y k | x k ) exp µ 1 ρ 1 φ ( x k ) − Φ k | k −1 α k ( x k ) . ˆ ∫ p( y k | x k ) exp µ1 ρ 1 φ ( x k ) − Φ k |k −1 α k (x k )dx k

(

(

(7.10)

7.3.3 Particle Implementation •

Initialization; k=0 Sample X 0(i ) ~ p 0 (.) to obtain αˆ (x 0 ) =

(

)

1 N (i ) ∑ δ x0 − X 0 , N i =1

(7.11)

where N is the no of particles and i is the particle index, Set Φˆ 0 = arg min ∑ exp(µ 2 ρ 2 (φ (X 0(i ) ) − ς )) . N

ς ∈R

i =1

(7.12)

~

Sample X 0(i ) ~ q 0 (.) , where q 0 (.) is a proposal density, and compute

(

) (

(( ) ( )

)) ( )

~ ˆ p X~ (i ) p y 0 | X 0(i ) exp µ1 ρ1 φ X 0(i ) − Φ ~ 0 0 0 W0(i ) ∝ ~ (i ) q0 X 0 N

~

Normalize weights such that ∑ W 0(i ) = 1 . i =1

{~

~

Resample particles X 0(i ) , W0(i )

•

(7.13) (7.14)

}

At time k; k>0

(

~

)

Sample X k(i ) ~ p . | X k(i−)1 . 1 N It follows that αˆ k (x k ) = ∑ δ (x k − X k(i ) ) N

i =1

129

(7.15)

Chapter 7

Set Φˆ k |k −1 = arg min ∑ exp(µ 2 ρ 2 (φ (X k(i ) ) − ς )). N

~

(7.16)

i =1

ς ∈R

(

~

)

Sample X k(i ) ~ q k . | X k(i−)1 , where q k (.) is a proposal density,

(

) (

(( )

))

~ (i ) ~ (i ) ~ (i ) ~ (i ) ˆ ~ (i ) p( X k | X k −1 ) p y k | X k exp µ1 ρ1 φ X k − Φ k |k −1 Compute weight Wk ∝ . ~ (i ) ~ (i ) q k X k | X k −1 N

(

)

~

Normalize weights such that ∑ W k(i ) = 1 .

(7.18)

i =1

{~

~

(7.17)

}

Resample particles X k(i ) , Wk(i ) .

7.3.4 Optimal Proposal One of the key factors for efficient implementation of particle filter is choice of proposal successfully. The same statement is true for risk sensitive particle filter. In the algorithm described in previous section we need to select the importance distributions q0 ( x0 ) and q k (x k | x k −1 ) . It is straightforward to check that the distribution minimizing the conditional

variance of the weights is

(

(

ˆ q 0opt (x 0 ) ∝ p(x 0 ) p( y 0 | x 0 ) exp µ1 ρ1 φ (x 0 ) − Φ 0

))

at k=0

(7.19)

which gives an importance weight proportional to ~ W0(i )opt ∝ 1 .

(7.20)

At any instance k>0, the optimal importance distribution is

(

) (

(

ˆ q kopt (x k | x k −1 ) ∝ p (x k | x k −1 ) p y k | x k exp µ1 ρ1 φ (x k ) − Φ k |k −1

))

(7.21)

It is possible to sample from this optimal importance distribution in some special cases. In other situation when it is not possible to sample from optimal distribution, some approximate distribution is used.

Case 1 Computation of Optimal Proposal for RSPF with Nonlinear System and Linear Measurements For nonlinear system and linear measurement optimal expression for proposal can be derived. To do this let us consider the following system and measurement equation. x k +1 = f ( x k ) + w k , yk = Hxk + vk ,

wk ~ N (0, Qk )

(7.22)

v k ~ N (0, R k )

(7.23)

For this type of system optimal importance distribution can be obtained from the distribution qk

opt

( x k | x k −1 ) ~ N (m k , Σ k ) .

where Σ −k 1 = H T Rk −1 H + Qk −1 − 2µ1 I and

(7.24)

130

Chapter 7

−1

m k = Σ k ( H T R k y k + Qk

−1

ˆ f ( x k −1 ) − 2µ1 Φ k |k −1 )

(7.25)

Doucet [Doucet 2000] provided the expression for optimal proposal for ordinary particle filter. But the expression has not been derived in that paper [Doucet 2000]. Here we provide a derivation for optimal proposal for risk sensitive particle filter. For particle filter similar steps may be carried out to derive optimal proposal only the reader need to put zero in place of risk sensitive parameter.

(

(

))

To obtain optimal proposal the integration ∫ p(x k | x k −1 ) p (y k | x k )exp µ1 ρ1 φ (x k ) − Φˆ k |k −1 dx k need to be evaluated. But unfortunately the integration cannot be evaluated for all class of system and noise characteristics. The integration can only be evaluated for linear measurement system and Gaussian noise sequence. For square error type of kernel and Gaussian probability density function the above expression can be written as

(

) (

)

∫ p( x k | x k −1 ) p y k | x k exp µ1 (x k − xˆ k |k −1 ) (x k − xˆ k |k −1 ) dx k T

or α ∫ exp[−0.5( xk − f ( xˆk −1 ))T Q −1 ( xk − f ( xˆk −1 )) − 0.5( yk − Hxk )T R −1 ( yk − Hxk ) + µ1 ( xk − xˆk |k −1 )T ( xk − xˆk |k −1 )]dxk or α exp[(0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk )T (0.5Q −1 + 0.5 H T R −1 H − µ1 I ) −1 (0.5H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ) − (0.5 f ( xk −1 )T Q −1 f ( xk −1 ) + 0.5 ykT R −1 yk − µ1 xˆkT xˆk ) × ∫ {exp[−( xk − (0.5Q −1 + 0.5 H T R −1H − µ1I ) −1 (0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ))T ) × (0.5Q −1 + 0.5 H T R −1H − µ1I )( xk − (0.5Q −1 + 0.5 H T R −1H − µ1I ) −1 (0.5 H T R −1 yk + 0.5Q −1 f ( xk −1 ) − µ1 xˆk ))]}dxk

From the above expression it can be seen that the term under the integration is nothing but the Gaussian probability density function with mean m k and covariance Σ k . Where 0.5Σ −k 1 = (0.5Q −1 + 0.5H T R −1 H − µ1 I ) Or Σ k−1 = (Q −1 + H T R −1 H − 2µ1 I ) And m k = (0.5Q −1 + 0.5H T R −1 H − µ1 I ) −1 (0.5H T R −1 y k + 0.5Q −1 f ( x k −1 ) − µ1 xˆ k )

Or m k = Σ k ( H T R −1 y k + Q −1 f ( x k −1 ) − 2µ1 xˆ k ) So for this class of system the optimal proposal will be the Gaussian distribution with mean m k and covariance Σ k i.e N (m k , Σ k ) . The term outside the integration will act as the weight for the particle generated from the optimal distribution.

Case 2 Computation of Approximate Optimal Proposal with Nonlinear System and Measurements This example deals with a general system with nonlinear process and measurements. The system dynamics and measurement equation are given by equations (7.1) and (7.2) respectively, where, wk ~ N (0, Qk ) and v k ~ N (0, R k ) . For such nonlinear Gaussian case no optimal proposal

distribution exists. The optimal importance distribution [Doucet 2000] may be approximated

131

Chapter 7

using local linearisation. Let us called such proposal as linearised optimal proposal. To derive such a proposal let us consider the nonlinear Gaussian measurement equation. y k = h( x k ) + v k , where noise is Gaussian i.e v k ~ N (0, R k )

Above nonlinear measurement equation can be linearised similar to [Doucet 2000] y k ≈ h( f ( x k −1 )) +

∂h( x k ) | xk = f ( xk −1 ) ( x k − f ( x k −1 )) + v k ∂x k ∂h( x k ) ∂h( x k ) | xk = f ( xk −1 ) f ( x k −1 ) = | xk = f ( xk −1 ) x k + v k ∂x k ∂x k

or

y k − h( f ( x k −1 )) +

or

∂h( x k ) ~ yk = | xk = f ( xk −1 ) x k + v k ∂x k

∂h( x k ) where ~y k = y k − h( f ( x k −1 )) + | xk = f ( xk −1 ) f ( x k −1 ) ∂x k

For prior RSPF estimator, q kopt (x k | x k -1 ) ~ N (m k , Σ k ) , T

where Σ k

−1

= Qk

−1

∂h(x ) R −1 ∂h(x k ) k + − 2 µ1 I k ∂x k x = f ( x ) ∂x k xk = f ( xk −1 ) k k −1

,

T

∂h(x ) −1 R −1 ~ k ˆ mk = Σ k (Qk f ( xk −1 ) + k yk − 2 µ1 xk |k −1 ) ∂xk x = f ( x ) k k −1

7.4

Posterior Risk Sensitive Particle Filter Formulation

7.4.1

Solution of Risk Sensitive Estimation Problem for Posterior Estimation

Similar to section 7.3 the posterior solution of risk sensitive estimation for nonlinear case has been provided in this section. The solution for posterior can be obtained as

( (

))

(7.26)

ˆ = arg min ∫ exp(µ ρ (φ ( x ) − ς ))α (x )dx Φ k |k 2 2 k k k k

(7.27)

ˆ αk (xk ) = p( yk | xk )∫ p(xk | xk −1) exp µ1ρ1 φ(xk −1) − Φ k −1|k −1 αk −1(xk −1 )dxk −1

and the optimal estimate is +∞

ς ∈R

−∞

The relation has been derived by the present author and has been presented in a form of theorem in chapter [4].

7.4.2 Probabilistic Interpretation Similar to prior RSPF probability interpretation of posterior RSPF has been given in this chapter.

132

Chapter 7

We normalize the probability distribution of {α k (.)} to obtain probability distributions {α k (.)} defined by α k (x k ) =

α k (x k ) . ∫ α k ( x k )dx k

Let us introduce another probability density function

(

(

))

β k −1 ( x k −1 ) = p( y k | x k ) exp µ 1 ρ 1 φ ( x k −1 ) − Φˆ k −1|k −1 α k −1 ( x k −1 ) The normalised version of that probability

density function can be obtained as β k −1 (x k −1 ) =

{ }

The normalised probability density α k

β k −1 (x k −1 )

∫ β k −1 (x k −1 )dx k −1

can be written using newly introduced probability

density function β k as follows: +∞

α k ( xk ) =

( ( | x ) exp(µ ρ (φ ( x

)) ))α

ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 p ( x k | x k −1 )dx k −1

−∞ +∞ +∞

∫ ∫ p( y k

− ∞ −∞

k

1

1

k −1

ˆ )−Φ k −1|k −1

k −1

p( x k | x k −1 )dx k −1 dx k

After some simple manipulation we get α k ( x k ) = ∫ p( x k | x k −1 ) β

k −1

(7.28)

dx k −1

Where β k −1 (x k −1 ) =

(

k

=

(

(

))

ˆ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 ( x k −1 ) ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 (x k −1 )dx k −1 ˆ α (x ) p( y | x ) exp µ ρ φ ( x ) − Φ k

(

(

(

1

(

1

(

k −1

k −1|k −1

ˆ ∫ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1

(

)) )) ))α

(

k −1

k −1

k −1

(x k −1 )dx k −1

))

ˆ β k −1 (x k −1 ) ∝ p( y k | x k ) exp µ1 ρ1 φ ( x k −1 ) − Φ k −1|k −1 α k −1 ( x k −1 )

(7.29)

7.4.3 Particle Implementation •

Initialization; k=0 Sample X 0(i ) ~ p 0 (.) to obtain w0i =

•

(

1 N (i ) ∑ δ x0 − X 0 N i =1

)

At time k; k>0

(

~

~

Sample X k(i ) ~ q k . | X k(i−)1

)

Assigned weight i

wk ∝

(

)

(

(

~ ~ ~ ~ ˆ p y k | X k(i ) p ( X k(i ) | X k(i−)1 ) exp µ1 ρ 1 φ ( X k(i−)1 ) − Φ k −1|k −1 ~ i ~ (i ) q k ( X k | X k −1 )

Normalise weight

133

))

Chapter 7

Resample { X ki , wki } N Estimate Φˆ k |k = arg ζ min ∑ exp(µ 2 ρ 2 (φ ( x k ) − ς )) i =1

7.4.4 Optimal Proposal Case 1 Computation of Optimal Proposal for RSPF with Nonlinear System and Linear measurements Optimal proposal for posterior RSPF in Gaussian noise linear measurement situation can be derived following the same steps as done in section 7.3.4. In that situation proposal density function will be q k opt ( x k | x k −1 ) ~ N (m k , Σ k ) . Where Σ −k 1 = H T Rk −1 H + Qk −1

(7.30)

and m k = Σ k ( H T Rk −1 y k + Qk −1 f ( x k −1 ))

(7.31)

And the weights associated with each particle can be given by

(

(

))

ˆ wki ~ exp(−0.5( y k − Hf ( x k −1 )) T ( HQk H T + Rk ) −1 ( y k − Hf ( x k −1 )) + µ1 ρ 1 φ ( x k −1 ) − Φ k −1|k −1 )

It should be noted here that for prior formulation of risk sensitive particle filtering the risk sensitive parameter appears in mean and covariance of the proposal distribution as well as in expression for weight. But in posterior formulation risk sensitive parameter appears only in calculation of weight not in proposal density function.

Case 2 Computation of Approximate Optimal Proposal with Nonlinear System and Measurements The system dynamics and measurement equation for nonlinear Gaussian system are given by equations (7.1) and (7.2) respectively, where, wk ~ N (0, Q k ) and v k ~ N (0, R k ) . For such nonlinear Gaussian case no optimal proposal distribution exists as discussed in prior RSPF. The linearised optimal proposal may be approximately obtained by following the steps [Doucet 2000] described in 7.3.4. Doing similar mathematical manipulation as done in prior RSPF we get ∂h( x k ) ~ yk = | xk = f ( xk −1 ) x k + v k ∂x k

Now qk

opt

the

linearised

optimal

importance

distribution

( x k | x k −1 ) ~ N (m k , Σ k ) T

Where

Σk

−1

∂h(x ) −1 ∂h( xk ) k = Q + Rk ∂xk x = f ( x ) ∂xk x k = f ( xk −1 ) k k −1

m k = ∑ k (Q k

−1 k

−1

f (x k −1 ) + (

∂h( x k ) −1 | xk = f ( xk −1 ) ) T R k ~ yk ) ∂x k

The weights associated with each particle can be assigned as

134

can

be

computed

as

Chapter 7

∂h(x k ) T ∂h( x k ) wki ~ exp(−0.5( ~ y k − h( f (x k −1 ))) ( | xk = f ( xk −1 ) ×Qk ( | xk = f ( xk −1 ) ) T + Rk ) −1 ( ~ y k − h( f (x k −1 ))) ∂x k ∂x k ˆ + µ ρ φ(x ) − Φ )

(

7.5

1

1

(

k −1

k −1|k −1

))

Notes •

In this chapter prior and posterior risk sensitive particle filter have been formulated. Taking the value of risk sensitive factor as zero, ordinary prior particle filter can be formulated which is not very common in particle filtering literature. That type of filter may become useful for real time system with delayed measurement.

•

If we take the value of µ1 =0 in RSPF, all the expressions reduce to simple particle filter equations.

•

It should be noted that in prior RSPF the risk sensitive parameter appears in the expression of mean and covariance of optimal importance distribution (which is Gaussian) and also in weights where as in posterior RSPF the risk sensitive parameter appears in expression of weights only.

•

In particle filter we know that the efficiency of particle filtering depends on successful choice of proposal. Similar statement is true for risk sensitive particle filter. In this chapter the optimal proposal and linearised optimal proposal have been derived. Other proposals such as EKF, UKF, applicable to particle filter can also be applied to RSPF.

•

The use of other risk sensitive nonlinear filters such as extended risk sensitive filter (ERSF), Central difference risk sensitive filter (CDRSF) as a proposal of RSPF may be more appropriate instead of ordinary risk neutral filters. Further investigation is necessary in this direction.

•

The main draw back in particle filtering is its computational burden. It has been noticed that approximately 10 times of particle is necessary comparative to grid points for equivalent results obtained from AGRSF. RSPF requires random sampling and resampling method, which is not necessary for AGRSF. Further grid adaptation policy is necessary for AGRSF, which is not necessary for RSPF. However it is observed that RSPF is computationally more faster that AGRSF to obtain the same accuracy.

•

In the next section we will experimentally investigate about the convergence property of RSPF. Our investigation reveals that the risk sensitive particle filter converges quickly towards the true solution for linear Gaussian models. Most rigorous derivation of convergence is necessary. This remains under the scope of future work.

135

Chapter 7

•

To obtain the estimated value of the convex function, searching is necessary among the particles in state space as described in equation (7.6). To obtain or to search the appropriate value for states all the mechanisms tried in AGRSF have also been tried for RSPF. However it should be noted that computational burden increases heavily due to search mechanism. It can be replaced by assuming normal probability density function and hence finding out the mean and variance.

•

Throughout the formulation in this chapter the estimate of any function of state ( φ ( x k −1 ) ) and for risk sensitive cost function generalized convex functions ( ρ 1 (.) and ρ 2 (.) ) have been considered. If the variables to be estimated are the state variables themselves ( Φ ( x ) = xˆ ), and both the convex functions ρ 1 (.) and ρ 2 (.) are known quadratic functions of the error then ρ j (α ) = α 1 T α 1 where

α1

is the error in estimation of state

variables. In the following section where case studies has been considered the quadratic cost function has been assumed and values of the states have been estimated.

7.6

Case Study

Like AGRSF, first RSPF has been applied to single dimensional linear Gaussian case. The estimated value obtained from RSPF compared with risk sensitive Kalman filter (RSKF). The exact matching between RSKF and RSPF for linear Gaussian case validates the risk sensitive particle filter formulation. In this section RSPF has been applied to a single dimensional nonlinear problem as well as BOT problem. The results and different issues will be discussed.

7.6.1

1D Nonlinear Case

We consider the one-dimensional process and measurement sequence described in section 3.4. We have considered the time from 0 to 0.8 sec. The state of the system is estimated using the ERSF and AGRSF and newly proposed RSPF. Value of risk sensitive parameter ( µ1 ) is taken as 0.0756. For AGRSF, number of grid points taken is 100. For RSPF 1000 number of particle has been used. The truth and estimated value of state obtained from the above mentioned filters is shown in figure 7.1 for a representative run. It is noted that for this particular run the ERSF fails to track the truth while AGRSF and RSPF track the truth well. It can also be noticed that for this particular problem the error settling property for RSPF is found to be better than AGRSF. RMS errors of AGRSF, ERSF have been compared using 1000 Monte Carlo runs in figure 7.2. It can be noted that the RMSE of ERSF is quite high where as the RMSE of AGRSF convergences to a much lower value. The convergence rate of RSPF is higher than AGRSF.

136

Chapter 7

1.5 1

truth & estimate

truth AGRSF

0.5

RSPF ERSF

0 0

20

40

60

80

-0.5 -1 -1.5

step

Fig 7.1 Truth & estimated value of state obtained from ERSF, RSPF and AGRSF 1.2 AGRSF RSPF

1

ERSF

RMSE

0.8 0.6 0.4 0.2 0 0

20

40 Step

60

80

Fig 7.2 RMSE of ERSF, RSPF and AGRSF for 1000 MC run

Study of Computational Load Computational load for AGRSF and RSPF has been studied and compared. Although it is extremely difficult to compare the computational load between these two numerical risk sensitive filters, because different number of grid points and particles are necessary to obtain satisfactory and consistent results. It has been observed for RSPF approximately 10 times of particle is necessary than that of AGRSF method. The time for a single run of AGRSF (with different number of grid points) and the time for a single run of RSPF (for different number of particles) in Matlab software in a P-IV (2.66 GHz) processor is summarised in table 7.1. From the table it is seen that that the computational efficiency of RSPF is at least 4 times better than AGRSF.

137

Chapter 7

Table 7.1: Time Calculation for a single run of RSPF and AGRSF for different number of grid point and particle

AGRSF No of grid 100 200 400 7.6.2

Run time (in sec) 80.375 312.407 1265.3

RSPF No of particle 1000 2000 5000

Runtime (in sec) 21.06 56.703 252.844

2D Bearing Only Target Tracking In this section, the risk sensitive particle filter (RSPF) has been applied to the second

order bearing only tracking test problem describe in section 3.6.1. The measurement model has been taken as discussed in section 3.6.2 (measurement model I). The filter has been initialised as discussed in section 3.6.5.

Sensitivity With Risk Sensitive Parameter In this study RMSE for both position and velocity has been calculated for different risk sensitive parameter using 1000 MC run. It is observed that the estimation is very sensitive with risk sensitive parameter. For example for risk sensitive parameter µ1 =0.001, the RMSE of position diverges (the case has not been shown in position RMSE error plot). It can be seen from the simulation results that for larger µ1 the RMSE for both position and velocity is higher than PF as risk sensitive parameter decreases to zero RSPF result matches exactly with particle filter (Fig 7.3 and Fig 7.4). It has been seen earlier for BOT problem if the initialisation error for position is large EKF diverges but PF does not. Similar to that for large initial position error ERSF diverges but RSPF does not for the same risk sensitive factor. RSPF for different risk sensitive parameter 25

mu=0 mu=0.0001

20

RMSE (position)

mu=0.000001

15

10

5

0 0

5

10

step

15

20

25

Fig 7.3 RMSE of position for different risk sensitive parameter

138

Chapter 7

RSPF for different risk sensitive parameter 1.2 mu0

1

mu=0.001

RMSE (velocity)

mu=0.0001 mu=0.000001

0.8 0.6 0.4 0.2 0 0

5

10

step

15

20

25

Fig 7.4 RMSE of velocity for different risk sensitive parameter

Another experiment on BOT problem has been done where the value of sampling time is perturbed. The nominal value of sampling time has been taken as one second and it has been perturbed to +-0.5 second for the truth model where as the filter model assume one second sampling time. RSPF has been simulated in that situation. Experiment has been done with different risk sensitive parameter. Comparison of terminal RMSE for position and velocity has been made. From the result the superiority of RSPF over PF cannot be claimed for this situation.

7.7

Convergence Study

Experiment 1: 1D Nonlinear System and Nonlinear Measurements The system taken for this experiment is described in section 3.5. Risk sensitive parameters are taken as 0.2 ( µ1 = µ 2 =0.2). Fig 7.5 shows that the variance of the error across Monte Carlo (MC) runs converges to zero as the number of particles is increased. This indicates that for this example, the risk sensitive particle filter converges as the number of particles is increased.

Experiment 2: 2D BOT Problem The RSPF has been simulated for 2D BOT problem described in section 3.6 using different number of particle. RMSE for 1000 run with different number of particle has been shown in fig 7.6 and 7.7. The value of risk sensitive parameter has been chosen as 0.00001. From the figure it can be seen that as the number of particle increases the RMSE for both position and velocity decreases. As the number of particle increased the error between two consecutive run (for example run with 400 particle and 500 particle) decreases. It indicates RSPF is converging.

139

Chapter 7

Again for BOT problem variance of error for position across several Monte Carlo runs at a particular time step, (i.e. 18 sec), versus the number of particles has been plotted in fig 7.8. It is seen from the figure that RSPF converges with increasing number of particles for this nonlinear application example. The variance of error for velocity also shows a very similar trend.

Variance of Error across MC Runs

0.09 0.08 0.07 0.06 0.05 0.04 0.03 0.02 0.01 0 10

100

1000

10000

No. of particles

Fig 7.5 Variance of error across MC runs vs no of particles for 1D nonlinear system RSPF for different number of particle

25

N=100 N=200 N=300 N=400 N=500

20

RMSE

15

10

5

0 0

5

10

step

15

20

Fig 7.6 RMSE of position for different particle

140

25

Chapter 7

RSPF for different numbe of particle

1.2 N=100

RMSE(velocity)

1

N=200 N=300

0.8

N=400 N=500

0.6 0.4 0.2 0 0

5

10

step

15

20

25

Fig 7.7 RMSE of velocity for different particle

7.8

Scope for Further Work (i)

It is well known and discussed above that the estimation accuracy of RSPF depends on the choice of proposal similar to ordinary particle filter. For PF it has been proven that using other nonlinear filter such as extended Kalman filter, unscented Kalman filter (UKF), the estimation accuracy can be increased in comparison to prior proposal. Similarly using other risk sensitive filter such as ERSF, RSUKF, and CDRSF as proposal for RSPF the accuracy may be increased. The possibility need to be explored more.

(ii)

In this chapter convergence of RSPF has been proved numerically using simulation results. But this is not a rigorous way to prove the convergence of RSPF. So in future the convergence of RSPF need to be proved in a more rigorous mathematical way. In this regards Crisan’s work [Crisan 2002] may be helpful.

(iii)

In this chapter developed RSPF has been applied to real time situation in a simple BOT problem. But no improvement can be claimed in comparison to PF even for the situation when truth model differs from filter model. So it is necessary to find out the situations under which RSPF behave significantly better than PF. This can be achieved through proper understanding of properties of RSPF.

141

Variance of Error for position across MC runs (m)

Chapter 7

3 2.5 2 1.5 1 0.5 0 50

150

250

350

450

550

650

750

850

No. of Particles

Fig 7.8 Variance of error for position vs no. of particles in 2D BOT problem

7.9

Discussion and Conclusion Particle filter based solutions of the risk sensitive estimation problem for nonlinear non

Gaussian systems has been developed in the present chapter. Both the prior and posterior formulation has been discussed. The developed filter is expected to be robust with respect to model uncertainty. As for standard particle filters, it is important in practice to use or approximate the optimal importance distribution to obtain good performance. Here optimal proposal distribution has been derived for a Gaussian system with linear measurement. Applicability of different type risk sensitive filter as a proposal has been discussed. Experimentally, our investigation reveals that the risk sensitive particle filter converges quickly towards the true solution. Rigorous mathematical proof of convergence remains under scope of future work. Computational load is obviously a key concern for considering RSPF in real time scenario. In the earlier chapter AGRSF has been formulated. In this chapter comparison is made with RSPF in terms of computational efficiency and accuracy of estimation. It has been found that RSPF remains numerically superior although a larger number of particles compared to the number of grid points has to be used to achieve the same accuracy.

142

8

Cramer Rao Lower Bound and Preliminary Design for Target Tracking Systems

8.1

Introduction Cramer Rao Lower bound (CRLB) for parameter estimation provides the minimum

variance of estimation error that is achievable by any unbiased filter and the theory for the same had been around for more than half a century. According to [Ristic 2004a], the Cramer Rao inequality was first stated by Ronald Fisher (1925) later reformulated simultaneous by C R Rao and Harold Cramer in 1945. Although the bound parametric estimation evolved nearly fifty years ago, assigning bounds for state estimation of nonlinear multivariable dynamic system has a fairly recent origin, mainly due to the work of Taylor [Taylor1979] and Tichavsky [Tichavsky1998]. The bound developed by Taylor for state estimation does not have mathematical rigour what now a days available with CRLB. But that has started enjoying fairly wide patronage [Kerr 1989, Doerschuk 1995] among target tracking practitioners. Though some authors have reservations, we would call the techniques for providing lower bounds for state estimation of nonlinear multivariable dynamic system by the generic name CRLB. As for linear estimation problem Kalman filter gives the optimal solution, CRLB coincides with the error covariance calculated from Kalman filter algorithm and its use is limited. For nonlinear problems, the CRLB gives lower bound of the error covariance. As of now no general optimal filtering method is available for nonlinear system. So no matter what filtering technique we use, CRLB will provide the lowest error covariance. We can compare the different nonlinear filtering technique about to what extent it is close to CRLB. So the bound acts as a benchmark to demonstrate efficiency of a specific filter [Ristic 2003a]. In this work we extend its use to answer questions like whether a set of projected filtering specification is realistic with the given kinematic scenario and tracker instrumentation /signal processing performance restrictions, before detailed design effort is undertaken. We demonstrate further, how the technique also permits a design trade off analysis with performance improvement versus cost of improved instrumentation and signal processing equipment. The present worker had possibly been the first [Bhaumik 2006 c] to carry out a systematic parametric analysis of the Crammer Rao bound for tracking systems and to point out the potential use of CRLB for preliminary system design of tracking filters. This section is primarily based on the above publication. For additive noise models simple algorithms have been published for CRLB computation [Tichavsky 1998]. Noting that such popular algorithms for CRLB computation require either a

Chapter 8

non singular [Ristic 2003a] process noise covariance matrix ( Q ) or assumes it to be a null matrix [Taylor 1979]. Simple expression provided by the present author in [Bhaumik 2006c] can be used to compute CRLB for linear process in presence of singular Q matrix. In Srinivasan’s thesis [Srinivasan 2007] informal formulation of the CRLB recursive equation has been done which can handle singular Q matrix for both linear as well nonlinear processes. The reformulation also provides a deeper insight into the variation of error with respect to several parameters. In the tracking scenario the seeker usually provides the line of sight (LOS) angle and the LOS rate with respect to the interceptor body frame or inertial frame for guidance [Oshman 2003, Bar Shalom 1993]. When the target tracking is made using LOS only the tracking is known as bearing only tracking (BOT). It has become an important field of study and has attracted the attention of many workers [Sadhu 2006, Lin 2002, Bar Shalom 1993]. The primary motivation for studying BOT is it’s practical military and civil applications including under water weapon systems, Infra Red seeker based tracking, sonar based robotic navigation and TV camera or stereo microphone based people tracking. For weapon system guidance, BOT allows use of passive trackers [Chan 2002] with the consequent tactical advantages. No less important is the fact that BOT problems have nonlinearity and poor observability [Song 1996], which generally stresses the tracking filters. Detailed literature available on this has been reviewed in chapter 2. Use of CRLB was initially restricted to underwater weapon system designers [Kerr 1989] but of late, its increasing popularity among aerospace system designers [Ristic 2003a, Chan 2002] had become noticeable. One possible reason could be the availability of newer sensors and newer filtering techniques like particle filter and sigma point Kalman filter [Sadhu 2006] and renewed interest in homing interceptors against tactical ballistic missiles using bearing only sensors [Ristic 2003b]. Yesteryears extended Kalman Filter was probably the only choice available for nonlinear tracking problems. Now a days a variety of filters [Ristic 2003a] like unscented Kalman filter and particle filter are available. So choice of filter becomes as important as the choice of guidance laws. The configuration selection has to be carried out at an early phase of the design life cycle of the interceptor, when even the airframe and propulsion system of the interceptor may still be evolving, subsystem data may be only partially available. The design/selection toolset for this phase should have been entirely different from the detailed design and validation tools. In reality, a team either depends on the intuitive feel of an experienced designer or goes for a detailed analysis. For example, the effects of faster sampling, lower measurement noise etc. on filtering

144

Chapter 8

performance are known qualitatively, for specific nonlinear problems, it is difficult to estimate even “ball park” quantitative results without extensive Monte Carlo simulation. Proper Monte Carlo analysis with 6 DOF simulation mode, need far more design details than available in the configuration selection phase. We therefore advocate taking a middle road between the “gut feelings” of a veteran and a detailed MC study. Use of CRLB fits our requirement admirably. The CRLB gives an indication of the achievable lower bound of the error covariance and acts as a benchmark to demonstrate efficiency of a specific filter [Ristic 2003a], especially so, as the filters for nonlinear problems are suboptimal. CRB also permits design trade off analysis with performance improvement versus cost of improved instrumentation and signal processing equipment [Taylor 1979]. We demonstrate the power of CRLB analysis with the help of a bearing only tracking (BOT) problem. Results of earlier parametric studies sometimes indicated conflicting trends across different families of filters. In the present chapter, instead of using individual filtering techniques we use the generic Cramer Rao Lower Bound (CRLB) [Taylor 1979, Sadhu 2006] to study the effect of (i) sampling time (ii) measurement error covariance (iii) process excitation covariance (iv) initial error covariance (v) additional measurement (vi) eclipsing effect on state estimation. The study has already been in published [Bhaumik 2006b, Sadhu 2004a]. 8.2

CRLB Computation In this section the algorithm used to calculate CRLB has been demonstrated. The version that

has been stated here is the posterior version of CRLB called as posterior Cramer Rao lower bound or PCRLB [Tichavsky1998]. Let us consider nonlinear system and measurements with additive gaussian (process as well as measurement) noise described by: x k +1 = f k ( x k ) + wk

(8.1)

z k = h( x k ) + v k

(8.2)

Let Qk and Rk be the covariances of zero-mean noises wk , and v k respectively. Given a state estimation problem, the lower bound of the RMS error (LBRE) of individual state are given by the square root of the corresponding diagonal elements of Fisher information matrix J k . ( LBRE ) {x j (k )} =

J −jj1,k

(8.3) The minimum variance covariance matrix Pmv (often referred as the CRB matrix) is related to the information matrix as Pmv ,k = J k −1 .

145

Chapter 8

The CRLB computation becomes simplified if the state equation is linear, noise is Gaussian, additive and the output variable is a scalar. Applying these simplifying assumptions on the algorithm proposed in [Tichavsky 1998], the information matrix may be recursively computed [Tichavsky 1998] as follows: J 0 = P0

−1

J k +1 = Dk

(8.4)

22

− Dk [ J k + Dk ]−1 Dk 21

11

12

(8.5)

The expressions D ij are explained below. Dk

11

Dk

12

Dk

21

Dk

22

−1 T = − E{Fk }Q k ; 12 T = [ Dk ] −1 −1 = Q k + E{[∇ xk +1 hk +1 ( x k +1 )]R k +1 [∇ xk +1 hk +1 ( x k +1 )]T } T

−1

= E{Fk Q k Fk };

(8.6)

Where Fk = [∇ xk f k T (x k )]T and H k +1 = [∇ xk +1 hk +1T (x k +1 )]T are evaluated at the true state, where E{}is the

exception

operator

(to

be

determined

by

Monte

Carlo

simulation)

and

∇x = [

∂ ∂ ∂ T , , ..., ] . For linear plant model the process model will be xk +1 = Fk xk + wk . In this ∂x1 ∂x2 ∂xn

case the expression D ij will be 11

T

−1

Dk = E{Fk Qk Fk } ; Dk

12

T

−1

= − E{Fk }Qk ;

(8.7)

For linear measurement model the measurement equation will be z k = H k x k + v k and D ij will be Dk

22

8.3

−1

T

−1

= Qk + E{H k +1 Rk +1 H k +1}

(8.8)

Parametric Studies of BOT Problem Using CRLB In this section CRLB has been applied to BOT problem described in section 3.6.

Variation of CRLB with different model parameter has been observed. The first difficulty in implementation of CRLB to BOT problem arises due to singular Q matrix because to evaluate equation 8.6, Q matrix needs to be invertible. This however, does not imply that CRLB is undefined for singular Q matrix. For this BOT problem initially the Q matrix was perturbed by infinitesimal small value from singularity (typical value 0.001 in diagonal terms) so that it becomes invertible and apply the equation 8.6. The formulation by Tichavsky [Tichavsky 1998], does mention this problem but provides a very round about way of tackling it. Later the problem

146

Chapter 8

was addressed by the present author in [Bhaumik 2006c]. CRLB with a null Q matrix may be computed [Taylor 1979] from the relations −1

J k +1 = ( F −1 )T J k F −1 + E ( H k +1 Rk +1 H k +1 ) T

(8.9)

The difficulties of singular Q matrix may be avoided by reformulating eqn (8.9). We use matrix inversion lemma [Bar Shalom 1993] and the fact that the state matrix is linear. It may be noted that [Ristic 2003b] mentions a similar formula but there are obvious errors in their version. The recursion formula for the information matrix may be reformulated as −1

−1

J k +1 = (Qk + Fk J k Fk ) −1 + ( H k +1 Rk +1 H k +1 ) T

T

(8.10)

For the initial covariance matrix P0 , the difference between CRLB approach and the other filters should be noted. In the present BOT problem, the truth model of the initial condition is deterministic. As the truth is unknown or partially known, for specific filter a designer is free to choose a suitable P0 . However, the CRLB formulation assumes the problem initial uncertainty to be solely defined by the initial covariance matrix P0 . For the initial position uncertainty, this does not pose a problem. But for the initial velocity uncertainty, the specific filters cited here have some advantage (over the CRLB) as prior knowledge has been used. This would be reflected in the simulation results. 8.3.1

Nominal Performance Comparison

The LBRE results obtained from Monte Carlo run, when different simulation parameters (measurement error covariance, initial error covariance, process noise covariance and sample time) are in nominal condition, are shown in fig 8.1 and 8.2. The nominal condition of these parameters means the simulation has been carried out with the values mentioned in section 3.6. The corresponding RMS error for EKF [Sadhu 2006, Lin 2002] and Particle filter [Lin 2002] has been plotted alongside. No risk sensitive filter has been compared with CRLB because no lower bound for risk sensitive filtering is available presently. 8.3.2

Influence of Sampling Time on CRB

One important design issue in tracking system is the sampling time (inverse of sampling frequency). A faster sampling rate generally involves faster computation and possibly costlier computers. Faster sampling time may have other consequences like it may increase measurement noise, specially for radar based systems, where higher integration time (inter sample time) reduces noises. In this section we do not consider such inter coupling issues and wish to

147

Chapter 8

investigate the effect of sampling time on the CRLB performance while other parameters remain the same.

LBRE and RMSE of position (m)

30

LBRE PF

25

ekf

20 15 10 5 0 0

5

10

15

20

25

Time (sec)

LBRE and RMSE of velocity(m/s)

Fig 8.1 Comparison RMS error performance of EKF and PF with LBRE of position at nominal condition of Rk , P22,0 , Q and T

1.2

LBRE PF

1

ekf

0.8 0.6 0.4 0.2 0 0

5

10

15

20

25

Time (sec)

Fig. 8.2 Comparison of RMS error performance EKF and PF with LBRE of velocity at nominal condition of Rk , P22,0 , Q and T

With the assumptions as above, it is reasonable to expect that CRLB and the LBRE would decrease with the decrease of sampling time. However, whether such expectations are justified and to what quantitative extent need to be investigated for specific cases. From Figs 8.3 and 8.4, where the LBRE is calculated and plotted for both position and velocity for different sampling times (0.2s, 0.5s, 0.8s and 1s) it is seen that a lower sampling time indeed noticeably decreases the CRLB for both position and velocity. More specifically, while for the nominal sampling time (T=1 sec), the time required for the position LBRE to settle to 5 m (about

148

Chapter 8

one fifth of the initial) is about 9 sec, the corresponding time for T=0.5 sec and T=0.2 sec are respectively 5.5 sec and 2.8 sec. For the position error case the beneficial effects of reduced sampling time is more pronounced in initial error settling and the differences taper out as the filter converges. For the velocity LBRE, the time required to settle to 0.6 m/s are respectively 14s (for T=1s), 11.5s (for T=0.5s), 9.2s (for T=0.2s). It may be seen that for velocity error, the effect of faster sampling is not visible up to about first 3 seconds, whereas, at the later phase the effects become substantial. It may be noted that the above effects could not have been predicted without the CRLB analysis. 30

T=0.2s T=0.5s T=0.8s T=1s

LBRE of position (m)

25 20 15 10 5 0 0

5

10

15

20

25

Time (sec)

Fig. 8.3 Influence of the sampling time on LBRE of position

8.3.3

Influence of Measurement Error Covariance

The measurement error covariance indicates basically the quality of the sensor and its allied electronics. This is therefore of fundamental importance. A sensor with a lower error covariance is generally costlier. Therefore a trade off may be necessary. The effect of substantial changes in measurement error covariance is evident from eqn (8.6). With increase in R k , the terms of matrix J k +1 decreases, thus increasing the LBRE of position and velocity. The reverse is true with decrease in R k . Figs 8.5 and 8.6 show that increase in measurement error covariance causes poorer settling of position error. When the measurement error covariance is 1/5th of the nominal value, the terminal position error reduces to 0.48 m compared to 1.01 m in the nominal case. Velocity error reduces to 0.2 m/s compared to 0.29 m/s in the nominal case at 20s.

149

Chapter 8

In the sampling time studies in the previous section, the measurement error covariance has been kept constant. However, the measurement error covariance may be coupled with sampling time for some types of passive tracking instruments. A longer sampling interval sometimes provide lower measurement error and this should be kept in mind, while interpreting the result. 1.2

T=0.2s T=0.5s T=0.8s T=1s

LBRE of velocity(m/s)

1 0.8 0.6 0.4 0.2 0 0

5

10 Time (sec) 15

20

25

Fig 8.4 Influence of the sampling time on LBRE of velocity

LBRE of position (m)

30

0.1*R 0.2*R 0.5*R R 2*R 5*R

25 20 15 10 5 0 0

5

10

15

20

25

Time (sec) Fig. 8.5 Influence of measurement error covariance on LBRE of position

150

Chapter 8

1.2

0.1*R 0.2*R 0.5*R R 2*R 5*R

LRBE of velocity(m/s)

1 0.8 0.6 0.4 0.2 0 0

5

10

15

Time(sec)

20

25

Fig. 8.6 Influence of measurement error covariance on LBRE of velocity

8.3.4 Influence of Process Excitation Covariance The effect of changes in process excitation covariance is that, with increase in Qk , J k +1 decreases as is evident from eqn 8.6. This has the effect of increasing the LBRE of position and velocity. However, it is observed from figs 8.7 and 8.8 that large variation of Q produces little change in position LBRE. For velocity LBRE, however, error settling becomes poorer with large error overshoots in velocity as the norm of Q is increased. This implies that there is an overall increase in LBRE with the increase in Q . A larger value of Q indicates larger manoeuvres by the target or greater modelling inaccuracies. The result shows that these may adversely affect the velocity estimation but not so much for position estimation. A wide range of Q shows the applicability of the proposed formulation without any numerical problem.

8.3.5 Influence of P22,0 It would be of interest to know the effect of the initial covariance term ( P22, 0 ) associated with velocity, as this value was tinkered with and a nominal value of 1 was used in the original filtering problem [Lin 2002] to avoid large values of P22 , 0 . Figs 8.9 and 8.10 show the variation of LBRE of position and velocity respectively with P22 , 0 changed over some reasonable range. It is observed that P22 , 0 , does not have any effect on the LBRE of position even for large values of P22 , 0 . However, it does have a significant effect on the LBRE of velocity. Also, a change in 151

Chapter 8

P22, 0 does not have significant effect on the final error settling after 20s, which is 1.01 m (for

position) and 0.29 m/s (for velocity) irrespective of the initial condition chosen. Thus the gain obtainable by using prior knowledge is to a large extent quantified.

30

0.1*Q 0.2*Q 0.5*Q Q 5*Q 10*Q 15*Q 20*Q 25*Q

LBRE of position(m)

25 20 15 10 5 0 0

5

10 15 Time(sec)

20

25

Fig. 8.7 Influence of process excitation covariance on LBRE of position 1.6

0.1*Q 0.2*Q 0.5*Q Q 5*Q 10*Q 15*Q 20*Q 25*Q

LBRE of velocity (m/s)

1.4 1.2 1 0.8 0.6 0.4 0.2 0 0

5

10 15 Time (sec)

20

25

Fig. 8.8 Influence of process excitation covariance on LBRE of velocity

152

Chapter 8

LBRE of position (m)

30

P22(0)=0.2 P22(0)=0.5 P22(0)=1 P22(0)=2 P22(0)=5

25 20 15 10 5 0 0

5

10

15

Time (sec)

20

25

Fig. 8.9 Influence of P22,0 on LBRE of position

LBRE of velocity (m/s)

2.5 P22(0)=0.2 P22(0)=0.5 P22(0)=1 P22(0)=2 P22(0)=5

2 1.5 1 0.5 0 0

5

10

15

Time (sec)

20

25

Fig. 8.10 Influence of P22,0 on LBRE of velocity

8.3.6 Adding Another Measurement Sightline rate signal is usually available from the seeker. It would be of interest to know whether use of such signal would improve the estimation accuracy. Measurement Model-II discussed in chapter 3.6.3 and a sampling time of 0.1 s have been used for this study. Three cases are considered for SLR noise covariance ( R22 ) of 0.01, 0.001 and 0.0001 rad/ s 2 . The last figure is equivalent to the one sigma ( 1σ ) error of 10 mil/s and considered fair for this class of seeker. While introducing this additional measurement has hardly impact on the LBRE of position (fig 8.11), the same for velocity is shown in fig 8.12 indicates substantial improvement over the single measurement case (with only sight line angle measurement) when R22 is small. For large value of R22 the results is the same as that for single measurement.

153

Chapter 8

LBRE of position(m/sec)

30 R22=0.0001 R22=0.001 R22=0.01 SLAmeasure

25 20 15 10 5 0 0

5

10

Time (sec)

15

20

Fig-8.11. Effect of introducing sight line rate measurement (position)

LBRE of velocity (m/sec)

1.2

R22=0.0001 R22=0.001 R22=0.01 SLA mesure only

1 0.8 0.6 0.4 0.2 0 0

5

10 Time(sec)

15

20

Fig-8.12. Effect of introducing sight line rate measurement (velocity)

8.3.7 Influence of Eclipsing Effect The eclipsing effect [Zrnic 2001] is studied with the LOS-only measurement scheme (Measurement Model III in section 3.6.4). In pulse radars, the echo from the target can be completely or partially eclipsed, depending on the relative moment of its return (relative to the transmission pulse). The eclipse is defined as a situation when the echo has returned, but the receiver is off, since the transmitter is on. The SNR for a high PRF MMW seeker is shown qualitatively in fig 3.9. Note that as the range decreases, the SNR improves as shown by the envelope of the curve. The eclipsing is characterised (approximately) by an absolute sine wave modulated by the SNR envelope. The corresponding error covariance matrix in logarithmic scale is shown in fig 3.10 in chapter 3.6.4.

154

Chapter 8

The effect of eclipsing, as contrasted to the same for the SNR envelope on the LBRE of the position and velocity is shown in fig 8.13 and 8.14. It may be noted that in this example, the effect of eclipsing is not as severe as one would apprehend. 30

with eclipsing without eclipsing

LBRE of position

25 20 15 10 5 0 0

5

10 Time (sec)

15

20

Fig. 8.13 Effect of Changing SNR on LBRE of Position

1.2 LBRE of velocity (m/s)

with eclipsing

1

without eclipsing

0.8 0.6 0.4 0.2 0 0

5

10 Time

15

20

Fig.8.14 Effect of Changing SNR on LBRE of velocity

8.4

Preliminary Tracking System Design Using CRLB

The case study presented above paves the way to discuss how such analysis can be useful in preliminary design of a tracking system. Though some points have been discussed to a limited extent in the previous sections, we consolidate the main issues here. We have noted that selection of nonlinear filter can be finalized only after substantial experimentation. Selection of hardware components cannot wait till such experiments are complete. This is because software can be changed as the project proceeds but the hardware

155

Chapter 8

issues need to be frozen earlier. CRLB allows a quick analysis of the parametric sensitivity of filter performance in the manner shown in the foregoing study and helps to freeze the important hardware issues. For example, before going to the detailed design of the tracking system the designer needs to know whether the target tracking with a specified accuracy is at all possible or not with the present instrumentation and sensor system. If CRLB result indicates infeasibility, then either the specifications of the filter performance must be relaxed (with consequence in guidance and control performance) or better instruments (lower measurement error covariance) and/or higher sampling frequency (and consequent costlier on board computer) must be chosen. Costs are involved in better instruments and computer. A cost-performance trade-off can then be carried out. Only if the CRLB results indicate that the desired accuracy is possible (with sufficient margin for specific filter efficiency) then only the designer should proceed with trying different nonlinear filtering techniques and compare the results among them. The studies in the previous section also exemplify how the impact of specific sensors can be assessed beforehand by the CRB analysis. The case in point is the eclipsing effect which occurs with milimetric wave based seeker with high pulse repeat rate. The study similar to above could immediately dispel the doubt whether such eclipsing would substantially reduce the filter performance.

8.5

Discussion and Conclusion

The role of CRLB for analyzing tracking system specification and preliminary system design has been presented. The analysis indicated how a quantitative understanding of the influence of design parameters on the tracking filter performance could be obtained through the CRLB. The potential of using Cramer Rao bound (CRB) as a tool for selecting sensors and sampling frequency has thereby been established. A co-worker [Srinivasan 2007] has subsequently extended this to aid guidance system configuration design. In order that the CRLB tool can be effectively used, alternative expressions are provided to calculate CRLB for (singular and) non singular process noise covariance. The concepts have been demonstrated by the fairly well known bearing only target tracking (BOT) problem. Specifically, the effects of (i) sampling time (ii) measurement error covariance (iii) process excitation covariance (iv) initial error covariance (v) additional measurement have been clearly brought out. For a typical case of high PRF, MMW seeker, it has been demonstrated that the adverse effects of eclipsing is limited. Also explored are the effects of using more

156

Chapter 8

measurements on tracking performance. All these have been achieved without going into the specifics of filter design and without elaborate Monte Carlo performance analysis for evaluating such filters. Such analysis brought out that fact that large estimation errors may occur from limitations of tracking instruments and signal processing limitation (and also due to target-observer relative kinematics) even if the theoretically best filters were available. Before trying to put substantial effort in redesigning the filter or applying novel filtering techniques, a CRB analysis would provide valuable insight. It may be noted that the trends discovered through the CRLB analysis were of the same nature that obtained from EKF and UKF though such results had not been presented here. A caveat is in order. Filter parameters like filter initialisation scheme, choice of the process noise covariance and initial error covariance matrix are often heuristically changed (keeping the truth model as it is) to obtain performance improvement of tracking filters. The CRB results should not be used as a guide blindly to obtain a cue whether such effort is justified and whether further improvement in the filtering performance is possible or not. This is because of the fact that CRB uses identical models for truth and the minimum variance estimator. Another issue called “super efficiency” needs to be mentioned here which needs further investigation and has been included in the suggested future work section. In some of the simulations runs presented herein, the RMS errors of specific filters were found to be smaller than that predicted by the CRLB at some sample times. In the parameter estimation scenario, if a specific estimator gives a performance equaling that of CRLB, the estimator is called an efficient estimator. The nomenclature has now been extended to state estimation of dynamic systems as well. Wherever, a specific filter outperforms CRLB, reason of such super-efficiency [Stoica 1996] needs to be resolved by examining whether identical assumptions had been made. It may be noted in the provided simulation results, that though there is a mild tendency of superefficiency for EKF and PF, on the whole, CRLB results are better than individual filter results. While this issue needs to be addressed as a future work, this hardly invalidates the usefulness of CRLB for design studies. Nor this trend of super efficiency unique to the study presented here. The phenomenon of super efficiency is also noticeable in results obtained by [Ristic 2003a], though it has not been explicitly discussed therein. The reasons (bias variance trade-off) for super efficiency suggested by [Bergman 1999 b, page 70] do not apply in this case as is evident from the PF result, which is generally unbiased.

157

9 9.1

Risk Sensitive Filter For INS Self Alignment

Introduction

Having developed a number of methods for nonlinear risk sensitive estimation, in this chapter we explore the applicability of such filters for alignment of strap down inertial navigation system (SDINS). The literature survey done in chapter 2 points out that Inertial Navigation had been a fertile application area for estimation theory especially for nonlinear estimation techniques. The topic, therefore, has drawn significant amount of interest from the researchers [Barth 1999, Moon 1999, Basil 2005]. In Navigation, estimation techniques are primarily employed in Alignment, Calibration and Fusion. Among the three types of problems, alignment problem has been considered in this thesis for subject of study and augmentation. In the context of Strap down INS, it may be recalled that alignment means finding the instrument axis system orientation with reference to a known coordinate axis system and is typically done in two phases, namely, (i) Coarse alignment and (ii) Fine alignment. In coarse alignment the INS axis (the analytic axis) is roughly and rather rapidly aligned [Ali 2004, Feng 1996] with respect to the reference axis with residue misalignment errors, which are less typically than 3 degree that is 0.05 radian. After the coarse alignment is done, small angle approximations ( cos θ = 1, sin θ = θ ) may be employed for the fine alignment and the resultant equations are often linear. This allowed application of linearized filters like EKF or even KF to be successfully employed for the fine alignment. In this chapter we would consider both small initial error (linear problem) and large initial error (nonlinear) problem. In this chapter, we would restrict our attention to self-alignment where the SDINS is aligned from the gravity vector and the earth rotation vector. Though coarse self-alignment is possible in the presence of zero mean small oscillation of the platform, we would consider when the platform is essentially stationary, as required for fine alignment. For self-alignment, it is customary to consider only the (horizontal) velocity outputs from the INS. As the INS is stationary, the ideal output would be zero. The vertical velocity component is seldom used firstly because it does not effectively contribute towards alignment and secondly as the built in navigation algorithm might sometimes exhibit slow instability. It is well known that observability in self-alignment process using only velocity measurements and stationary base (or with unmeasured platform motion) is poor as discussed in [Chuanbin 2004]. This primarily affects the settling of azimuth error in the case of instrument biases. Unless

Chapter 9

special measures are taken (for example using filtered gyro outputs as additional measurement [Chuanbin 2004]) fine alignment is virtually ruled out. However, from [Chuabin 2004] it appears that coarse alignment is still possible with residual errors through a long error settling time. We examine this and demonstrate that this assumption is invalid and the lack of observability can not be cured with out augmenting the measurement. The initial results on risk sensitive filtering by the present worker in this thesis and in [Bhaumik 2005a] indicated that risk sensitive filters (RSF) are good at estimating despite the presence of unmodelled biases. It is therefore considered interesting to explore whether the RSF can provide faster convergence and lower residual error in the presence of instrument biases. Probably this is the first work where nonlinear risk sensitive filtering has been applied to a navigation problem. The problem has been formulated without the assumption of small misalignment angle which results in nonlinear equation. Under small angle assumption the problem becomes linear one. Initially biases are considered as a part of state variable as discussed in [Chuanbin 2004]. Different aspects of that scenario including observability, bias sensitivity and condition for obtaining good performance have been discussed. Also best achievable performance has been determined by considering biases as known inputs. Sensitivity analysis has been carried out in presence of different unmodelled bias. To illustrate the benefits of additional measurements, Gyro outputs have been considered as additional measurements and study for estimation accuracy and bias sensitivity have been carried out. Risk sensitive estimation has been done for system with only velocity measurement and also for system with additional gyro measurements in unmodelled bias situation. Later nonlinear risk sensitive filters such as extended risk sensitive filter (ERSF), risk sensitive unscented Kalman filter (RSUKF) and risk neutral EKF have been applied to the nonlinear problem. The results obtained from these filters are compared.

9.2

Problem Formulation

Here a self-alignment problem has been formulated where the misalignment angles between the INS frame and computer frame is to be estimated. As the misalignment does not vary during the alignment process, this could have been considered as a parameter estimation problem. However, as is customary in KF literature, the problem is reformulated as a state estimation problem by considering the unknown misalignment as additional state variables. In western literature the North-East-Down (NED) coordinate system is more commonly used [Kelly 1994], whereas Chinese and Russian literature tend to use the ENU convention [Gao 2007], [Kong 2000]. Here East north up (ENU) co ordinate is chosen as navigational co ordinate

159

Chapter 9

system and general nonlinear form of the INS equations in the ENU frame are derived. For small angle assumption it has been shown that the expressions match with the expressions provided in [Chuanbin 2004]. The motivation behind the choice of ENU co ordinate system is to compare Chuanbin’s result in linear case. Formulation of nonlinear ENU model became necessary as the publication referred by Chuanbin is not generally available in public domain. 9.2.1

Velocity Error Equation

The velocity error equation resolved in INS frame or computer frame can be written as [Kong 2000]: c δv& = (Ccp − I ) f c − (2ωiEc + ω Ec ) × δv + ∇

(9.1)

Where p denotes Navigational co ordinate system and c denotes Computer (or INS) co ordinate system. δv is velocity error vector expressed in INS frame. So δv = [δve δvn δvu ] . f c is T

specific force vector comprising gravity plus vehicle acceleration in INS co ordinate system. ωiEc c is the angular velocity of earth vector resolved in the INS frame. The quantity ω Ec is the angular

velocity of INS co ordinate with respect to earth fixed frame. The rate vector of INS with respect c to earth fixed frame ( ω Ec ) arises due to motion of the vehicle. If we assume the vehicle is

stationary then this term can be assumed as zero. From now on for the rest of the discussion we assume the vehicle is stationary. ∇ is accelerometer bias vector expressed in INS (navigation) co ordinate frame. So ∇ = [∇ e ∇ n ∇ u ] . If it is assumed that the acceleration bias is in body frame T

then appropriate transformation need to be applied and after that they can be used in equation (9.1). Ccp is co ordinate transformation matrix from INS frame to navigational frame. It has been mentioned earlier that in this study east north up [ENU] convention of co ordinate frame has been chosen. So in east north up co ordinate system the ωiEc

can be written as

ωiEc = [0 ωiE cos(ϕ ) ωiE sin(ϕ )]T , where ϕ is the latitude of the vehicle. Let us assume the

misalignment angle in east, north and azimuth (up) direction is φe , φ n and φu . Then the transformation matrix Ccp can be written according to [Kong 2000] as cn cu − sn se su Ccp = − ce su sn cu + cn se su

cn su + sn se cu ce cu sn su − cn se cu

− s n ce se cn ce

Where c e = cos(φ e ), c n = cos(φ n ), c u = cos(φ u ) s e = sin(φ e ), s n = sin(φ n ), s u = sin(φu ) . From equation (9.1) we obtain

160

Chapter 9

δv&e δve 0 c p c δv&n = (Cc − I ) f − 2ω iE cos(ϕ ) × δv n + ∇ c δv&u 2ω sin(ϕ ) δvu iE δv&e − 2ωiEc sin(ϕ ) 2ω iEc cos(ϕ ) δve ∇ e 0 p c c 0 0 δv& n = (C c − I ) f − 2ω iE sin(ϕ ) δv n + ∇ n c δv&u − 2ω iE cos(ϕ ) δvu ∇ u 0 0

or,

Now we consider only north and east velocity error because with vertical velocity component navigation algorithm might sometimes exhibits slow instability. As the vertical velocity is not considered the acceleration bias in vertical component will not come into picture. δv&e 0 − 2ωiE sin(ϕ ) δve ∇ e p c = (C c − I ) f − + & δ v 2 ω sin( ϕ ) 0 n iE δvn ∇ n

or

δv&e = (cn cu − s n se su − 1) f e + (cn su + s n se cu ) f n − s n ce f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.2)

δv&n = −ce su f e + (ce cu − 1) f n + se f u − 2ωiE sin(ϕ )δve + ∇ n

(9.3)

Small Misalignment Angle: If it is assumed that coarse alignment had been completed before, initial misalignment angle between the axes can be taken as small enough. In such scenario φ e , φ n and φ u can be considered as very small. Under this assumption we can use small angle

approximation rule ( cos θ ≈ 1, sin θ ≈ θ ). So in this situation equations (9.2) and (9.3) will be δv&e = φu f n − φn f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.4)

δv&n = −φu f e + φe f u − 2ωiE sin(ϕ )δve + ∇ n

(9.5)

If we consider vehicle itself does not experience any acceleration other than gravity. So the specific force vector f = [0 0 g ]T . Here the gravity vector in up direction is taken as positive because the sensor measure specific force needs to overcome gravity [Titterton 1997 pp 283]. So equations (9.4) and (9.5) become δv&e = −φn g + 2ωiE sin(ϕ )δvn + ∇ e

(9.6)

δv&n = φe g − 2ωiE sin(ϕ )δve + ∇ n

(9.7)

It can be noted here the equation (9.6) and (9.7) is same as described in [Chuanbin 2004].

9.2.2

Attitude Error Equation

Attitude error equation for large angle is c c φ& = ( I − Ccp )(ωiEc + ω Ec ) + δω Ec +ε

(9.8)

161

Chapter 9

Where Ccp is the DCM for transformation from INS frame to navigation frame. T c δω Ec = [− δvn / R δve / R δve tan(ϕ ) / R ] ; where R is the radius of the earth. ε is the gyro drift

vector ( ε = [ε e ε n ε u ]T ). So equation (9.8) can be written as φ&e 1 − cn cu + sn se su − (cn su + sn se cu ) s n ce 0 − δvn / R ε e & ω cos(ϕ ) + δv / R + ε = c s 1 − c c − s φ e u e u e e n iE n φ& − ( s c + c s s ) − ( s s − c s c ) (1 − c c ) ω sin(ϕ ) δv tan(ϕ ) / R ε n u n e u n u n e u n e iE e u u

or φ&e = −(cn su + sn se cu )ωiE cos(ϕ ) + s n ceωiE sin(ϕ ) − δvn / R + ε e

(9.9)

φ&n = (1 − ce cu )ωiE cos(ϕ ) − seωiE sin(ϕ ) + δve / R + ε n u

(9.10)

φ&u = −( sn su− cn se c u )ωiE cos(ϕ ) + (1 − cn ce )ωiE sin(ϕ ) + δve tan(ϕ ) / R + ε

(9.11)

Small Misalignment Angle: If it is assumed that coarse alignment had been completed before, initial misalignments between the axes can be taken as small enough. In such scenario φ e , φ n and φ u can be considered as very small. Under this condition the equations (9.9-9.11)

become φ&e = −φuωiE cos(ϕ ) + φnωiE sin(ϕ ) − δvn / R + ε e

(9.12)

φ&n = −φeωiE sin(ϕ ) + δve / R + ε n

(9.13)

φ&u = φeωiE cos(ϕ ) + δve tan(ϕ ) / R + ε u

(9.14)

It should be noted that the equations (9.12-9.14) are exactly same as the equations described in [Chuanbin 2004].

9.3

Estimation of Misalignment Angle Considering Biases in State Variable

In this section a self alignment problem has been formulated where the velocity errors, misalignment angles, accelerometer and gyro biases have been considered as state variable. Small angle assumption has been taken as a result the process equation becomes linear one. It has been verified that the process equation exactly matches with that of described in [Chuanbin 2004]. As the system is unobservable, decrease in error with time as shown in [Chuanbin 2004] seems to be illusive. Again the Chuanbin’s paper did not provide the Monte Carlo Simulation results. Only estimation error obtained from typical run had been shown. From earlier chapters we have seen that error for single instance is not sufficient to assess the performance of any filter. Because in single instance filter may track the truth but RMSE may diverge due to track loss cases. So our primary objective behind this study was to get more insight about self alignment and assess the filter’s performance in more precise way through Monte Carlo simulation.

162

Chapter 9

Let us consider the state vector consists of east and north velocity error, three misalignment angles and accelerometer and gyro biases. So X = [δv e δv n φ e φ n φ u ∇ e ∇ n ε e ε n ε u ]T . Combining equations (9.6), (9.7) and (9.12 - 9.14) we can write process dynamics as 0 2ωiE sin(ϕ ) 0 0 −g δv&e δv& − 2ω sin(ϕ ) 0 0 0 g iE n & φe ωiE sin(ϕ ) − ωiE cos(ϕ ) 0 0 −1/ R & 1/ R 0 0 0 − ωiE sin(ϕ ) φn φ& tan(ϕ ) / R ω ϕ 0 cos( ) 0 0 iE &u = 0 0 0 0 0 ∇e & ∇ 0 0 0 0 0 n & ε 0 0 0 0 0 e ε& 0 0 0 0 0 n ε&u 0 0 0 0 0

1 0 0 0 0 δve 0 1 0 0 0 δvn 0 0 1 0 0 φ e 0 0 0 1 0 φ n 0 0 0 0 1 φu + w 0 0 0 0 0 ∇ e 0 0 0 0 0 ∇ n 0 0 0 0 0 ε e 0 0 0 0 0 ε n 0 0 0 0 0 ε u

Where w is process noise assumed to be zero mean Gaussian with covariance Q ( w ~ N (0, Q) ). The measurement equations are written as Z = HX + v

(9.15) 1 0 0 0 0 0 0 0 0 0 and v is measurement noise assumed to be zero mean 0 1 0 0 0 0 0 0 0 0

Where H =

Gaussian with covariance η i.e. ( v ~ N (0,η ) ). An observability matrix of the system has been formulated. It is seen that the rank of the observability matrix is seven. But the system is 10th order system. So three states of the system are unobservable. The system is linear Gaussian so the states are estimated using Kalman filter. The parameters for simulation are chosen as described in [Chuanbin 2004].

•

The truth model misalignment angles are initialized as φ e = φ n = φ u = 10 .

•

Initial acceleration biases for truth are chosen as ∇ e = ∇ n = 100µg

•

Initial gyro constant drift rates for truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Initial error velocities of truth model are chosen as 0.01 m/s.

•

Sampling time 1 sec

•

Initial state values of the estimator are chosen as zero.

•

Latitude of the vehicle is chosen as 45 degree.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

Angular velocity of earth ω iE = 7.27220e-5.

163

(0.005 0 h −1 ) 2

0 0 0 0 0

]

Chapter 9

•

Radius of the earth R=6378137.0 m

•

Acceleration due to gravity = 9.81 m/s2

•

Initial error covariance is chosen as

[

P0 = diag ( 0.012

0.012 10 10 10 100 µg 100 µg

0.010 h −1

0.010 h −1

]

0.010 h −1 )

From this point till rest of the chapter the values of sampling time, latitude, angular velocity, radius of the earth and acceleration due to gravity will remain same for each simulation.

Simulation Results The Kalman filter has been realized in discrete domain. But the process equation described above is in continuous domain. So discretization of the process has been done with one second sampling time interval. The errors in estimation of east, north and azimuth misalignment angles obtained from KF have been calculated and the result matches fairly with that of the figure shown in [Chuanbin 2004], hence is not shown in this section. Root mean square errors (RMSE) of three misalignment angles have been plotted in figure 9.19.3 for 100 Monte Carlo run. It has been seen from the figure that all the RMS errors of misalignment angles are converging well. Although convergence rate of east and north misalignment angle estimation is very high and almost instantaneous (with in 8 second RMSEs reach 0.0058 degree which are very near to their respective settling point), RMS error in azimuth misalignment angle converges slowly (in 50 second it reaches to 0.1568 degree and it settles on 0.0656 degree). Chuanbin [Chuanbin 2004] in his paper did not provide RMSE for misalignment angles. Figure 9.1 - 9.3 may be referred for completeness of their analysis. One point needs to be mentioned here that, linear velocities are the measurements with which the self-alignment is carried out. So following convention, the results for velocity error estimation are not presented throughout this chapter. The system described above is unobservable and in spite of that all the RMS errors are settling well. This is quite surprising. Being curious about the unrealistically high efficiency of the estimator, RMS errors for different values of initial gyro and accelerometer biases are calculated. The RMSE for different initial accelerometer and gyro biases have not been shown here as the nature of the figures are more or less same as the figures 9.10 – 9.13. The RMS error settling value of different misalignment angles for different gyro and accelerometer biases have been summarized in table 9.1. It has been seen from the table that as the initial values of accelerometer and gyro biases increase the error settling values also increase. So it is found out through the study that, the good performance shown by Chuanbin, in spite of unobservibity, is due to very small choice of initial accelerometer and gyro biases.

164

Chapter 9

RMSE plot of east misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Step

Fig 9.1. RMSE for east misalignment estimation considering biases as state variable RMSE plot of north misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Step

Fig 9.2. RMSE for north misalignment estimation considering biases as state variable RMSE plot of azimuth misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

step

Fig 9.3. RMSE for azimuth misalignment estimation considering biases as state variable

165

Chapter 9

Table 9.1 RMS error settling values for different unmodelled gyro and accelerometer bias

Values of unmodelled bias

RMSE error settling values (deg)

Gyro biases Accelerometer biases East (deg/h) (micro g)

9.4

North

Azimuth

0.01

0.005

0.0058

0.064

0.05 0.1 0.15 0.2

100

0.0056 0.0056 0.0057 0.0057

0.0056 0.0058 0.0058 0.0058

0.3279 0.6538 0.8976 1.0518

0.01

100 1000 2000 3000 40000

0.005 0.0056 0.027 0.0377 0.0433

0.0058 0.0633 0.1365 0.2165 0.2998

0.064 0.1211 0.1943 0.274 0.3571

Estimation of Misalignment Angles Considering Biases as Known Input

From the above discussion it might be realised that it is worthy to know about the estimator’s performance in presence of accurately known gyro and accelerometer biases. Because that performance can be considered as the achievable lower bound and hence treated as benchmark. So towards this goal the system has been modified as fifth order system where the state variables are velocity errors and misalignment angles. So

X = [δve

δvn φe φn φu ]T . Gyro and

accelerometer biases are considered as known inputs. Again we consider small angle assumption so that system remains linear. Process dynamics of the system can be written as −g 0 2ωiE sin(ϕ ) 0 0 δv&e δve ∇ e δv& − 2ω sin(ϕ ) δv ∇ 0 g 0 0 iE n n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + ε e + w − 1/ R 0 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn ε n φ&u tan(ϕ ) / R φu ε u ωiE cos(ϕ ) 0 0 0

(9.16)

Where w is process noise assumed to be zero mean Gaussian with covariance Q ( w ~ N (0, Q) ). The measurement equation is taken as discussed in equation 9.15 in section 9.3.

Simulation parameters •

The truth model misalignment angles are initialized as φ e = φ n = φ u = 10 .

•

Initial error velocities of truth model are chosen as 0.01 m/s.

•

Gyro constant drift rate for both filter as well as truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Acceleration biases are chosen as ∇ e = ∇ n = 100µg

166

Chapter 9

•

Initial state values of the estimator are chosen as zero.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

]

Initial error covariance is chosen as P0 = diag ([0.012 0.012 10 10 10 ])

The errors in estimation of east, north and azimuth misalignment angles obtained from KF have been plotted in figure 9.4 - 9.6 respectively. From the figures it can be realized that the convergence rate of east and north misalignment angle estimation is very good and almost instantaneous where as the estimation in azimuth misalignment angle converges slowly. The root mean square errors (RMSE) of east, north and azimuth misalignment angle out of 100 MC run have been calculated and shown in figure 9.7- 9.9 respectively. Results show RMSE of all the misalignment angles are converging well and settling on almost zero degree although the convergence rate of azimuth RMSE is significantly slower that other two misalignment angles. For example at 50 second the RMSE of east and north misalignment angle reach at 0.001 degree where as the RMSE of azimuth reaches at 0.1 degree. So these RMS error profile may be considered as the benchmark if self alignment is carried out using velocity error measurements. Estimation error of east misalignment angle

Estimation Error (deg)

1.2 1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.4 Estimation error of east misalignment angle for a typical run

167

Chapter 9

Estimation error of north misalignment angle

Estimation Error (deg)

1.2 1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.5 Estimation error of north misalignment angle for a typical run Estimation error of azimuth misalignment angle

Estimated Error (deg)

2 1.6 1.2 0.8 0.4 0 0

50

100

150

200

250

300

-0.4 Time (sec)

Fig 9.6 Estimation error of azimuth misalignment angle for a typical run RMSE plot of east misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.7 RMSE of east misalignment angle for 100 MC run

168

Chapter 9

RMSE plot of north misalignment angle 1.2

RMSE (deg)

1 0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

250

300

Time (sec)

Fig 9.8 RMSE of north misalignment angle for 100 MC run RMSE of azimuth misalignment angle 1.2

RMSE(deg)

1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig 9.9 RMSE of azimuth misalignment angle for 100 MC run

9.5

Effect of Unmodelled Bias

The presence of accelerometer and gyro biases make the self alignment most difficult. If they have been considered as state variables the system becomes unobservable and the estimation accuracy degrades. So it may be worthy to see how worst will be the estimation if filter model consider the gyros and accelerometers as bias free but in truth they have biases. So this situation may be considered as the worst case situation for self alignment problem. Truth process model is described as

169

Chapter 9

0 2ωiE sin(ϕ ) 0 −g 0 δv&e δve ∇ e δv& − 2ω sin(ϕ ) δv ∇ 0 g 0 0 iE n n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + ε e + w 0 − 1/ R 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn ε n & φu tan(ϕ ) / R φu ε u ωiE cos(ϕ ) 0 0 0

(9.17)

Where as the process model for filter is 0 2ωiE sin(ϕ ) 0 −g 0 δv&e δve δv& − 2ω sin(ϕ ) δv 0 g 0 0 iE n n φ&e = ωiE sin(ϕ ) − ωiE cos(ϕ ) φe + w 0 − 1/ R 0 & 1/ R 0 − ωiE sin(ϕ ) 0 0 φn φn φ&u tan(ϕ ) / R φu ωiE cos(ϕ ) 0 0 0

The measurement model is taken as described in equation 9.15. For this experiment the initial values of truth and initial values of filter, process and measurement noise covariance have been chosen as described in section 9.4. Accelerometer unmodelled biases have been kept constant value of 100 µg . Gyroscope biases have been changed from 0.01 deg/h to 0.2 deg/h in step. It has been observed that for the change of gyro bias in this range there is almost no effect on east and north misalignment estimation. For this reason RMSE plots of east and north misalignment angle have not been shown here. Azimuth misalignment estimation is heavily affected for unmodelled gyro bias. The RMSE of azimuth misalignment angle obtained from 100 MC run in presence of unmodelled biases has been shown in figure 9.10. From the figure it has been seen that as the value of unmodelled gyro biases increase the RMSE settling value for azimuth misalignment angle also increases. RMSE for different unmodeled gyro biases gyrobias=0.2deg/h gyrobias=0.15deg/h gyrobias=0.1deg/h gyrobias=0.05deg/h gyrobias=0.01deg/h

1.4

RMSE (Deg)

1.2 1 0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig. 9.10 RMSE of azimuth misalignment angle for different unmodelled gyro biases

170

Chapter 9

Similar experiment has been carried out to know the effect of unmodelled accelerometer biases. For that gyro biases are kept at constant value of 0.01 degree/h where as unmodelled accelerometer biases have been varied from 100 µg to 5000 µg . Interestingly unmodelled accelerometer bias has the effect on the estimation of each misalignment angle. Figure 9.11 – 9.13 show the effect of unmodelled acceleration biases on RMSE of east, north and up mis alignment angle. For each case RMSE settling values increase with the increase of unmodelled acceleration biases.

RMSE of east misalignment angle for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

-0.2 Time (sec)

Fig 9.11 RMSE of east misalignment angle for different unmodelled accel bias RMSE of North misalignment for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

-0.2 Time (sec)

Fig 9.12 RMSE of north misalignment angle for different unmodelled accel bias

171

Chapter 9

RMSE of Azimuth misalignment for different unmodeled accel bias

AccelBias=100microg AccelBias=1000microg AccelBias=2000microg AccelBias=3000microg AccelBias=4000microg

1.2

1

RMSE (deg)

0.8

0.6

0.4

0.2

0 0

50

100

150

200

250

300

Time (sec)

Fig 9.13 RMSE of azimuth misalignment angle for different unmodelled accel bias

9.6

Risk Sensitive Kalman Filter With Unmodelled Bias In earlier chapter it has been shown that risk sensitive filter performs better in presence of

unmodelled bias compare to risk neutral filter. So it seems to be worthy to apply risk sensitive Kalman filter in the situation of unmodelled bias and examine whether the estimation would be better in case of RSKF. Initial state values of truth, process and measurement noise covariance and initial co variance are chosen as described in section 9.4. The values of unmodelled accelerometer and gyro biases have been chosen as 2000 µg and 0.1 degree/h respectively. The value of risk sensitive parameter is taken as 9. The RMSE plots of east, north and up misalignment angle obtained from 100 MC run of KF and RSKF have been shown in figure 9.14, 9.15 and 9.16 respectively. From the figures it is obvious that instead of unmodelled bias no improvement in estimation accuracy by RSKF over KF has been observed.

172

Chapter 9

RMSE of East misalignment angle under unmodeled bias 1.2 KF 1

RSKF

RMSE (deg)

0.8 0.6 0.4 0.2 0 0

50

100

150

200

Time (sec)

250

300

Fig 9.14 RMSE of east misalignment angle under unmodelled bias RMSE of North misalignment angle under unmodeled bias 1.2 KF

1 RMSE (deg)

RSKF

0.8 0.6 0.4 0.2 0 0

50

100

150

200

250

300

Time (sec)

Fig 9.15 RMSE of north misalignment angle under unmodelled biases RMSE of up misalignment angle under unmodeled bias 1.4 KF RSKF

RMSE (deg)

1.2 1 0.8 0.6 0.4 0

50

100

150

200

250

300

Time(sec)

Fig 9.16

RMSE of azimuth misalignment angle under unmodelled biases

173

Chapter 9

9.7

Misalignment Angle Estimation Using Gyro Output as Measurement

Measurement Equation It is worthy to consider the gyro output as measurement of the system along with velocity errors because it will cure the unobservability problem encountered by Chuanbin [Chuanbin 2004]. Also additional measurement will increase the observability of the system with known input bias, which has been described in section 9.4. So considering gyro output the measurement equation becomes Z = HX + v 1 0 0 0 0 0 1 0 0 0 ωiE sin(ϕ ) − ωiE cos(ϕ ) H = 0 −1/ R 0 0 − ωiE sin(ϕ ) 0 0 1/ R tan(ϕ ) / R 0 cos( ) 0 0 ω ϕ iE

v is measurement noise assumed to be zero mean white Gaussian. Strictly speaking we cannot use gyro outputs as measurements directly. The reason behind this is, gyro output equations are described in navigational frame where as measurements are in instrumental frame. But here we make approximation that the gyro output in navigational frame is same as that of in instrumental frame. The error arises due to this approximation is taken care of by large measurement noise covariance.

Biases Considered as Known Input If biases are considered as known input the process dynamics of the system can be represented using equation (9.16). The simulation parameters except measurement noise covariance are taken as described in section 9.4. The covariance of measurement noise is taken as

[

R = diag (1e − 4) 2

(1e − 4) 2

]

2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h . RMSEs for

all the three misalignment angles have been calculated in this scenario and the values are compared with respect to velocity error only measurement case. The RMSE for east and north misalignment are same in nature as shown in figures 9.7 & 9.8 except the fact that errors settle at little lower value compare to velocity error only measurement case, hence is not shown here. For example after 200 second, for velocity error only measurement scenario, the RMSE of east and north reach at 0.0051deg and 0.0058 deg respectively. If additional measurements are incorporated the east and north RMSE misalignment angles settle to 4.2048e-005 and 4.0229e005 degree respectively. For azimuth misalignment angle with five measurements significant amount of improvement has been achieved in terms of convergence. With these additional measurements the rate of convergence of azimuth misalignment estimation become comparable

174

Chapter 9

with that of east and north. The RMS errors of azimuth misalignment angle for both cases have been shown in fig 9.17. RMSE of azimuth misalignment angle for system with additional gyro output measurement 1.2

Velocity error only meas with additional gyro meas

1

RMSE (deg)

0.8 0.6 0.4 0.2 0 -0.2

0

50

100

150

200

Time

Fig 9.17 RMSE for azimuth misalignment for velocity error only measurement case and additional gyro output measurement case with known input bias

Estimation With Unmodelled Bias Now experimentation has been carried out to investigate whether increase of observability can minimise the effect of unmodelled bias. For this experiment the gyro and accelerometer bias have been considered in truth but not in filter equations. So process dynamics is represented by equation (9.17). Here the simulation has been carried out using two different measurement error covariances ( R) to know the effect of. Initially the measurement error covariance is taken as

[

R = diag (1e − 4) 2

(1e − 4) 2

]

2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h 2 * (0.005) 2 deg/ h .

Accelerometer biases are kept constant at 100 microg and gyro biases have been varied from 0.01 deg/h to 0.2 deg/h. The effect of different unmodelled bias is negligible in east and north misalignment estimation. For this reason RMSE results of east and north misalignment estimation for different unmodelled biases have not been presented. The RMSE of azimuth misalignment estimation for different unmodeled gyro biases has been presented in fig 9.18. It is observed from the figure that as the value of unmodelled gyro bias increases the settling value of RMSE is also increase. For example, after 200 second when gyro bias 0.01 deg/h RMSE reaches at 0.0082 deg and for gyro biases 0.2 deg/h RMSE reaches at 0.0481. For in between unmodelled gyro biases RMSE remains in between the above two values. In high value of gyro bias (0.2deg/h) the RMSE shows slightly diverging nature. The reason for that may be more severe effect of biases with the increase of time.

175

Chapter 9

RMSE of Azimuth with Unmodelled bias 1.2

gyrobias=0.01 gyrobias=0.05

1

gyrobias=0.1 gyrobias=0.15

0.8 RMSE

gyrobias=0.2 0.6 0.4 0.2 0 0

50

100

150

200

-0.2 Time (sec)

Fig 9.18 RMSE of azimuth estimation for different unmodelled gyro biases.

Similar simulation has been carried out for higher value of measurement error covariance ( R = diag [(1e − 4) 2 (1e − 4) 2 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h] ) to realize the effect of high measurement noise covariance. The RMSE obtained for this case is very similar with that obtained for small R (see fig 9.19). The only difference is that the RMSE becomes little higher for higher value of R. For example for lower value of R, for unmodelled gyro bias 0.01, the RMSE after 200 second become 0.00481 degree, where as for higher value of R and for same gyro bias after 200 second it is 0.1002 degree. So from this study it may be concluded that the effect of unmodelled biases can be reduced by increasing observability. RMSE of Azimuth with large gyro measurement noise 1.2 gyrobias=0.01 1

gyrobias=0.05 gyrobias=0.1

RMSE

0.8

gyrobias=0.15 gyrobias=0.2

0.6 0.4 0.2 0 0 -0.2

50

100

150

200

Time

Fig 9.19 RMSE of azimuth estimation for different unmodelled gyro biases for higher value of R.

176

Chapter 9

Risk Sensitive Estimation: In this problem RSKF and KF have been simulated to test whether any improvement can be achieved using RSKF under unmodelled accelerometer and gyro bias condition. In this simulation initial values of state, process noise covariance and initial error co variance are chosen as described

in

[

R = diag (1e − 4) 2

section

9.4

The

measurement

noise

covariance

is

taken

as

(1e − 4) 2

5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h 5 * (0.005) 2 deg/ h . The values of

]

unmodelled accelerometer and gyro biases have been chosen as 100 µg and 0.2 degree/h respectively. The value of risk sensitive parameter is chosen as 3275. RMSE obtained from RSKF and KF is plotted in fig 9.20. From the figure it is obvious that instead of unmodelled bias situation and good observability no improvement in estimation accuracy by RSKF over KF has been observed. RMSE for RSKF & KF

1.2

RSKF 1

KF

RMSE

0.8 0.6 0.4 0.2 0 0

50

100

150

200

-0.2 Time

Fig 9.20 KF and RSKF for gyro output measurement situation

9.8

State Space Model For Large Misalignment Angle

For large misalignment angle the state space equation is the combination of (9.2), (9.3) and (9.9 – 9.11) δv&e = (cn cu − s n se su − 1) f e + (cn su + s n se cu ) f n − s n ce f u + 2ωiE sin(ϕ )δvn + ∇ e

(9.18)

δv&n = −ce su f e + (ce cu − 1) f n + se f u − 2ωiE sin(ϕ )δve + ∇ n

(9.19)

φ&e = −(cn su + sn se cu )ωiE cos(ϕ ) + s n ceωiE sin(ϕ ) − δvn / R + ε e

(9.20)

φ&n = (1 − ce cu )ωiE cos(ϕ ) − seωiE sin(ϕ ) + δve / R + ε n

(9.21)

φ&u = −( sn su− cn se c u )ωiE cos(ϕ ) + (1 − cn ce )ωiE sin(ϕ ) + δve tan(ϕ ) / R + ε u

(9.22)

177

Chapter 9

In this formulation state vector is consists of velocity errors, and misalignment angles ie, X = [δve

δvn φe φn φu ]T . Bias vector consists of accelerometer biases in two channel and gyro

biases in three channels. So for nonlinear case the process equations (9.18 -9.22) can be written as X& = f ( X ) + biasvector + w

(9.23)

We use two velocity errors as measurement as described in equation (9.15).

9.8.1

Alignment Considering Biases as Known Input

In this case bias is considered as known input hence the effect of biases is minimized. For nonlinear case the process model can be described with the equation (9.23). The measurement equation is considered same as in linear case described by the equation (9.15). For simulation the parameters are chosen as follows:

•

The truth misalignment angles are initialized as φ e = φ n = φ u = 10 0 .

•

Initial error velocities for truth model are chosen as 0.01 m/s.

•

Gyro constant drift rate for both filter as well as truth are chosen as, ε e = ε n = ε u = 0.010 h −1 .

•

Acceleration biases are chosen as ∇ e = ∇ n = 100µg

•

Initial state values for the estimator are chosen as zero.

•

Latitude of the vehicle is chosen as 45 degree.

•

Measurement noise covariance is = diag ([(10 −4 m / s) 2 (10 −4 m / s) 2 ]).

•

Process noise covariance

[

Q = diag ( (10 µg ) 2

•

(10 µg ) 2 (0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

(0.005 0 h −1 ) 2

]

Initial error covariance is chosen as P0 = diag ([0.012 0.012 10 0 10 0 10 0 ])

It should be noted that the process equations described by (9.23) are in continuous domain. During simulation the truth value of the states are generated by numerical integration starting from initial value of the state. Here accelerometer and gyro biases have been considered in process model of both truth as well as filter. Extended Kalman filter (EKF) and unscented Kalman filter (UKF) have been simulated. The RMSE in estimation of east, north and azimuth misalignment angle obtained from 100 MC run have been shown in figures (9.21- 9.23). From the figures it is observed that all the misalignment angles are converging to almost zero value. For east and north misalignment angle the RMS errors of the estimator reach at 0.0001 degree after 200 second. For azimuth misalignment angle the RMSE of estimator reaches at 0.0076 degree after 200 second.

178

Chapter 9

RMSE of East Misalignment Angle 12

EKF

10

UKF RMSE (deg)

8 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.21 RMSE of east misalignment angle RMSE of North Misalignment angle 12

EKF

10

UKF RMSE (deg)

8 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.22 RMSE of north misalignment angle RMSE of Azimuth Misalignment Angle

12

EKF

10

UKF RMSE(deg)

8

6

4

2

0 0

50

100

150

200

250

Time(sec)

Fig 9.23 RMSE of azimuth misalignment angle

179

300

Chapter 9

9.8.2

Alignment in Presence of Unmodelled Bias

In this section experimentation has been done with nonlinear system with unmodelled bias situation. The truth has been considered with accelerometer and gyro biases but the filter process equation does not know that. So the truth-value of the variables has been generated using the equation (9.23) where as the filters are realized using the process equation X& = f ( X ) + w

(9.24)

The simulation parameters are chosen as described in section 9.8.1 except gyro and accelerometer biases. Gyro constant drift rate for truth are chosen as, ε e = ε n = ε u = 0.2 0 h −1 . The filters assume the gyro biases as zero. Truth acceleration biases are chosen as ∇ e = ∇ n = 2000µg . The filters assume zero accelerometer biases. Under this unmodelled bias condition EKF, ERSF and RSUKF have been simulated. For ERSF the value of risk sensitive parameter has been chosen as 8 and for RSUKF it was chosen as 0.0001. RMS errors have been calculated for east, north and azimuth misalignment angles from 100 Monte Carlo runs. RMSE for east north and up misalignment angles for EKF, ERSF and RSUKF are shown in figure 9.24-9.26. No improvement in estimation accuracy has been observed using ERSF and RSUKF in comparison to EKF in presence of unmodelled bias. It can also be observed from figure 9.26 that RMSE is wrongly at a value of 0.98 degree which is quite large. It is confirmed that this occurs due to unmodelled bias not due to nonlinearity. RMSE of east misalignment angle 12 EKF

10

ERSF RMSE (deg)

8

RSUKF

6 4 2 0 0 -2

50

100

150

200

250

300

Time (sec)

Fig 9.24 RMSE of east misalignment angle with unmodelled bias

180

Chapter 9

RMSE of north misalignement angle

12 EKF

10

ERSF

RMSE (deg)

8

RSUKF 6 4 2 0 0

50

100

150

200

250

300

-2 Time (sec)

Fig 9.25 RMSE of north misalignment angle with unmodelled bias RMSE of azimuth misalignment angle

12 EKF

10 RMSE (deg)

ERSF 8

RSUKF

6 4 2 0 0

Fig 9.26

50

100

150 Time(sec)

200

250

300

RMSE of azimuth misalignment angle with unmodelled bias

9.9 Discussion and Conclusion In this chapter a self-alignment problem for stationary base has been formulated independently where the misalignment angles between the INS frame and navigational frame is to be estimated. Considering biases in state variable the problem reduces exactly same as described by Chuanbin [Chuanbin 2004]. After verifying Chuanbin’s results for single run, RMS errors, which were not mentioned by Chuanbin, have been plotted in this chapter. The RMSE, shown here may be referred for completeness of the result presented in [Chuanbin 2004]. In Chuanbin it is wrongly concluded that in spite of unobservability the estimator is capable enough to estimate the misalignment angles. Further study made by present author reveals that the good performance shown by Chuanbin, in spite of unobservibity, is due to very small choice

181

Chapter 9

of initial accelerometer and gyro biases. It has been shown that if higher values of initial accelerometer and gyro biases are taken the estimator’s performance degrades rapidly. Results are provided for the system with exactly known biases for benchmarking the achievable lower limit. Also RMSE results for different misalignment angles are provided for different unmodelled gyro and accelerometer biases. Being motivated by the superiority of RSKF in presence of unmodelled bias, the results obtained from KF and RSKF has been compared. But no benefit has been observed. It has been argued that gyro measurements could be used as additional measurement to overcome the problems arises from unobservability. Again RSKF has been applied with this additional gyro measurements. But no improvement has been reported. With out small angle assumption the developed self-alignment problem is nonlinear. In that case misalignment angles have been estimated using extended Kalman filter (EKF) and unscented Kalman filter (UKF) for large initial condition. From the results it is seen that the RMSE of the misalignment angles are converging at zero value for both the filters. The same problem has been solved in presence of unmodelled gyro and accelerometer biases using extended Kalman filter (EKF), extended risk sensitive filter (ERSF) and risk sensitive unscented Kalman filter (RSUKF). The results obtained from nonlinear risk sensitive estimators are compared with EKF. But no improvement in estimation accuracy has been observed with nonlinear risk sensitive filtering in comparison to nonlinear risk neutral filtering. In this case RMSEs of azimuth misalignment for both the cases settle wrongly at unaccepted large value. It is concluded that this occurs due to unmodelled biases not due to nonlinearity. So in self alignment problem biases would be the main concern not nonlinearity. The additional measurement as considered in linear case should also be considered in nonlinear situation to increases observability. This remains in the scope for future work.

182

10 Discussions and Conclusions

10.1

Discussions

The main motivations behind this present work had been to develop and characterize novel nonlinear estimators. The objective of the thesis had been further narrowed down (see the introduction chapter) so as to obtain deeper insight and understanding about the properties of risk sensitive filtering, developing various advanced nonlinear risk sensitive filtering and investigate the scenario where risk sensitive filters can outperform existing filters. The work presented in this thesis has the following aspects: •

Development of various novel techniques for nonlinear filtering, leading to alternative formulation of risk sensitive filtering.

•

Exploring new properties and possible application areas, including aerospace tracking and navigation problems, of risk sensitive filters, using the algorithms.

•

Exploring the use of Cramer-Rao bound for tracking system preliminary design.

In the rest of this chapter we summarise the main findings of this thesis and also discuss about the scope for future work. Some of the concluding comments made in individual chapters would be repeated for the sake of completeness.

10.2 (i)

Conclusions An alternative formulation for posterior recursive solution for nonlinear risk sensitive estimation was proposed. The proof of the theorem has been derived through an alternative information state based induction method.

(ii)

Recursive equations for risk sensitive Kalman filter are alternatively derived using linear matrix inequality based approach.

(iii)

Robustness properties of risk sensitive estimator have been studied and some unknown properties have been found out.

(iv)

The central difference risk sensitive filter (CDRSF), which has been developed jointly with another scholar M.Srinivasan, is based on the central difference approximation. The risk sensitive unscented Kalman filter (RSUKF) based on the unscented transform, has been developed independently by the author. Both the scaled and unscaled versions of RSUKF have been derived. Both the CDRSF and RSUKF may be used for on board application due to its computational efficiency.

Chapter 10

Superiority of proposed filtering techniques with respect to ERSF has been demonstrated through case study (v)

Two new types of (approximate) risk sensitive filtering algorithms namely (i) central difference risk sensitive filter (CDRSF) and (ii) risk sensitive unscented Kalman filter (RSUKF) have been proposed. Both the scaled and unscaled version of RSUKF have been derived. These two filters may be used for on board application due to its computational efficiency. Superiority of proposed filtering techniques with respect to ERSF has been demonstrated through case study.

(vi)

Grid based method has been utilized to formulate risk sensitive filter for nonlinear non Gaussian cases. The adaptation of grid improves the numerical efficiency substantially. The proposed technique has been named as adaptive grid risk sensitive filtering (AGRSF). The algorithm has been found to be straight forward and simple to understand. Superiority of AGRSF, over existing ERSF has been demonstrated.

(vii)

The accuracy of AGRSF can be improved by choosing larger number of support points.

(viii)

Though the grid adaptation tends to reduce computation time, substantially, the AGRSF was found to be numerically less efficient than risk sensitive particle filter (RSPF) algorithm for the same level of accuracy. Though AGRSF is relatively numerically inefficient, it has been used successfully to validate risk sensitive particle filter (RSPF) algorithm.

(ix)

A novel technique based on particle filtering has been proposed for nonlinear risk sensitive estimation. The proposed method has been named as risk sensitive particle filter (RSPF). Both the prior and posterior versions of the algorithm have been developed. The superiority of RSPF over ERSF has been demonstrated through a highly nonlinear system.

(x)

The role of Cramer Rao lower bound (CRLB) for quantitative understanding of the influence of decision parameters on the tracking filter performance has been demonstrated through an example. The potential of using CRLB as a tool to aid guidance and tracking system configuration design has been established. The effects of different parameters such as (i) sampling time, (ii) measurement error covariance, (iii) process noise covariance, (iv) initial error covariance, (v) additional measurement; on estimation accuracy have been studied for bearing only tracking problem. In that problem the filtering performance in presence of eclipsing effect of

184

Chapter 10

the seeker has also been studied using CRLB. It has been concluded that the eclipsing effect on state estimation of target is not as severe as perceived by tracking community. (xi)

A self alignment problem for aerospace strapdown Inertial navigation system (INS) has been formulated. The problem is in general nonlinear in nature but under small angle assumption it became linear. Both the linear and nonlinear problems have been solved with risk sensitive and risk neutral filtering. For linear problem KF and RSKF have been applied and for nonlinear problem EKF, ERSF and RSUKF have been applied. The comparison has been made between risk neutral filter and risk sensitive filter under unmodelled bias condition. Under this condition no significant improvement with risk sensitive filtering has been found in comparison to risk neutral filter. Different aspects of the problem including observability, bias sensitivity and effect of additional measurement have been discussed. It has been concluded that the effect of gyro and accelerometer biases is severe in comparison to system nonlinearity.

10.3

Suggestion for Further Research

In the course of the present work, several ideas for further work have emerged. A significant subset of such ideas is presented here. (i)

Nonlinear risk sensitive filters need to be applied rigorously in target tracking and navigation problems. It is necessary to find out situations where the risk sensitive filters are the best choice. Reasons why risk sensitive filters are not exhibiting sufficient and substantial advantages need to be explored. Performance comparison with other robust filters, such as extended H ∞ filters may be useful.

(ii)

As discussed in chapter 5 it is necessary to explore the possibility of existence of square root risk sensitive unscented Kalman filter. Also the development of risk sensitive version of Gauss-Hermite filter (GHF) remains under the scope of future work.

(iii)

Risk sensitive particle filter formulated in this thesis used ordinary sequential sampling resampling method and prior proposal density function. Risk sensitive particle filter may also be formulated using different risk sensitive LRKF as proposal. Also the possibility of different other variant of risk sensitive particle filter such as Auxiliary risk sensitive particle filter Rao blackwellised risk sensitive particle filter need to be explored.

185

Chapter 10

(iv)

In the present work only EKF has been used for generating proposal for AGRSF. The use of other risk neutral filters such as UKF, CDF and also use of risk sensitive filters such as ERSF, RSUKF and CDRSF as proposal remains under the scope of future work.

(v)

Cramer Rao bound used in this thesis, are applicable for additive Gaussian noise. Similar relations for non additive and non Gaussian noise would be welcome. Again Cramer Rao lower bound is known to be a weaker bound. Tighter alternatives are available [Reece 2005] for example Bhattacharyya, Bobrovsky-Zakai and WeissWeinstein lower bounds. Recently these lower bounds are being applied for filtering problems. The advantages and applicability of those bounds in comparison to CRLB need to be figured out. Also lower bound for robust filtering would be appreciated.

(vi)

In linear Gaussian system where process and measurement noises are unknown or difficult to assume designers try to estimate the noise covariance at each step using different techniques (among them auto covariance least square technique is most popular), and then estimate the values of state variable using Kalman filter. This method is well known as adaptive Kalman filter. For nonlinear system adaptive unscented Kalman filter [Lee 2005] is available where process and measurement noise covariance have been estimated at each step then UKF is used to estimate state variables. In such scenarios where noise covariance is unknown risk sensitive nonlinear filtering proposed in this thesis may become competitor to nonlinear adaptive filtering methods. Even adaptive risk sensitive filters can also be formulated. Future research is necessary in this direction.

186

Bibliography V.J. Aidala, S.C. Nardone, "Biased Estimation Properties of the Pseudolinear Tracking Filter ", IEEE Transaction on Aerospace and Electronic System Vol AES-18, No.4 July 1982. V.J.Aidala, S.E.Hammel,"Utilization of Modified Polar Coordinates for Bearings[Aidala 1983] Only Tracking"IEEE Transaction on Automatic Control Vol Ac 28, No 3, March 1983. Jamshaid Ali, Fang Jiancheng, "Alignment of strapdown inertial navigation [Ali 2004] system: A literature survey spanned over the last 14 years", School of instrumentation Science and Optoelectronics Engineering, Beijing University of Aeronautics and Astronautics, Beijing 100083, China, 2004. [Anderson 1979] B.D.O.Anderson, J.B.Moore, "Optimal Filtering", Prentice Hall Inc, Englewood cliffs,New Jersey, 1979 [Andrieu 2003] C. Andrieu, M. Davy, and A. Doucet, Efficient Particle Filtering for Jump Markov Systems.Application to Time-Varying Autoregressions", IEEE Transaction on Signal Processing, Vol. 51, No. 7, March 2003. [Andriseu 2001] C.Andriseu, N.D.Feritas, A Doucet, “Rao-Blackwellised Particle Filtering Via Data Augmentation",Advances in Neural Information Processing Systems (NIPS13), 2001. [Angelavo 2003] D Angelova, I Simeonova and T Semerdjiev, "Monte Carlo Algorithm for Ballistic Object Tracking with Uncertain Drag Parameter",International conference on large-scale scientific computing No 4, Sozopol, Bulgarie 2003. [Arulampalam S.Arulampalam, S.Maskell, N.Gordon, T.Clapp,"A Tutorial on Particle Filters for Online Nonlinear / Nongaussian Baysian Tracking", IEEE transaction on signal 2002] processing, Vol 50, issue 2, Feb 2002. [Banavar 1994] R.N.Banavar, J.L.Spere,"Risk Sensitive Estimation and a Differential Game",IEEE Transaction on Automatic Control, Vol. 39, No.9, June 1994 [Banavar 1998] R.N.Banavar, J.L.Spere,"Properties of Risk Sensitive Filters/ estimators", IEE Proc-Control Theory Appl Vol 145, No 1, January 1998. [Banbrook 1970] Banbrook H.W, Halamandaris H, "Navigation at Sea using the invariants form of Kalman Filtering ", in Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970. [Bar Shalom Bar-Shalom Y, Li X, Estimation and Tracking- Principles, Techniques and Software, Artech House, 1993. 1993] [Barbour 2003] N. Barbour, "Advances in Navigation Sensors and Integration Technology", NATO/RTO Educational Notes, EN-SET-064, Oct. 2003 [Barham 1970] Barham,P.M.,Manville P,"Application of Kalman Filtering to Baro/Inertial Height Systems", Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970 Helen Basil, “Studies on a low cost integrated navigation system using MEMS[Basil 2005] INS and GPS with adaptive and constant gain Kalman filters”, MSc Thesis, Department of Aerospace Engineering, Indian Institute of Science, Bangalore, India, February 2005. Battin R.H., Levine G.M., "Application of Kalman Filtering Techniques to the [Batin1970] Apollo Program",in " Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970 [Aidala 1982]

Bibliography

[Bellantoni1 970] Bellantoni J.F., "Some Applications of Kalman Filtering in Space Guidance" Theory and Applications of Kalman Filtering" (Agardograph 139),C.T.Leondes (Ed).AGARD 1970. [Bergman 1997] N. Bergman, L. Ljung , F.Gustafsson,"Point-mass filter and Cramer-Rao bound for Terrain- Aided Navigation",Proceedings of the 36th Conference on Decision & Control San Diego, Califomla USA December 1997. [Bergman 1999 N. Bergman, L. Ljung, and F.Gustafsson, "Terrain Navigation Using Bayesian Statistics",IEEE control system magazine, Vol 19, issue 3, Jun 1999. a] [Bergman 1999 Niclas Bergman,"Recursive Bayesian Estimation Navigation and Tracking Applications", Phd thesis, Department of Electrical Engineering, Linkoping 1999. b] Matthew Barth and Jay A. Farrell, "The Global Positioning System and Inertial [Barth 1999] Navigation", McGraw Hill, January 1999 [Bhaumik 2005a] S.Bhaumik, S Sadhu, T.K.Ghoshal, "Risk Sensitive Estimators for Inacurately Modelled system", IEEE Indicon 2005 Conference Chennai India, 11-13 Dec 2005. [Bhaumik 2005b] S.Bhaumik, M. Srinivasan, S Sadhu, T.K.Ghoshal,"Adaptive Grid Solution of Risk Sensitive Estimator Problems",IEEE Indicon 2005 Conference Chennai India, 11-13 Dec 2005. [Bhaumik 2006a] S. Bhaumik, M. Srinivasan, S. Sadhu, T. K. Ghoshal ,"A Risk Sensitive Estimator for Nonlinear Problems Using the Adaptive Grid Method, Nonlinear Statistical Signal Processing Workshop 2006, University of Cambridge, U.K. [Bhaumik 2006b] S.Bhaumik, S.Sadhu, and T.K.Ghoshal, “Alternative Formulation for Risk Sensitive Particle Filter (Posterior)”, IEEE INDICON, September, 2006. [Bhaumik 2006c] S.Bhaumik, S.Sadhu, T.K.Ghoshal,"Parametric Performance Analysis of Tracking System Using Posterior Cramer Rao Lower Bounds", Journal of the institution of Engineers (India), Volume 87, december 2006. [Bhaumik 2007] Shovan Bhaumik, Smita Sadhu, M.Srinivasan and T.K.Ghoshal, “Adaptive grid method for nonlinear risk sensitive estimation problem” submitted to International journal of modelling and simulation. [Bhaumik 2008] S.Bhaumik, S.Sadhu, T.K.Ghoshal “Risk Sensitive Formulation of Unscented Kalman Filter”, accepted for publication, IET Control Theory & Applications, 2008. B.Boberg and S.L. Wirkander,"Integrating GPS and INS: Comparing the Kalman [Boberg 2002] Estimator and Particle Estimator", 7th International Conference on Control, Automation, Robotics And Visioo, Dec 2002. Rene K. Boel, Matthew R. James, and Ian R. Petersen,"Robustness and Risk[Boel 2002] Sensitive Filtering", IEEE Transaction on Automatic Control, Vol. 47, No. 3, March 2002. Gilbert Strang, K. Borre, "Linear Algebra, Geodesy, and GPS", Wellesley [Borre 1997] Cambridge, October S.Boyd, L.El Ghaoui, E. Feron, and V. Balakrishnan, "Linear Matrix Inequalities [Boyd 1994] in System and Control Theory", Society for Industrial and Applied Mathematics, Philadelphia, 1994. Mark Briers, Simon R. Maskell, Robert Wright, "A Rao-Blackwellised Unscented [Briers 2003] Kalman Filter", 2003 0 lSlF, 2003 [Britting 1968] Kenneth R. Britting, "Analysis of Space Stabilised Inertial Navigation Systems", MIT Experimental Astronomy Laboratory, Report RE-35, January 1968. Britting 1969 "Error Analysis of Strapdown and local Level Inertial systems [Britting1969] Which Compute Geographic Coordinates". MIT report RE52, NASA 19700007517_1970007517. 188

Bibliography

Brown and Hwang, "Introduction to random signal and applied Kalman filtering", Wiley 1996. Alison Brown and Dan Sullivan, "Precision Kinematic Alignment Using a Low[Brown 2002] Cost GPS/INS System", Proceedings of ION GPS, Portland, Oregon, September 2002. Marcelo G. S. Bruno and Anton Pavlov, "Improved Particle Filters for Ballistic [Bruno 2004] Target Tracking" IEEE International Conference on Acoustics, Speech, and Signal Processing, 2004. Marcelo G. S. Bruno and Anton Pavlov, "A Density Assisted Particle filter [Bruno 2005] Algorithm for Target Tracking with Unknown Ballistic Coefficient", IEEE International Conference on Acoustics, Speech, and Signal Processing, 2005. [Buchler 2006] Robert J. Buchler, Peter Kyriacou, Daniel A. Tazartes, "Attitude Alignment of a Slave Inertial Measurement System", United States Patent, Patent No. US 7133776 B2, Nov. 7, 2006. R.S.Bucy, K.D.Senne, “Digital synthesis of nonlinear filters”, Automatica, Vol. 7, [Bucy1971] pp 287-298, 1971. J-Pierre. LE Cadre, O T Mois, "Bearings-Only Tracking for Maneuvering [Cadre 1998] Sources", IEEE Transaction on Aerospace and Electronics Vol. 34, No. 1 January 1998. [Cardillo 1999] G.P.Cardillo, A.V.Mrstik, T.Plambeck, "A Track Filter Reentry objects with uncertain drag", IEEE Transaction on Aerospace and Electronic System Vol 35, No2, April 1999. [Carpenter 1999] J carpenter, P Cliffford, P Fearnhead, "Improved particle Filter for nonlinear Problem", IEE Proc Radar sonar Navigation Vol 145, No 1 February 1999. Y.T Chan, T.A.Rea, “Passive Tracking Scheme for a Single Stationary Observer”, [Chan 2002] IEEE Transactions on Aerospace and Electronic Systems, Vol 38, No.3, 2002 pp 1046-1054. [Chatterji 1997] G. B. Chatterji, P. K. Menon, B. Sridhar, "GPS Machine Vision Navigation System for Aircraft", IEEE Transaction on Aerospace and Electronic systems Vol 33, no 3 July 1997. Zhe Chen, “Bayesian filtering: from Kalman filters to particle filters, and beyond”, [Chen2003] Technical report, Adaptive Systems Lab,. McMaster University, 2003. Y.Cheng; J. L. Crassidis, "Particle Filtering for Sequential Spacecraft Attitude [Cheng 2004] Estimation", AIAA Guidance, Navigation, and Control Conference, Providence, RI, Aug. 2004. Y.T. Chiang, L.S. Wang, F.R. Chang and H.M. Peng, "Constrained filtering [Chiang 2002] method for attitude determination using GPS and gyro", IEE Proc Radar Sonar Navigation vol 149, No 5, October 2002. [Chuanbin 2004] Zhang Chuanbin, Tian Weifeng and Jin Zhihua, "A novel method improving the alignment accuracy of a strapdown inertial navigation system on a stationary base", Meas. Sci. Technol. V 15, April 2004, page 765-769. D.Y. Chung, J.G. Lee, C.G. Park and H.W. Park, "Strapdown INS Error Model for [Chung 1996] Multiposition Alignment", IEEE Transactions on Aerospace and Electronic Systems, Vol. 32, No. 4, 1996, pp. 1362-1366. J. M. C. Clark R. B. Vinter M. M. Yaqoob, "The Shifted Rayleigh Filter for [Clark 2005] Bearings Only Tracking", 7th International Conference on Information Fusion 2005 [Collings 1994] lain B. Colhgs,Matthew R. James,John B. Moore,"An Information-State App roach to Linear/Risk-Sensitive /Quadra/Gaussian Control", Proceedings of the 33rd Conference on Decision and Control - December 1994. [Brown 1996]

189

Bibliography

[Collings 1996]

[Cooperman 2002] [Coutinho 2003]

[Crassidis 1997]

[Crassidis 2000]

[Crassidis 2005]

[Crassidis 2003]

[Crisan 2002]

[Crocker1970]

[Dey 1995] [Dey 1997]

[Dey 2001]

[Dey 2003]

[Dezert 1999] [Dissanayake 2001]

[Dong 2006]

[Doraiswami 1996]

Iain B. Collingsy Matthew R. Jamesy John B. Moorey, "An Information-State Approach to Risk-Sensitive Tracking Problems", Journal of Mathematical Systems, Estimation, and Control, Vol. 6, No. 3, 1996. Robert L. Cooperman, "Tactical Ballistic Missile Tracking using the Interacting Multiple Model Algorithm", Proceedings of the Fifth International Conference on Information fusion 2002. D. E. Coutinho, K. A. Barbosa, A.Trofino, Carlos E. de Souza, "Linear Hinf Filter Design for a Class of Uncertain Nonlinear Systems", Proceedings of the 42nd IEEE Conference on Decision and Control, December 2003. John L. Crassidis; F. Landis Markley, "Predictive Filtering for Attitude Estimation Without Rate Sensors", AIAA Journal of Guidance, Control, and Dynamics, Vol. 20, No. 3, May-June 1997, pp. 522-527. John L. Crassidis, Roberto Alonsoy,John L. Junkinsz, "Optimal Attitude and Position Determination from Line-of-Sight Measurements", the Journal of the Astronautical Sciences, Vol. 48, Nos. 2-3, April-Sept. 2000, pp. 391-408 . John L. Crassidis, "Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation", AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, Aug. 2005. J.L. Crassidis, F. LandisMarkley, "Unscented Filtering for Spacecraft Attitude Estimation", Journal of Guidance Control and Dynamics, Vol. 26, No. 4, JulyAugust 2003. Dan Crisan and Arnaud Doucet, "A Survey of Convergence Results on Particle Filtering Methods for Practitioners", IEEE Transaction on Signal Processing, Vol. 50, No. 3, March 2002. Crocker E.D., Rabins L.," Application of Kalman Filtering Techniques to Strapdown System Initial Alignment", in " Theory and Applications of Kalman Filtering" (Agardograph 139), C.T.Leondes (Ed).AGARD 1970 S. Dey, J.B. Moore, "Risk sensitive filtering and smoothing for hidden Markov models", Systems & Control Letters, Vol 25, issue 5, 1995 Subhrakanti Dey and John B. Moore, "Risk-Sensitive Filtering and Smoothing via Reference Probability Methods", IEEE Transaction on Automatic Control Vol 42, No.11, November 1997. Subhrakanti Dey and Charalambos D Charalambous, “ Discrete time risk sensitive filters with non gaussian initial conditions and their ergodic properties” Asian Journal of Control, vol. 3, no.4, pp. 262-271, December 2001. Subhrakanti Dey and John B. Moore, "Finite-Dimensional Risk-Sensitive Filters and Smoothers for Discrete-Time Nonlinear Systems", IEEE Transaction on Automatic Control Vol 44, No.6, June 1999. Jean Dezert, "Improvement of Strapdown Inertial Navigation Using PDAF", IEEE Transaction on Aerospace and Electronic System, Vol 35 No3 April 1999. G. Dissanayake, S.Sukkarieh, E Nebot, H. D.Whyte, "The Aiding of a Low-Cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications", IEEE Transaction on Robotics and Automation, Vol 17, no 5, October 2001. Zhe Dong and Zheng You, "Finite-Horizon Robust Kalman Filtering for Uncertain Discrete Time-Varying Systems With Uncertain-Covariance White noises", IEEE Signal Processing Letters, Vol 13, No 8, Aug2006. D Doraiswami, "A Noven Kalman Filter Based Navigation Using Beacons", IEEE Transaction on Aerospace and Electronic System Vol 32 No2 April 1996.

190

Bibliography

Peter C. Doerschuk, "Cramer-Rao Bounds for Discrete-Time Nonlinear Filtering Problems", IEEE Transaction On Automatic Control, Vol 40. No 8. August 1995. A.Doucet, "On Sequential Monte Carlo Sampling Methods for Bayesian Filtering", Technical Report CUED/F-INFENG/TR310, 1998. A Doucet, S.Godsill, C. Andrieu, "On Sequential Monte Carlo Sampling Methods [Doucet 2000] for Bayesian Filtering", statistics and computing, vol 10, pp 197-208, 2000. [Doucet 2001 a] A Doucet, Nando De Freitas, N Gordon, "Sequential monte carlo methods in practice", Springer, 2001. [Doucet 2001 b] A.Doucet, N.J. Gordon, and V.Krishnamurthy, "Particle Filters for State Estimation of Jump Markov Linear Systems", IEEE Transaction on Signal Processing, Vol. 49, No. 3, March 2001 [Eduardo 2006] Nebot Eduardo, Durrant-Whyte Hugh, "Initial Calibration and Alignment of an Inertial Navigation", 4th Annual Conference on Mechatronics and Machine Vision in Practice, IEEE Computer Society, pp 175-180, 1997. R.J.Elliott, J.B.Moore, S.Dey, "Risk Sensitive Maximum Likelihood Sequence [Elliott 1996] Estimation", IEEE Transaction on Circuits and systems I Fundamental Theory and Application, vol 43, 1996. [El-Sheimy 2004] N. El-Sheimy, S. Nassar and A. Noureldin, "Wavelet De-noising for IMU Alignment", IEEE Aerospace and Electronic Systems Magazine, October 2004, pp. 32-39. P.K.Enge, F.B.Vicksell, R.B.Goddard, "Combining Pseudoranges from GPS and [Enge1990] Loran C for Air Navigation", IEEE Symposium on Position Location and Navigation, 1990. [Erzberger 1967] Heinx Erzberger, " Application of Kalman filtering to error correction of inertial navigators", NASA Technical note D-3874, February 1967. Jiang Cheng Fang, De Jun Wan, "A fast initial alignment method for strapdown [Fang 1996] inertial navigation system on stationary base", IEEE Transactions on aerospace and electronic systems VOL. 32, NO. 4, October 1996. Alfonso Farina, "Target tracking with bearings Only measurements" Signal [Farina 1999] Processing Vol 78, 1999. [Farina 2002a] A. Farina, D. Benvenuti, B. Ristic, "Estimation accuracy of a landing point of a ballistic target", Proceedings of the Fifth International Conference on Information Fusion, 2002. [Farina 2002b] A. Farina, B. Ristic, D. Benvenuti, "Tracking a Ballistic Target:Comparison of Several Nonlinear Filters" IEEE Transaction on Aerospace and Electronic Systems Vol. 38, No. 3 July 2002. [Farina 2002c] A.Farina, M.G.Del Gaudio, U.D.Elia, S.Immediata, L.Ortenzi, "Detection and Tracking of Ballistic Target", the IEEE Radar Conference, Proceedings 2004. Jay Farrel, Matthew Barth, "The Global Positioning System and inertial [Farrel 1998] Navigation", McGraw Hill companies, Inc, 1998. Gang Feng, "Robust Filtering Design of Piecewise Discrete Time Linear [Feng 2005] Systems", IEEE Transaction On Signal Processing, Vol 53, No2, Feb 2005. Jason Ford, " Risk Sensitive Filtering and Parameter Estimation" Technical report [Ford 1999] Weapons Systems Division Aeronautical and Maritime Research Laboratory, January, 1999. [Frykman 2003] Petter Frykman, "Applied particle filters in integrated aircraft navigation", Master's thesis, Automatic Control and Communication Systems, Dept. of Electrical Engineering, LinkÄopings universitet, 28th April 2003 M. Fujitat, A. Mamyamat, T. Taniguchit and K. Uchidai, "Finite Horizon Discrete[Fujitat 1993] Time H, Filter with Application to an Active Vision System", Proceeding. of the [Doreschuk 1995] [Doucet 1998]

191

Bibliography

32nd Conference on Decision and Control San Antonio, Texaa, December 1993. Wei Gao, Yueyang Ben, Feng Sun and Bo Xu, "Performance Comparison of Two Filtering Approaches for INS Rapid Transfer Alignment", Proceedings of IEEE international conference on mechatronics and automation, Aug 5-8, 2007, China. [Gerome 1999] J. C. Geromel, J. Bernussou, G. Garcia, and M.C.D.Oliveria, "H2 and H∞ Robust Filtering for Discrete Time Linear Systems", SIAM J. Control & Optimization , Vol. 38, No. 5, pp. 1353–1368. [Geromel 2000] J. C. Geromel, J. Bernussou, G. Garica and M. C. De Oliveira, “H2 and H∞ robust filtering for discrete time linear system”, SIAM J. Control Optimization, Vol. 38, No. 5, pp. 1353–1368 2000. Saurabh Godha, "Performance evaluation of low cost MEMS-based IMU [Godha 2006] integrated with GPS for land vehicle navigation application", MSc Thesis, Department of geomatics engineering, University of Calgary, Calgary, Alberta, Canada, February 2006. [Gordon 1993] N.J. Gordon, D.J. Salmond, A.F.M. Smith, "Novel approach to nonlinear/nonGaussian Bayesian state estimation IEE Proceedings-F, Vol. 140, No. 2, April 1993. M.S.Grewal, R.S.Miyasako and J.M. Smith, "Application of fixed point smoothing [Grewal 1988] to the calibration alignment and navigation data of inertial navigation systems", IEEE Position Location and Navigation Symposium, 1988. M.S.Grewal, V.D. Henderson, R.S. Miyasako, "Application of Kalman Filtering to [Grewal 1990] the calibration and Alignment of Inertial Navigation Systems", Proceedings of the 29th. Conference. on Decision and Control. Honolulu, December. 1990. M.S.Grewal, and P.S.Andrews, “Kalman Filtering: Theory and practice using [Grewal 2001] Matlab”, 2nd edition, John Wiley & Sons, 2001. [Grimble 1990] M. J. GRIMBLE and A. El Sayed, "Solution of the Hinf Optimal Linear Filtering Problem for Discrete-Time Systems", IEEE Transaction On Acoustics, Speech and Signal Processing, Vol 38. No 7, July 1990. Paul D. Groves, "Optimising the Transfer Alignment of Weapon INS", The journal [Groves 2003] of Navigation, Vol. 56, pp. 323-335, 2003. Clay E. Hagan, Christopher S. Lofts, "Accelerometer Based Alignment Transfer", [Hagan 1992] IEEE, Position Location and Navigation Symposium, 1992. Record. 500 Years After Columbus - Navigation Challenges of Tomorrow. IEEE PLANS '92, 1992 [Halamandaris Halamandaris H., Ozdes D. "A Kalman Filter Augmented Marine Navigation System" Theory and Applications of Kalman Filtering" (Agardograph 1970] 139),C.T.Leondes (Ed).AGARD 1970. Peter D. Hanlon and Peter S Maybeck, "Characterization of Kalman Filter [Hanlon 2000] Residual in the Presence of Mismodelling", IEEE Transaction on Aerospace and Electronic System Vol 36 No1 April 2000. Yanling Hao, Zhilan Xiong, Wei Wang and Feng Sun, "Rapid Transfer Alignment [Hao 2006] Based on Unscented Kalman Filter", Proceedings of American control conference USA June 2006. B. Hassibi, Ali H. Sayed, T.Kailath, "Linear Estimation in Krein Spaces- Part I&II [Hassibi 1996] : Theory", IEEE Transaction On Automatic Control, Vol. 41, No. 1, Jan 1996 [Helmick 1993] R.E.Helmick, T.R.Rice, "Removal of alignment errors in an integrated system of two 3-D Sensors", IEEE Transactions on Aerospace and Electronic Systems, Vol. 29, No. 4, 1993, pp. 1333-1348 M. Hernandez, B.Ristic, A. Farina, and L.Timmoneri, "A Comparison of Two [Hernandez Cramér–Rao Bounds for Nonlinear Filtering with Pd < 1", IEEE Transaction on 2004] [Gao 2007]

192

Bibliography

Signal Processing, Vol. 52, No. 9, September 2004. [Hewitson 2003] S.A. Hewitson, J. Wang, A.H.W. Kearsley, "Performance evaluation of inertial navigation systems for surveying", The 6th International Symposium on Satellite Navigation Technology Including Mobile Positioning & Location Services (SatNav 2003), Melbourne, Australia, 22-25 July 2003 C.Hide, T.Moore and M.Smith, "Adaptive Kalman Filtering for Low-cost [Hide 2003] INS/GPS", The Journal of Navigation, Vol, 56, 2003, pp143-152. K. C. Ho and I Z Chan, "An Unbiased Estimator for Bearing Only Tracking and [Ho 2003] Doppler Bearing Tracking", IEEE International conference on Acoustics, Speech, and Signal Processing, 2003. H.S. Hong, J.G. Lee and C.G. Park, "Performance improvement of in-flight [Hong 2003] alignment for autonomous vehicle under large initial heading error", IEE Proceedings - Radar Sonar Navigation, Vol. 151, No. 1, February 2004. Daiwen Hou, Fuliang Yin and Liyan Zhang, "Joint Sigma-Point Kalman Filter [Hou 2006] Based Bearing-Only Tracking "Proceedings of the 6th World Congress on Intelligent Control and Automation, June 21 - 23, Dalian, China, 2006. Li-Sheng Hu, James Lam, Yong-Yan Cao, and Hui-He Shao, "A Linear Matrix [Hu 2003] Inequality (LMI) Approach to Robust H2 Sampled-Data Control for Linear Uncertain Systems", IEEE Transaction on Systems man and cybernetics -P Art B:Cybernetics Vol 33 No1 Feb 2003. Yufei Huang, and Petar M. Djuric, "A Hybrid Importance Function for Particle [Huang 2004] Filtering", IEEE Signal Processing Letters , Vol. 11, No. 3, March 2004. Carine Hue, Jean-Pierre Le Cadre, Patrick Pérez, "Sequential Monte Carlo [Hue 2002a] Methods for Multiple Target Tracking and Data Fusion", IEEE Transaction on Signal Processing, Vol. 50, No. 2, February 2002. C.Hue, J.P.Le Cadre and P Perez, "Tracking Multiple Objects with particle [Hue 2002b] filtering", IEEE Transaction on aerospace system vol 38, No 3, 2002. Andreas Huster, Eric W. Frew and Stephen M. Rock, "Relative Position [Huster 2002] Estimation for AUVs by Fusing Bearing and Inertial Rate Sensor Measurements", Oceans 2002 Conference, Biloxi, MS, pp.1857-1864, October, 2002. G.Hyslop, D.Gerth, J.Kraemer, "GPS/INS Integration On the Standoff Land [Hyslop 1990] Attack Missile (SLAM)", Aerospace and Electronic Systems Magazine, IEEE, 1990. G.Hyslop, D.Gerth, J.Kraemer, "GPS/INS Integration On the Standoff Land [Hyslop1990] Attack Missile (SLAM)", Aerospace and Electronic Systems Magazine, IEEE, 1990. A.G.iremus, A. Doucety, A.C.Escher, J.Y.Tourneret, "Nonlinear Filtering [Iremus 2004] approach for INS/GPS Integration", European Signal Processing Conference 2004. K. Ito and K. Xiong, "Gaussian Filters for Nonlinear Filtering Problems", IEEE [Ito 2000] Transaction on Automatic Control Vol 45 No 5, May 2000 Joseph Ben-Jaacov, "Method for transfer alignment of an inertial measurement [Jaacov 2003] unit in the presence of unknown aircraft measurements delays", United States Patent, Patent No US 6556895 B2, Apr. 29, 2003 C.-W.Jang, J.-C.Juang and F.-C.Kung, "Adaptive fault detection in real-time GPS [Jang 2000] positioning", IEE Proc-Radar Sonar Navigation., Vol. 147, No. 5, October 2000 [Jauffret 1996] C.Jauffret, D.Pillon, "Observability in passive Target Motion analysis" IEEE Transaction on aerospace and electronics systems Vol 32 No 4 Oct 1996. M. Jayakumar and Ravi N. Banavar, "Risk-Sensitive Filters for Recursive [Jayakumar Estimation of Motion From Images ", IEEE Transaction on Pattern Analysis and 1998]

193