Multiple Model Estimation for Linear Stochastic Hybrid Systems with Non-Homogeneous Transition RCHIVES Probabilities MASSAC HUSETTS INSTITUTE OF TECHNOLOGY by Michael William Kasperski 0 T114 2015 B.S., The University of Texas at Austin (2013) LI 3RARIES Submitted to the Department of Aeronautics and Astronautics in partial fulfillment of the requirements for the degree of Masters of Science in Aeronautics and Astronautics at the MASSACHUSETTS INSTITUTE OF TECHNOLOGY September 2015 Massachusetts Institute of Technology 2015. All rights reserved. Signature redacted Author .. I.. epartnment eronautics and Astronautics August 20, 2015 Certified by........ Signature redacted.. Hamsa Balakrishnan Associate Professor of Aeronautics and Astronautics Thesis Supervisor Accepted by............ Signature redacted Paulo C. Lozano I Associate Professor of Aeronautics and Astronautics Chair, Graduate Program Committee 2 Multiple Model Estimation for Linear Stochastic Hybrid Systems with Non-Homogeneous T'ransition Probabilities by Michael William Kasperski Submitted to the Department of Aeronautics and Astronautics on August 20, 2015, in partial fulfillment of the requirements for the degree of Masters of Science in Aeronautics and Astronautics Abstract This thesis investigates the field of stochastic hybrid estimation. A broad introduction to the framework surrounding estimation, filtering, and multiple model based systems is presented. More specifically, the often made assumption of a constant time-invariant mode transition probability matrix is relaxed. Recent work done in the area of non-Markov jump stochastic hybrid systems is explored, including semiMarkov systems, non-homogeneous transition probability matrices, and continuousstate-dependent mode transitions. Algorithms needed to develop linear multiple model based filters with non-homogeneous transition probabilities are detailed. Finally, a case study for the practical implementation of an extended Kalman filter in the application of attitude heading and reference systems is conducted. Thesis Supervisor: Hamsa Balakrishnan Title: Associate Professor of Aeronautics and Astronautics 3 4 Acknowledgments I would like to extend my heartfelt thanks to Jason McKnight, for all your help and support. Without your constant encouragement, this thesis would not have existed. I would like to thank my advisor Hamsa Balakrishnan for your understanding, patience, and help throughout my time at MIT. Finally, thanks Mom and Dad, for pushing me forward, and for always being there when I needed someone to talk to, even if I didn't know it. 5 6 Contents 1 1.1 Estim ation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 1.2 F iltering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 1.3 M odel Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 Multiple Model Estimation . . . . . . . . . . . . . . . . . . . . 16 1.3.1 2 . . . . . . . . . . . . . . . . . . 21 . . . . . . . . . 22 2.2 Semi-Markov Jump Systems . . . . . . . . . . . . . . . . . . . . . . . 23 2.3 Non-Homogeneous Markov Transitions . . . . . . . . . . . . . . . . . 24 2.4 Linear Hybrid Systems with Non-Homogeneous Transition Probabilities 25 Markov Jump Systems and the IMM 2.1.1 The IMM Estimator: A Common Application 29 Filtering and Multiple Model Estimation 3.1 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 . . . . . . . . . . . . 32 Multiple Model Estimation . . . . . . . . . . . . . . . . . . . . . . . . 32 3.2.1 General Formulation . . . . . . . . . . . . . . . . . . . . . . . 33 3.2.2 The Interacting Multiple Model Estimator . . . . . . . . . . . 34 3.2.3 Continuous-State-Dependent Mode Transition Probabilities. . 36 3.1.1 3.2 4 19 Literature Review 2.1 3 13 Introduction The Kalman Filter Recursion Equations Case Study: Attitude Heading and Reference Systems 4.1 Variables and Coordinate Definitions . . . . . . . . . . . . . . . . . . 7 39 40 4.3 The Extended Kalman Filter. 44 4.4 Process Model . . . . . . . . 45 4.5 Measurement Model..... 47 4.6 Simulation Model . . . . . . 49 4.7 Results . . . . . . . . . . . . 51 4.8 Summary . . . . . . . . . . 56 . . . . . . Sensor Characteristics 59 Discussion . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . 59 5.2 Future Work. . . . . . .. . . . . . . . . . . . . . . . . . . . . . . 60 . . 5.1 . Conclusion . 5 42 4.2 8 List of Figures [31 . . . . 1-1 State estimation, a concise representation, reproduced from 4-1 Body fixed reference frame (left), and inertial reference frame (right) 14 [2 3] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 4-2 Simulated roll, pitch, and yaw for (a) Model 1 and (b) Model 2 . . . . 50 4-3 Simulated side slip and climb angles used in Model 3 . . . . . . . . . 51 4-4 Estimation divergence for the (a) designed filter, under the same noise conditions as the (b) original filter . . . . . . . . . . . . . . . . . . . . 4-5 52 Monte Carlo simulation for (a) filer with altitude measurements and (b) without altitude measurements, for the same sensor noise parameters 53 4-6 Sample trajectory for one of the fifty iterations . . . . . . . . . . . . . 54 4-7 Mean of the roll and pitch error for the fifty iterations . . . . . . . . . 54 4-8 Mean innovations sequences for the fifty iterations . . . . . . . . . . . 55 9 10 List of Tables 4.1 Standard definitions of relevant variables . . . . . . . . . . . . . . . . 40 4.2 Noise in the simulated sensors . . . . . . . . . . . . . . . . . . . . . . 51 4.3 Error statistics for the Monte Carlo simulation . . . . . . . . . . . . . 53 4.4 Error statistics for miss-matched model . . . . . . . . . . . . . . . . . 56 11 12 Chapter 1 Introduction In modern systems, many applications can be classified as hybrid, or systems with both discrete and continuous dynamics 141. There has been a wealth of studies on how to perform estimation on a class of hybrid problems coined as jump Markov systems. These systems behave according to a finite number of dynamic modes, with one mode being active at any given time, and the transition between modes being modeled as a Markov chain. The investigation in this thesis focuses on relaxing common assumptions about the structure of the Markov chain governing the mode transitions, such as its often assumed time invariance and constant state-independent nature. 1.1 Estimation Estimation, in the simplest terms, is making a "best guess" of some aspect of a system based on incomplete, indirect, intermittent, and uncertain observations. In other words, to map a set of measurements of the systems behavior to an estimate of the needed state variable. It is an incredibly deep field, with a variety of unique applications. For instance, consider a standard GPS receiver, where there is no information about the current location of the object, but multiple "noisy" (uncertain) measurements off different satellites. Estimation will fuse those different indirect observations into the "best" estimate of the receivers position. Applications can range 13 System t Dynamic System Prior Information Measurement Error Source Error Source ttState System State ,Measurement System Measurement, State Estimator Estimate, State Uncertainties Figure 1-1: State estimation, a concise representation, reproduced from [31 from tracking problems, to parameter or model determination, to stochastic control, and to many more. The main focus of this thesis is on the topic of state estimation. illustrates the process of state estimation. Figure 1-1 Here, the observer has access only to the measurements. There can be uncertainty in both the measurements taken, and the understanding of the dynamic system in question. The observer must use both the uncertain understanding of the system, the uncertain measurements, and prior knowledge to infer some aspect of interest in the system. 1.2 Filtering In order to implement estimation in practice, recursive algorithms are often used. That is, given the previous estimate of the state, use a new piece of information (measurement) in order to compute a new estimate of the state. Of all the estimators, perhaps the most well known and widely used is the Kalman filter. It provides a recursive framework that computes the optimal state estimate in the sense of leastsquared error for linear Gaussian systems. Often, state and measurement equations used to model physical systems are nonlinear. Although the Kalman filter will not provide optimal estimates in that case, an approximate solution can be computed through linearization. The extended Kalman filter does this using first order Taylor expansions within the state and measurement equations. Several other methods for non-linear filtering exist depending on the application and system being considered, such as the unscented Kalman filter or particle 14 filter. 1.3 Model Uncertainty Kalman filter design, both in the linear and nonlinear cases, relies on some design model that represents the system being investigated. The uncertainties in these types of filters often consists of additive white noises in the system and measurement equations. That is, the underlying system and measurement model are well understood, including the transition matrices, input, gain, and noise properties. However, in many practical applications, much like other parts of estimation, there exists some uncertainty in these parameters. The system may be too complex, have hard to determine parameters, or have uncertain noise. In addition, a broader class of problems emerges when the system being considered dramatically changes over time in a stochastic sense. In general, there are two approaches to handling model uncertainty [33, 3]. 1. Robust estimation: Design a fixed filter that performs for all expected variations in the model parameters (a) Robust Kalman (72) filtering: Design the filter that gives small error for all models that might be active in the system. That is, reach a compromise between all expected behaviors of the system. (b) W... filtering: Consider the noise process to be deterministic, and attempt to minimize the energy of the estimation error signals relative to the noise energy. W,4 filtering is especially efficient at handling external noise sources with unknown statistics. 2. Adaptive state estimation: Design a filter that estimates both the state, and the model parameters (i.e. the system matrices) (a) Joint filtering of state and parameters: Add the state parameters to the state vector and create a bi-linear system. Particle filter methods often show good results with this type of estimation 15 (b) On-line noise tuning: Implement simple rules that modify noise levels when a filter is misbehaving, often through monitoring innovations and their covariance whiteness. (c) Multiple Model (MM) estimation: Divide the system dynamics into a number of modes (discrete states), and within each mode classify the continuous state dynamics as set of difference/differential equations. Estimate both the continuous state and the discrete mode of the system. 1.3.1 Multiple Model Estimation In the multiple model approach, the system in question behaves according to one of a finite number of models. The uncertainty in which model, or "mode", is active is a discrete uncertainty. The combination of the continuous uncertainties (i.e. noise) and discrete uncertainties (mode) classify this type of problem as stochastic hybrid estimation. In the dynamic sense, the actual system may switch between modes of operation, but only one mode will be active at a given time. The multiple model filter keeps track of both the continuous statistics (the state estimate and covariance) and the discrete statistics (mode probabilities) For example, when tracking an aircraft there is a large difference in system dynamics when it is undergoing uniform motion compared to when it is maneuvering. If the filter used to track the aircraft is based on the more prevalent mode of flight, uniform motion, then the portions of flight where the aircraft is maneuvering will cause the filter to give less accurate estimates. As discussed above, one way to handle this is through the use of robust Kalman filtering, where a compromise is reached between the two modes, giving an acceptable level of root-mean squared (RMS) error when tracking either mode of flight. However, another approach is to use a multiple model filter, where there are two distinct filters, each providing estimates for one mode of flight, which are combined by keeping track of mode probabilities. It has been shown that this form of multiple model estimation gives much better performance in both modes of flight [7, 211. 16 In the multiple model approach approach, the continuous state probability density functions (pdfs) are represented as a mixture of Gaussian hypotheses, each conditioned on a mode of the system. The transitions between modes is modeled by a Markov chain. A bank of Kalman filters, matched to the modes of the system, is used to propagate the conditional Gaussian pdfs based on new measurements. As shown in [3, 1, 7], the optimal state estimator for hybrid systems relies on the state estimates of each individual mode, and must include all possible mode histories. It can be shown that this leads to an exponential growth in the number of hypotheses being tracked. Even in the most basic case of linear Markov switching, the algorithm requires exponentially growing memory. In practice, to implement a filter it is necessary to have a recursive algorithm where the complexity does not grow with time. Thus, a sub-optimal estimator is required. These sub-optimal estimators differ in that they use a Gaussian mixture as an approximate information state to curb the growth of complexity. There are several such estimators, including generalized pseudo-Bayesian algorithms (GPB1, GPB2), and the interacting multiple model (IMM) algorithm [1, 31. The differences between the algorithms lies in the timing and method of hypotheses reduction. 1. History pruning: A na~ive approach, where the hypotheses with low probability are discarded and the remaining ones have the probabilities renormalized. 2. History merging: Use fixed depth merging where equivalent Markov chain paths are merged to a single Gaussian pdf. For example, the GPB algorithms will merge histories after the measurement update occurs. Much of the framework surrounding multiple model estimation assumes that the mode transitions are governed by a Markov process where the transition probabilities between different modes are constant, well defined, and depend only on the previous mode. However, in practical applications this is not always the case. For example, an aircraft navigating through sectors in the national air space (NAS), it is much more likely to start a maneuver near a waypoint, and mode transitions will be a function of position. As mentioned, this thesis is primarily concerned with the case where the 17 transition probabilities are allowed to vary. 18 Chapter 2 Literature Review Multiple model estimation has been the focus of research in many different disciplines. There exists a wide array of systems that can be modeled as a finite set of dynamic modes with well understood transition properties, or as jump Markov systems (JMS). A system that exhibits dynamics that abruptly change within a finite set can be fit into the multiple model framework. These abrupt changes in system behavior can be due to a variety of factors, such as external environmental changes or internal system component failures. Hybrid systems are defined by two main components [25]: 1. State: The configuration of an object at a given time, that changes based on a stochastic difference/differential equation. 2. Mode (or Model): One of a finite number of possible modes (each governed by a different set of difference/differential equations). The mode switches according to a set of transition probabilities, and is thus governed by a discrete stochastic process. In this sense, a multiple model estimator is able to provide more accurate estimates by using primarily the state estimator matched to the current system dynamics, and switch estimators when the mode switches. This approach provides an advantage when it is not possible to classify the dynamic behavior of a system with a single model. 19 Many different fields in science and engineering have utilized these types of filters in theory and practice. Much attention has been payed to target tracking, or trying to estimate the state of a maneuvering object through external measurements. For example, the process of tracking an aircraft through ground based radar measurements [2, 21, 31, 32], terrain-based ground target tracking 116], road constrained vehicle tracking [19], or multi-target grid based particle filter tracking [28]. The process of tracking has also been extended to intent based trajectory prediction for aircraft [381. Another field which has received extensive focus is failure detection in dynamic systems. Here, failures in components or subsystems can be represented as abrupt changes in the parameters of the system. This can range from simple detection in dynamic systems [34, 37]; targeted detection of sensor/actuator failure in complex systems like F16 jets [26]; detection of interference, jamming, and spoofing in GPS systems [36]. Similarly, repaired components can also cause this abrupt change in system parameters [5]. In the same vein, the area of diagnostics and fault diagnosis has been investigated, which goes beyond identifying a failure has occurred and seeks to answer why and how 113, 15, 20]. Multiple model based filters have also seen ex- tensive use in signal processing, with specific focus on detection of Bernoulli-Gaussian processes [12]. Also these filters have been used in sensor management problems [141. Another interesting extension of the multiple model framework is to the approximate modeling of nonlinear systems by a set of linearized models which encompass the systems dynamic range [27, 35]. Multiple model estimation has also seen use in medical applications, where it has been used to track tumors [301 and estimate cardiac phases in echographic images [291. Many complex dynamic applications exist that can be put in the framework of hybrid systems with a continuous state and set of discrete modes. In order to effectively handle estimation for these types of systems, multiple model algorithms have seen extensive development in the past decades. These algorithms approach the problem of multiple model estimation in a systematic and practical way. In general these algorithms are characterized by a bank of filters, each tuned to the dynamics of one of the finite modes of a system (mode-matched filter) and an underlying rule that merges 20 the individual filters into a single estimate [25]. The bank of filters can be comprised of simple Kalman filters (KF), extended KFs (EKF), and other similar algorithms. Work has also been done to utilize more complex methods such as particle filtering [8, 9, 20]. 2.1 Markov Jump Systems and the IMM One of the most basic forms of multiple model estimation is for Jump Markov linear systems. Consider a system of difference equations, with state Xk and measurements Zk: + w(k, Mk) (2.1) H(Mk)xk + v(k, Mk) (2.2) Xk = A(Mk)k-1 Zk where Mk - {1, 2,... ,r} is a finite Markov chain representing the systems mode at sampling period k, has known transition probabilities: 7rij = P{Mk = jlMk_ 1 = i} (2.3) and the noise statistics of w and v are mutually independent white Gaussian processes uncorrelated with the initial estimate. In this context, one algorithm that has emerged as the dominant method of estimating linear jump Markov systems is the interacting multiple model (IMM) algorithm. It has been shown to be a superior compromise between computational complexity and estimate accuracy [7, 21]. Blom and Bar-Shalom demonstrate that for this class of system the exact optimal estimator is characterized by exponentially increasing complexity due to the need to condition on a growing number of mode histories [7]. In practice, such an estimator can not be implemented. They highlight that prior to their paper there were several approaches to implementing a sub-optimal estimator, which differ in how the prior mode histories are managed. As outlined in section 1.3.1 there are two approaches to managing the mode histories. First, the 21 histories with low probability can be "pruned" or discarded, which is done in the detection estimation (DE) algorithm. Second, the histories can be merged as in the generalized pseudo Bayes (GPB1/GPB2) algorithms. Blom and Bar-Shalom saw room for improvement in the area of hypotheses (history) reduction. Their algorithm, similar to GPB takes advantage of merging, but uses a different timing. They coined this novel filter as the interacting multiple model (IMM) algorithm. In their paper they demonstrated that the IMM algorithm, which has linear computational complexity (as a function of the number of modes/filters), performed about the same as algorithms with quadratic complexity (GPB2) [7]. 2.1.1 The IMM Estimator: A Common Application One of the first applications of the IMM algorithm after it was proposed in [71 was for air traffic control (ATC) tracking [211. The ATC problem demonstrates several key ideas behind multiple model estimation. In standard civilian aviation, most aircraft will exhibit two main modes of operation. They will either be undergoing uniform motion (UM), i.e. non-accelerating strait line motion, or undergoing a maneuver (usually coordinated turns or climbing/descent). The goal for ATC is to provide accurate estimates of the speed, location, flight mode, and altitude of aircraft occupying an increasingly dense national air space (NAS). The measurements of the system (aircraft) available to ATC are given by radar reports, which are limited in both accuracy and reliability 1211. Given that the measurements are an uncertain sequence of position readings, the goal is to infer not only a more accurate position estimate, but also velocity, flight mode, etc.. At the time of the paper, the current method of estimating an aircrafts state was to use a single Kalman filter. Thus, the filter was basically an average of the different dynamic modes (using noise manipulation). In order to capture the maneuvering sections of the trajectory, there was a large compromise in terms of error (RMS) during the UM portions of flight. Li and Bar-Shalom demonstrated that the IMM filter provided much better estimated for both modes of flight, along with the ability to detect which mode the aircraft was currently in [211. 22 2.2 Semi-Markov Jump Systems One of the first assumptions to receive further scrutiny was the Markov nature of Approximate Bayesian filters for semi-Markov jump linear the mode transitions. systems were investigated in [6, 11, 24]. Even before Blom and Bar-Shalom published the IMM results, Mathews and Tugnait looked at hybrid stochastic estimation for linear discrete-time systems with fixed-lag [24]. That is, given some time t, use measurements up to time t + N, where N is defined as the lag. The changing system parameters, i.e. multiple modes, are modeled as a semi-Markov chain with known transition statistics. In this type of semi-Markov chain, the transition probability matrix is the same as a standard IMM filter, (i.e. assumed constant and known to the filter designer), but time between transitions are assumed to be an arbitrary random variable. The idea behind processing with fixed lag was based on the fact that in well-defined systems with known parameters, fixed-lag smoothing leads to better estimates. Thus, the idea was to apply the same methodology to systems in the hybrid class. One application they mention is the case of off-line tracking analysis of maneuvering targets, where time-delay has little effect. Mathews and Tugnait utilize a detection estimation algorithm (DEA), which is a sub-optimal method that uses history "pruning" to get around the inherent history growth. They show that, as expected, the addition of a fixed-lag delay in processing observations can lead to an improvement in both detection and estimation for hybrid systems. Later, Campo et. al. further investigated the issue of fixed-lag based estimators for hybrid systems [111]. They looked at the case where the modes switch after a random sojourn time, and in some cases the switching probabilities are a function of the sojourn time. This leads to a "sojourn-time-dependent-Markov" (STDM) chain which is used in the underlying mode switching model. This semi-Markov model is used in combination with the hypothesis merging techniques in the, at the time, recently developed IMM algorithm [7]. They demonstrated through simulation, that this STDM-IMM algorithm gave a stable estimation filter for both two and three model cases. Also, they showed that the could rapidly detect the true system model. 23 Blom furthered this development of semi-Markov based estimators [6]. In his paper, he developed the full Bayesian filter equations, both in the optimal (noncomputationally effecient) sense, and the sub-optimal sense. He further reduced the computational burden of the STDM-IMM filter developed by Campo et. al., reducing the number of computations involved in hypotheses merging. The key development in his paper was the modeling of semi-Markov mode switching as state-dependent mode switching, and deriving the exact Bayesian equations for that case. He also proposed two additional semi-Markov IMM algorithms (SM-IMM) which proved to be much more cost effective than the previous STDM-IMM and SM-DEA algorithms. 2.3 Non-Homogeneous Markov Transitions A natural further extension to the research is to look at non-homogeneous or uncertain Markov transitions. That is, further relax the assumption that the transition probabilities are fixed and known to the designer. There has been a fair amount of research done in this area. In practical applications, non-Markov jumps are the rule rather than the exception. For instance, an aircraft taxiing at an airport is much more likely to make a maneuver in certain positions, such as intersections. Similarly, aircraft in flight are much more likely to start a turn near fixed waypoints, or begin maneuvers at fixed positions or velocities. One proposed method was to consider the case of estimating the state of a hybrid system with state-dependent mode transition probabilities. Koutsoukos et. al. [20] and Blom and Bloem [8, 9] looked at this case through the application of particle filtering methods. Blom and Bloem noted that there exists a large challenge implementing nonlinear filtering when there are non-Markov jumps, and that this challenge can be countered by applying particle filter methods [8]. Their paper extends the exact Bayesian filter characterization to this much larger class of problems, and develops a novel particle filter for stochastic hybrid systems. This IMM particle filter (IMMPF) was tested through simulation and demonstrated to have better performance for situations with frequent mode switching. When the mode switches 24 were infrequent it performed about as well as a standard IMM, but is still much more useful for deviations from the standard Markov jump linear system (i.e. it can handle'nonlinearities and non-Markov mode switching). However, even though these methods can be applied to a large class of systems (nonlinear, non-Gaussian noise, non-Markov transitions, etc.), many problems can emerge when trying to implement particle filters. Computational complexity and stability concerns emerge, especially when the dimension of the state space is large. Blom and Bloem showed that in order to estimate the state in a hybrid system when the mode transitions are functions of the state vector, a multivariate integration of the continuous state pdfs and mode transition probabilities must be computed 19]. Some work has been done attempting to approximate this integration or simplify it in some way. Zhang looked at the case where the transition probability matrix is piecewise homogeneous [39]. That is, it is time-varying, but does so within a finite set of possible matrices. He considers variations of two types: arbitrary and stochastic. He notes that this is a special case of non-homogeneity within a larger class of these types of transition probability matrices. The filtering problem is developed within the '.. framework rather than IMM. Long and Yang furthered this line of thinking by looking at fault detection based on a delta operator approach [221. Like Zhang, they looked at the case where the transition probability matrix varies within a finite set, but further defined the set as a finite polytope set. Their approach was developed to investigate systems that have mode-dependent delays, non-linearities, and a class of non-homogeneous Markov switching. 2.4 Linear Hybrid Systems with Non-Homogeneous Transition Probabilities The work done by Koutsoukos et. al., Blom and Bloem highlights one of the challenges in implementing a filter for stochastic hybrid systems with non-homogeneous transition probabilities. That is, when the mode transition probabilities are a func25 tion of the continuous state, a multivariate integral must be evaluated in order to compute the recursion of the transition probabilities [8, 9, 20]. Their formulation relied on particle filtering, which despite covering a large class of estimation problems, is sometimes too complex. Seah and Hwang investigated the specific case of linear hybrid systems, and made approximations or simplifications to evaluate the multivariate integral [31, 32]. In their first paper, Seah and Hwang look at the case of tracking aircraft [31]. They use the IMM algorithm to counter the exponentially growing mode histories, and a Monte Carlo (MC) integration method to solve the multivariate integral that comes from the state-dependent mode transition probabilities. They show that, for some applications, the computational cost associated with the MC integration is small. As for the mode transition probabilities, they condition them based on the state-variables associated with the flight plan and pilot's intent modeling. Specifically, they base the transition probabilities on waypoints, navaids, or fixes in the pre-described flight plan, and make the assumption that the pilot will adhere to that plan. They key novel concept they present is the use of MC integration along with approximations similar to those found in the IMM (merging) in order to efficiently evaluate the multivariate integral. They provide a standard ATC example involving coordinated turn and uniform motion modes of flight. Their algorithm exhibited lower peak and average estimation error and more accurate mode prediction when compared to a standard naive IMM estimator with a constant transition probability matrix. Seah and Hwang furthered their work on continuous-state-dependent mode transitions by looking at the simplification of continuous state "guard conditions" [32]. They point out that in many cases, a mode transition will occur when the continuous state reaches a certain subset. They define this as a "guard condition" of the continuous state space. They reference the specific example of ATC, where the majority of flight profiles follow fixed patters. For example, when an aircraft is taking off from the runway, it will perform an initial acceleration until reaching a critical speed (guard condition), then climb at a constant velocity until reaching a certain altitude (guard condition) before resuming a gradual accelerating climb. To solve the problem 26 of multivariate integration, the method Seah and Hwang propose in this paper is a form of analytical integration where the computational cost depends on the dimension of the guard condition, and is independent of the continuous state space. They also consider uncertainty within the guard conditions, which more realistically models practical systems, and extend their work to cover stochastic guard conditions. They put the conditional mode probabilities in terms of Gaussian pdfs (they also do a version with cdfs). The algorithm developed is coined as the state-dependent-transition IMM (SDT-IMM). Two examples using a standard ATC example are presented. Zhao and Lie approach the problem of linear non-homogeneous jump Markov systems in a slightly different fashion [401. They look at the case where the transition probability matrix is time variant and takes values from a finite set randomly at each time step, and also reference Blom and Bloems particle filter work [9]. They regard the matrix as a continuous stochastic variable, allowing for an optimal recursion to find the posterior matrix pdfs. Each transition probability matrix in the set is used to run a different bank of KFs in a second mixing step. This proves to be more efficient that the PF methods developed by Blom and Bloem, but has the trade-off of only working when the transition probability matrix exhibits this structure. They provide simulation benchmarks comparing their algorithm to a standard IMM filter, where the underlying simulation is structured around the defined class of transition probability matrices. As expected, their filter is more accurate, while being more computationally demanding that the standard IMM. 27 28 Chapter 3 Filtering and Multiple Model Estimation As discussed, there is a wealth of literature and research done on the topic of filtering and multiple model based estimation. Several key concepts emerge when studying this class of problems. Fist, some form of filter is needed to recursively provide model matched estimates. Second, a Bayesian framework is applied to the stochastic nature of the discrete mode estimation. Lastly, sub-optimal techniques are needed to reduce both the complexity of the multiple model estimator, and the multivariate integration that arises in formulating the mode probability posterior when the mode probabilities are a function of the state. 3.1 The Kalman Filter As mentioned, the Kalman filter (KF) is a widely used optimal estimator given the problem fits within certain constraints and assumptions. Most filter derivations are done in the discrete time case, and assume that the noise statistics on the system and measurements are zero mean, uncorrelated, white, and Gaussian. Of course, it is possible to relax and modify most of those assumptions. For instance, the filter can easily be put into the continuous time framework. Also, the zero-mean assumption can be taken away by simple static shifting, and whiteness can be dealt with by pre29 conditioning the filter. In most cases, practical problems can be framed in a context that will make the Kalman filter the optimal linear estimator. For the purposes of this thesis the Kalman filter will be kept in its most basic form. That is, a linear estimator with well behaved noise statistics. This produces a simple recursive algorithm that provides estimates for a stochastic system, where the estimate takes the form of a Gaussian random variable with a given mean and covariance. First, consider the linear discrete-time system with state and measurement difference equations given by: Xk+1 =AkXk + Wk Zk = HkXk +Vk k = 0,1, 2, ... (3.1) k = 1,2,... (3.2) where Xk is the nx-dimensional state vector and zk is the nz-dimensional measurement vector. If necessary, an input vector (eg. control, forcing, etc.) can be included in the state equation without much problem. The noise vectors wk and Vk are both a sequence of zero-mean white Gaussian noise, with covariance Wk and Rk and mutually independent: E[WkWk'] = Wk (3.3) E[vkvk] = Rk (3.4) E[wkvkJ (3.5) = 0 In this context, the system matrices and noise covariances are all assumed known and can be time varying (Ak, Hk, Wk, and Rk). In addition the initial state is a modeled as a Gaussian random variable and is independent of both the process and measurement noise. The two values being computed by the algorithm are the state estimate and covariance. Keeping in mind that due to the stochastic nature of the problem the state 30 is a random variable, then the estimate is defined as: k~k (3.6) E[xkIZk] where: (3.7) Zk A {zIz2 ,...,zk} is the sequence of measurements up to and including time k. Specifically, the estimate of the state at time k is the mean of the random state conditioned on the past measurements Zk. The other value of interest, the covariance, is essentially the uncertainty associated with the estimate of the state. If the state estimation error is defined as: Xkjk Xk - (3.8) XkIk then the covariance associated with the estimate is defined: Qkjk A E[(xk - -k k)(Xk - 4k )T|Zk] (3.9) Thus, the KF seeks to propagate forward the statistics of the estimate given stochastic knowledge of the system, and new information in the form of stochastic measurements of the system. Put another way, at time index k there is an estimate 'kIk with uncertainty Qkjk based on measurements Zk. Then, at time index k + 1 filter receives a new measurement Zk+1. Through a simple set of matrix equations, the filter is able to produce the new optimal estimate .k+11k+1 and uncertainty Qk+l|k+1, and can do so recursively for future time steps. The underlying principles behind the Kalman filter can be applied to situations that do not meet the many assumptions made in deriving the optimal linear filter. Tools like the extended Kalman filter or unscented Kalman filter make extending the recursive nature of the Kalman filter to nonlinear systems possible. 31 3.1.1 The Kalman Filter Recursion Equations For a detailed derivation, refer to [3]. First, starting with the state estimate :kk and covariance QkIk at the current time step k propagate the statistics forward using the state equation: ik+l|k = A:kk (3-10) Qk+11k = AQkIkA T + W (3.11) then, calculate the optimal gain, measurement innovation, and innovation covariance: Vk+1 = Zk+1 - H-k +11k (3.12) Sk+1 = HQk+l1 kH T + R (3.13) Lk+1 = Qk+llkH TS-l (3.14) finally, update the state and covariance using the new information contained in the measurement: k+1|k+1 = &k+1|k Qk+llk+1 = (3.15) + Lk+lvk+1 (I - Lk+1H)Qk+llk This represents one cycle of the Kalman filter recursion. (3.16) Note, that the system matrices and noise covariances could still depend on time k but was left time-invariant for notational simplicity. 3.2 Multiple Model Estimation There are several ways to derive estimators in a multiple model sense. As mentioned in Chapter 2, it is possible to derive linear sub-optimal estimators 17, 24], or opti- mal estimators based on particle filtering methods [8, 9, 20]. In general, there exists trade-offs between filter complexity and computational load, and often, sub-optimal 32 estimators perform well enough in terms of accuracy, while incurring much less computational burden [1]. General Formulation 3.2.1 Consider the linear system given by: Xk+1 = Zk where Xk A(Mk)xk + w(k, Mk) = H(Mk)xk + v(k, Mk) is the nx-dimensional state vector and vector; Mk E {1, 2, . . , r} Zk k = 0, 1, 2, ... (3.17) k = 1, 2, ... (3.18) is the n,-dimensional measurement is one of r discrete modes active at time k. As before, the noise statistics on w and v are Gaussian, zero-mean, white, and uncorrelated with each other. w(k, Mk = j) ~ j(O, Qj) (3.19) v(k, Mk = j) ~ .(0, (3.20) Rj) The transitions between modes is governed by a Markov chain. In the standard derivation, probabilities governing the transitions are assumed to be constant: 7rij = P{Mk+1 = jIMk = i} (3.21) where the mode history, or sequence of modes is: Mk, = [M1 , M2 , ..., Mk] (3.22) ,Mk E {1,2, ... , }(3.23) M1, M2, .... l = 1,2,...rk (3.24) and is exponentially growing in time as a function of rk. Consider that the mode history given above is a mutually exclusive, collectively 33 exhaustive set of events. Employing the total probability theorem gives the pdf of the state conditioned on the measurements, or the state estimate, produces a Gaussian mixture with exponentially increasing complexity[3]: p[xk|Zk] = E p[xk|M k'1, Zk]P{Mk'I|Zk} (3.25) 1=1 where the second term is the probability of a given mode history, and can be calculated by Bayes rule [31: (3.26) P{Mk'l|Zk} = P{Mk'I|Zk-1, zk} = p[zk|jMk, Zk-1]P{'MlkIZk-1} p[zkI Zk-1] = = 0p[zk| Mk'I, Zk-1]P{AMk-l's, Mk|Zk-1} (3.27) (3.28) C 1 p[zk lMkl, zk-1]P{MkImk-1's, Zk-1}P{Mk-1'sjZk-1} (3.29) C where if, as assumed above, the Markov chain is only dependent on the previous mode becomes: P{Mk'l|Zk} - -p[zk|Mk', Zk-1]P{Mk|Mk_1}P{Mk-1's|Zk-1} C (3.30) This, combined with Equation 3.25, show that the optimal estimator requires conditioning on the entire mode history, even in Jump Markov systems [3]. This issue is easily handled by using some form of sub-optimal estimator, which as discussed in Chapter 2 is often the interacting multiple model (IMM) algorithm. 3.2.2 The Interacting Multiple Model Estimator One of the key pieces to performing multiple model estimation in a practical sense is some form of sub-optimal estimator. In the recent past, the interacting multiple model estimator has emerged as one of the best options for state estimation of hybrid systems [1]. At its core, the IMM estimator is simply a bank of filters (often Kalman 34 filters), each matched to a separate dynamical mode of the system. Through the clever use of a Gaussian mixture approximation, the algorithm is able to perform with O(r) complexity, as opposed to the O(rk) complexity of the optimal estimator. The IMM algorithm can be broken down succinctly in three distinct steps [11: 1. Mixing: Each filter in the bank calculates a state estimate and covariance separately. At the start of the recursion, these mode-conditioned estimates are "mixed" in a Gaussian sense using the discrete mode probabilities. 2. Filtering: Each mixed estimate is passed to the corresponding filter to produce an appropriate mode and measurement conditioned estimate. 3. Mode Probability Update: Based on the output of each filter and the transition probabilities the discrete mode probabilities are updated Consider the system outlined in Equations 3.17-3.20, where the superscript j E {1, 2, ... , r} denotes the discrete mode. Let the discrete mode probability at time k be: = P{Mk = J} (3.31) S = 1 k = 0, 1, . . (3.32) 14 r j=1 The following represents one recursion of the IMM filter from time index k -I to k, in the case where the mode transition probabilities are constant as in Equation 3.21. At the start of the recursion, the previous filter estimate for each mode . _-|l_1 and covariance Qilk-1 along with the discrete mode probabilities Pi_ 1 are available. First, calculate the predicted mode probability using the Markov transition: Il-i_ =i Z} 1 = P{M (3.33) then the IMM mixing probability is given by: pili A P{M_ 1 IM ,Zk-} = Iit 35 i (3.34) and each filter in the bank can then be initialized as: ZQ-1k-1 -l|k-1 k ilk -1k- kllk-1 T k1k-1 (3.35) -11k-11.' klk- k The initial values in Equations 3.35-3.36 are passed to the bank of j = 1, 2, ... ,r filters. For instance, the Kalman filter outlined in Section 3.1.1 can be used if the system is linear. In any case, the filters each produce their own estimate, covariance, innovations, and innovation covariance: kik, Qilk, , v, and Si. To update the mode probabilities, a likelihood function based on the innovations is formed as: Ai = (vA;0, S') =27rS Iexp(-1/2vj' S'v') (3.37) and the mode update becomes: ^3 = k A3 -1 iklk-lAi (3.38) Although the recursion does not explicitly calculate, or in fact need, the combined state estimate and covariance, it is now possible to compute it as a simple weighted Gaussian: Xk4k klkllk =Z (3.39) ii 3.2.3 Continuous-State-Dependent Mode Transition Probabilities In addition to the complication of increasing histories, consider the case where the mode transition probabilities are not constant as in Equation 3.21. Blom and Bloem investigated the exact Bayesian filter in the case where the mode transitions depended 36 on the continuous state [8]. Consider the more general system of stochastic difference equations: Xk = a(Xk-1, Mk, Wk) (3.41) Zk = h(xk, Mk, Vk) (3.42) Mk = c(Mk1, Xk_1, uk) (3.43) where (Xk, Mk) is now the hybrid system state, and a,h, and c represent general measurable mappings. Filtering,, in general, has the goal of finding the optimal estimate of a random variable. Therefore, in this problem, the goal is to find the density of the joint state (Xk, Mk) conditioned on the measurement sequence Zk: p[Xk, MkIZkI. From Equation 3.43 the current mode Mk is now defined as a function of the previous mode Mk_1, the previous state Xk_1, and the noise sequences {uk}, {Wk}. Therefore, the process {Mk} no longer represents a Markov chain. However, it can be shown the the joint process {Xk, Mk} satisfies the Markov property [8]. In their paper, Blom and Bloem derive the exact representation of the conditional density of the joint state. First, the state-dependent mode transition probabilities can be defined as: Irij(x) = p[Mk = iIMk_1 = i, Xk1] H(x) = {rij(x)},=1,...,r (3.44) (3.45) then, the joint distribution can be found as [8]: p[xk, MkZk ] = p[zklxk, Mk] Sj p[klxk-1, Mkl Z[H(x')p[xkk-1, Mk_1|Zk-']dx/ct j=1 (3.46) Here, Blom and Bloem apply particle filtering methods in conjunction with IMM like approximations to form an IMM based particle filter (IMM-PF) [8]. 37 However, in practice it is sometimes more desirable to have a simpler filter for computational and implementation concerns. As mentioned in Section 2.4 Seah and Hwang investigated the special case of linear filtering with state-dependent transition probabilities [31, 32]. Consider the system of linear stochastic equations given in 3.17-3.20, along with the state-dependent mode transition probabilities defined in 3.44-3.45. Seah and Hwang show that in this case, there are two challenges with implementing a filter [32]. First, the continuous state can be calculated by: Z p[xkIMk,1 p[xkIZk] = , Zk]P{Mk'I|Zk} (3.47) 1=1 which as discussed in Section 3.2.1 is plagued by the issue of exponentially increasing complexity, and can be approximated effectively with the IMM algorithm. Second, the update involved in calculating the conditional state transition probabilities, given by 1311: p[Mk = jIMk-1', Zk-1] = j 7rij(X)P[Xk-1 = xIMk-1', Zk-l]dx (3.48) which as outlined in Section 2.4 presents problems in obtaining an analytical solution. Seah and Hwang tackled this problem in several ways. First, they utilized a brute force MC integration method [31]. Second, they present two approximations for the conditional mode transition probabilities; a Gaussian pdf: 7rij (x) = aij + bijA/(Lijx; pi, Eij) (3.49) = aij + bij Dq(Lij x; pij, Eig) (3.50) or Gaussian cdf: 7rij(x) which proves useful in deriving a specific case of state driven mode transitions. They cite examples of both positional based waypoints in tracking (pdf), and inequality "fcut-off" based limits in manufacturing (cdf) [321. 38 Chapter 4 Case Study: Noise Bounds for Pitch and Roll Estimation using Attitude Heading and Reference Systems The operation of a low cost attitude and heading reference system (AHRS) aboard a fixed wing aircraft was derived and simulated. Parts of this section were presented in a term paper for 16.322: Stochastic Estimation and Control 118]. An AHRS is used to estimate the roll, pitch, and yaw of an object relative to a fixed reference frame. This estimate is formed by filtering noisy measurements taken with accelerometers, gyroscopes, and other sensing devices. These systems are used in a variety of applications, ranging from large scale navigation and control problems such as aircraft, to small scale applications such as camera stabilization and small unmanned vehicles. Typically, the components used in an AHRS are very expensive. However, advancements in micro electro-mechanical system (MEMS) technology has provided access to smaller, more affordable sensors. These low cost micro sensors are often be characterized by low accuracy and noisy outputs, so it is necessary to provide some correction, and understand the limitations of the estimated state. Also, using only inertial measurements from gyroscopes will lead to increasing error over time, as the noisy measurements are integrated to form the state estimate. To combat this estimate "drift," measurements from other sources must be added 39 to the MEMS measurements of body rates and accelerations. For an aircraft, these could include: airspeed, altitude, GPS, magnetometers, etc. Given data available from multiple sensors, each with their own noise characteristics, some technique is needed to combine the information into an estimate of the state. In this case, due to structure of the nonlinear dynamics, The fusion of measurements from multiple sources, each with distinct noise characteristics, is done through the use of an Extended Kalman filter. Here, the estimate under consideration is the roll and pitch of an aircraft. Four measurement devices are used: a gyroscope, an accelerometer, an airspeed sensor, and an altimeter. The goal is to evaluate the effect that different levels of sensor noise has on the estimate of the state. Ultimately, it would be useful to provide a bound on the gyroscope noise variance and drift that keeps the roll and pitch angle errors under 5'. This would be useful for informing the selection of instruments used in the inertial measurement unit (IMU) of the AHRS, specifically when considering the costs of different instruments. The following demonstrated some of the practical problems involved in implimenting a filter, especially when considering non-linear dynamics. Several useful noise characterizations are presented, along with common quantifiable outputs and measurements of a filters usefulness. Variables and Coordinate Definitions 4.1 Throughout the chapter, the variable definitions in Table 4.1 will be used. u v w 4 0 0 Var a 3 Table 4.1: Standard definitions of relevant variables p Body rate about the x-axis Airspeed in the body x-axis q Body rate about the y-axis Airspeed in the body y-axis r Body rate about the z-axis Airspeed in the body z-axis fx Accelerometer reading in the x-axis Roll angle fy Accelerometer reading in the y-axis Pitch angle fz Accelerometer reading in the z-axis Heading h Altitude Airspeed dt Resample time Angle of attack y Climb angle Side slip angle 40 C: North center of gravity Xu ZN South 9Earth Figure 4-1: Body fixed reference frame (left), and inertial reference frame (right) [231 Several reference frames are useful when describing and sensing the motion of an aircraft. This is because it is often easier to express different motions in different frames. For instance, it is typically easier to understand the attitude, or orientation, of an aircraft relative to some fixed reference. Typically, for aircraft applications coordinate systems are expressed in two categories: "body"-frame coordinates and "internal"-frame coordinates. The body frame is fixed to the aircrafts center of gravity, and moves with the aircraft as it translates and rotates. The inertial frame is defined relative to a fixed point, and does not change with time. Figure 4-1 shows the body fixed reference frame YB on the left, which is attached to the aircrafts center of gravity. The x-axis pointing out the nose and the z-axis points down, both along the aircrafts planes of symmetry. The y-axis forms a right-handed orthogonal system. The accelerometer and gyroscope both take measurements in this frame. Also, Figure 4-1 shows the inertial North-East-Down (NED) reference frame YN on the right. If a flat-Earth assumption is made, the XN and YN axes form the local horizon, with the x-axis pointing North, and the y-axis pointing East. The z-axis points down, towards the center of the Earth. Additionally, assuming the Earth is perfectly spherical, the gravity vector will point along the z-axis in the NED reference frame, g9 = [0 0 g] . The roll, pitch, and yaw are defined relative to this frame in that they represent a rotation between the NED frame and body frame. 41 In fact, it is a simple process to move between the two reference frames using a standard coordinate transformation. To transform from the NED frame to the body frame a sequence of rotations is performed: CI 1 0 Cq 0 cos# sin# 0 1 0 0 -sin# 0 -sin 0 cos 0 0 cos# (4.1) = Cx(#)Cy(O)Cz(P) cos0 0 -sinO cos@ sing' 0 0 cos0 0 0 1 (4.2) sin0 CB = ( ) (4.3) were it can be seen that the pitch and roll are directly related to the orientation of one frame with respect to the other. 4.2 Sensor Characteristics For this problem, four sources of measurements were considered: a gyroscope, accelerometer, airspeed sensor, and altimeter. A MEMS gyroscope is used to take measurements of the angular velocity about a set of body-fixed axes. Typically, a set of three sensors will be used on mutually orthogonal axes, providing measurements of the rates about each axis. There are many different classes of gyroscopes, covering a wide spectrum of accuracy and pricing. In order to design a low cost AHRS package, it is desirable to use a less expensive gyroscope, which will often be characterized by larger error. Typically this error can come from a variety of sources, such as bias, scale factor error, misalignment, and random noise. For simplicity, and because of simulation concerns, only the random noise and bias terms are considered in the analysis. The measurement taken by the gyroscope (pm) is modeled as the sum of the true state (p), the time varying bias in 42 the gyroscope (bp) and a stochastic white process (Ep): PM p + bp +Ep qm = r q+bq+Eq (4.4) r + br + Er] where the bias drift is modeled as a random walk process: (4.5) bi = Ebi To acquire the pitch and roll of an aircraft, the body rates must be integrated. Due to the error in measuring these rates, the error in state estimation will tend to grow over time. This leads to the requirement of additional sensor information to properly correct the state estimates. An accelerometer is a device that measures acceleration. It typically consists of three distinct sensors on orthogonal axes in order to measure acceleration in three dimensions. The output of the accelerometer is the true vehicle acceleration minus the gravitational acceleration: fm = a - g (4.6) The measurement obtained from the accelerometer is treated as the true value with additive white noise: f . fX + Ef1 fy[ fy + E fz + [z" (4.7) EfzJ Lastly, there are two additional sensor readings typically available on an aircraft. First, true airspeed is measured using a pitot-static probe or similar device. Again, this is measurement is modeled as the true value plus additive white noise. Similarly, the altitude can be measured through pressure, and is also modeled as the true value plus additive white noise: Va = Va + Eva 43 (4.8) hm = h+Eh 4.3 (4.9) The Extended Kalman Filter As detailed in Section 3.1, the Kalman filter is a tool used to estimate the state of a linear process given the presence of uncertainty in the process and measurements. The observations of a system are often incomplete, indirect, intermittent, and inexact, and the KF has ways of handling this [101. The filter forms estimates by propagating statistics through time and minimizing the variance of the error. The extended Kalman filter (EKF) is an extension of the KF to non-linear systems. The filter used for estimating the pitch and roll follows the EKF with a couple assumptions made in order to reduce computational time. A general continuous-time state space model with discrete measurements for nonlinear systems is: J(t) = f (x(t), u(t), t) + G(x(t), t)w(t) (4.10) z(tk) = h(x(tk), u(tk), tk) + v(tk) (4.11) where w(t) and V(tk) are mutually uncorrelated white noise sequences with covariance's W(t) and R(tk) respectively. First, the state and covariance is propagated according to the process equation. To do the one-step prediction, Euler's formula is used to perform the integration of the nonlinear state. This reduces the computational load significantly: Xk+11k = XkIk + f(x(tk), u(tk))dt (4.12) Then, the system is then linearized: F(tk) = Of af (4.13) and discretized with the following approximations, again to reduce computational 44 load: I + F(tk)dt + 0.5(F(tk)dt)2 (4.14) (tk+1, T)G(T)Q(r)GT (T) 1 T (tk+l, T)dT (4.15) = eF(tk)dt Wdk = k 'k W + G(tk+1)W(tk+1)G T (tk+1)) ~0.5dt('IkG(tk)W(tk)GT (tkT (4.16) The one step covariance prediction follows: (.17 Pk+1k = 4kPkIk k + Wdk Second, the predicted values are updated with the measurements of the system. First, the measurement equation is linearized: Hk+1 = ax-h (4.18) ,k+1jkU(tk+1) The Kalman gain: Lk+1 = Pk+1kH T+1[Hk+1Pk+1IkHkT+l + Rk- 1 (4.19) Finally, the updated state and covariance are given by: k+1k+1 = k+1k + Lk+1(zk+1 Pk+1lk+1 = 4.4 (I - - h(Xk+llk, U(tk+1))) Lk+lHk+1)Pk+llk (4.20) (4.21) Process Model The states of interest to this estimation problem are the pitch and roll of an aircraft. As outlined, measurements for the body angular rates are available from the gyroscope readings. Through kinematics, the body angular rates can be related to the pitch and 45 roll rates by: S 1 sin 0tan 0 cos 0tan 0 0 cosq$ -sinG 1 q (4.22) There are two main issues of note. First, it is safe to ignore the yaw angle and yaw rate since the dynamics do not depend on either. Second, the state equations include the values of body rates, which are directly being measured. However, the measurements of body rates depend both on white noise and a time varying bias term. In general, this bias will not be observable, and it would be necessary to estimate it. Therefore, the bias terms are added to the state vector. In addition, the dynamics of the altitude is easy to model which makes it a convenient state variable: 6 b, x(t) (4.23) b, br h] (4.24) u(t) = [Pm qm rm Va] - 6q)sin ptan&+(rm (pm - bp - cp)+(qm -bq - br - 6r)cosotan9 6 (qm - bq - 6q)cos - (rm- br -Er)sin# bp Ebp bq Ebq br Ebr h usinG -w sin cos0 -w cos sin0 (4.25) where the body velocities U V ] T can be related to the current state variables and measurements as detailed in Section 4.5. 46 The general state space model in Equation 4.10 becomes: (Pm - bp) + (q, - bq) sin 0 tan 0 + (r, - br) cos 0 tan 9 (qm - bq) cos 0 - (r, - b,) sin# 0 f(x(t), u(t),t) = (4.26) 0 0 Va sin 9 cos 0 - Va cos -1 G(x(t), t) = w(t) = -sinocos9 -cosotan9 # cos 9 sin 0 0 0 0 0 0 -cos# sin9 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 ,W =diag({o2 0, Cbq (4.27) (4.28) r2 ' r70 2, , 921) Ebr Ebp Ebq 4.5 Measurement Model The body frame accelerations are measured through accelerometers. The body frame accelerations can be related to the body velocity rates of change, body velocity, angular rates, roll, pitch, and gravity through: fx qw - rv [ = fy fz B i t> + ru -pv pv - qu 47 1 -sin - cos0 sin# cos 9 cos 4 g (4.29) Measurements for the airspeed are also available, and the airspeed can be related to the body velocities through: [ cos a sin 3 v (4.30) sin Va sin acos . w For the purpose of modeling the filter, it is assumed that the side slip angle is small, 0 0, the pitch angle is approximately the angle of attack 0 ~ a, and the time rate of change of the airspeed is small V ~ 0. With these assumpti ons, the body velocities and body velocity rates are given by: cos0 u Va v 0 sin 0 w'v [l -0cos0 0 ,j=Va Low sin 0 I (4.31) As previously outlined in Equation 4.24, the airspeed is treated as an input. The measurement vector is given by: T Z(tk) = [f,, fm fzm (4.32) hm Equations 4.29-4.31 can be put in the general form described in Equation 4.11. h(x(t), u(t), t) = Va((r, Va((qm - bq) cos - - (r, - br) sin #) sin 0 br) cos 0 - (p, # - (r, - + Va(q - bq) sin 0 + g sin 0 bp) sinG) - g cos 0 sin # -Va((q m - bq) cos # - br) sin 0) cos 0 - Va(qm - bq) cos 0 - g cos 0 cos q h (4.33) 48 Ea. v(k) =Ea , R = diag([, r , a or2]) (4.34) Eaz LEh 4.6 Simulation Model In order to test an estimation scheme, it is necessary to compare the estimate to some form of "truth". One of the characteristics of aircraft estimation is that it is no suitable way to directly measure the true state accurately. Often, when developing new sensors or estimation algorithms, the estimates are compared to more accurate sensor outputs. Another approach is to obtain a representation of the true state through simulation. Three distinct simulation cases were considered. In each, the trajectory of an aircraft was generated over 290s. In the first two, the simulation matched the model exactly. That is, steady flight with no side slip and no climb angle was considered (Va = 0, y = 0, and a = 0). In this case, the pitch and roll angles were chosen, and the yaw angle and rate were calculated through: sin# = (4.35) Integration was done numerically using the trapezoidal rule. The roll and pitch rates we found through numerical differentiation using simple finite difference approximation to reduce computational load. The first model used piecewise linear functions for the pitch and roll, having one section where the aircraft rolled at a fixed angle, and one section where it pitched at a fixed angle. The second model used a general spline through 24 reference points for both the pitch and roll. Model 1 is shown in Figure 4-2 (a), and Model 2 in Figure 4-2 (b). Through use of the kinematic and dynamic equations introduced earlier, the angles 49 (b) (a) 20 00 50 ---- 100 w 20 - - - --- - - - --------- ---200 250 300 . - 1 0 ..----- ...-- ---... -----... --. -..--..... 50 100 150 100 50 ------- 200 0 250 0 50 200 150 300 100 200 250 300 200 250 300 -..... - - --. -200 0 Time [s] 150 Time [9] -- tM 0 100 250 - ---------- .20 300 it20 50 200 20 ........................................................ Tim. [a] 0 10 rime Is) ---20 -------~1o------- S0 0 150 Tim. [s] - - 3 50 100 150 Time [s] 200 250 300 Figure 4-2: Simulated roll, pitch, and yaw for (a) Model 1 and (b) Model 2 were used to compute "truth" for the generated quantities: &V fy -- .- v. q - W r ] , 7i (4.36) fz The third, and final, trajectory relaxed two of the main modeling assumptions by allowing a non-zero side slip and climb angle. The pitch, roll, and yaw trajectories were kept the same as Model 2. However, since a non-zero side slip and climb angle were added through a similar spline procedure, this causes the aircraft is no longer undergo steady coordinated flight and changes the body velocities in Equation 4.30: cos a sin Va [wj sin3 sin a cos #J (cos 9 cos y + sin 9 cos -y) sin sin /3 = Va (4.37) (sin 9 cos -y - cos 9 sin -y) cos # rul This, along with its time derivative, can be used in Equation 4.29 to calculate the "true" body accelerations. 50 (a) S 51 ----------- . ................ K .. -------- 0 - - 0 . 0 ----------- ----------- I-------- ------------ -------------- ---------- -2 Ca _'6 50 100 150 Time [s] 200 250 50 300 50 100 150 200 250 300 Time [91 Figure 4-3: Simulated side slip and climb angles used in Model 3 4.7 Results The ultimate goal is to find the maximum permissible noise values on the measurements instruments that keeps the error under 50. As a starting point, some realistic values for sensor noise and initial conditions were taken from [23]. It was assumed that the accelerometer has the same noise statistics for each axis, and that each axes was uncorrelated. A similar assumption was made for the gyroscope. The filter noise statistics were then "tuned" using the simulation Model 2 until either the pitch or roll errors were close to 50. Three different versions of the filter were tested. In the first case, no bias or altitude measurements was included. The second filter added bias to the rate measurement. The final case included both bias and measurements of the altitude. 9a -gy Ub Table 4.2: Noise in the simulated sensors Starting Point Tuned Values 0.5 m/s 2 0.2 M/s 2 Accelerometer noise 0.3 o /2 0.05 o /s2 Gyroscope Noise 211 o /hr 211 o /hr Bias rate variance ov. Airspeed noise 0.6 m/s 3 m/s Uh Altitude noise N/A 20 m 0 0 0 (4.38) 51r 57r 57r 27r 27r 27r 180' 180' 180' 180' 180' 180 (4.39) oo[ 0 0 Two main design choices were changed during the construction of the filter. First, 51 (a) TuS tate 0- - Estimated State so 0 100 IO ime [a] (b) 200 250 Estimated State' lo- 050 300 100 150 'ime [s) 200 250 300 Figure 4-4: Estimation divergence for the (a) designed filter, under the same noise conditions as the (b) original filter the filter was originally designed without the inclusion of gyroscope bias. Performance suffered in terms of error. In addition, this did not really model the physical problem well. Although the addition of bias as a random walk process to the state variables increased the accuracy in terms of error, it also produced the phenomenon of trajectory divergence in the roll state. An example of this is shown in Figure 4-4, where (b) shows the trajectory for the original filter without bias (a) shows an untuned filter with bias included. This stresses the importance of properly testing any non-linear filter, especially in a Monte-Carlo sense. The observed divergent trajectory occurred 1/10 times for different sets of measurements. Interestingly, Figure 4-4 (b) also shows that not having bias in the measurements and process state produced lag in the estimate. The second main design choice was made to counter the issue of roll error divergence after it was observed in the estimate. To accomplish this, another type of measurement was added to the system in the form of altitude. A comparison of the filter with and without altitude measurements included, using the final tuned noise values in both cases, is shown in Figure 4-5. Figure 4-5 (b) shows that given the same noise parameters, a lack of altitude measurements was most likely the cause for filter divergence. This makes sense, since the rate of change of the altitude is a function of both pitch and roll under the assumptions made, and so indirectly provides 52 (a) 10 5- 50 0 100 150 200 250 300 200 250 300 Time [s] (b) - 40 - Z20 -20- 0 50 100 150 Time [s] Figure 4-5: Monte Carlo simulation for (a) filer with altitude measurements and (b) without altitude measurements, for the same sensor noise parameters more information for the estimator to use. Interestingly, the divergence amplitude can be reduced, or all but eliminated, by adding more measurement noise to the accelerometers and gyroscopes, but at the cost of increasingly large raw estimation error. Several relevant metrics for the final tuned filter are summarized in the following figures and table. The filter was run for fifty iterations of measurements generated from the same "truth". Table 4.3: Error statistics for the Monte Carlo simulation 2a Maximum State Error Mean # 0 Roll angle Pitch angle 0.1560 0.0080 2.5150 0.0977' 4.1860 0.1710 It was decided that the maximum estimation error should be bounded. Inter- estingly, In the choice of noise parameters there is some freedom. For instance, the gyroscope error could be increase, while the accelerometer error could be decreased, and still result in a 50 bound on the error. For the actual selection of low cost sensors, a cost/benefit analysis would have to be done on real values for the noise. Finally, the third model detailed in Section 4.6, that is, the one with side slip and climb angle added to the trajectory was passed through the "tuned" filter. This is 53 (a) 20 - 10 - - True State Estimated State a S-10 so 100 150 Time [s] 300 250 200 (b) 15 - 10 - True State Estimated State 5 cc 0 C -5 ) -I _0 50 100 150 Time [s] 200 300 250 Figure 4-6: Sample trajectory for one of the fifty iterations (a) 61 4 I _ 0 -2 50 100 150 Time [s] (b) 300 250 200 0.2r- w 0.1 0 I~ Vi -0.1 Y - Z 50 100 150 Time [s] 200 250 Figure 4-7: Mean of the roll and pitch error for the fifty iterations 54 300 (a) 0 0 -0 0 150 150 Time [s] (b) 200 200 250 250 50L 100 160 Time [s] (c) 200 250 3 00 50 100 150 Time [s] 200 250 3 00 F00020 0 0 -10 3 300 100 50 3 - U,0.5 0 C -0. 60 (d) - 150 " 1 -500 50 ' 0 __ IAPO 100 150 150 Time [9] 200 20 250 250 Figure 4-8: Mean innovations sequences for the fifty iterations 55 3 3 00 a case where the filter model does not exactly match the real trajectory. The error plots look similar to the figures presented for the base case, but with slightly more noise. The statistics of the error are given in Table 4. Table 4.4: Error statistics for miss-matched model Maximum 2State Error Mean 6.6670 0.1560 5.8860 <$ Roll angle 0.6000 0 Pitch angle 0.0080 0.215' In this case, there is more than 50 of error in the 95% interval of the roll angle. All things considered, this side slip and climb angles considered in Model 3 were fairly small, but they still drove the error in estimation higher. This suggests that perhaps a more robust model would better suit the problem for general trajectories. Or maybe not, if the aircraft in question is expected to fly on a coordinated trajectory. A less tightly "tuned" filter could also be considered, to reduce error in this type of uncoordinated flight, at the expense of more accurate sensors. 4.8 Summary The roll and pitch of an aircraft was estimated using an EKF. Four measurement devices are used: a gyroscope, an accelerometer, an airspeed sensor, and an altimeter. The goal is to evaluate the effect that different levels of sensor noise has on the estimate of the state, and demonstrate the practical implementation of a non-linear dynamic filter Ultimately, a bound on the gyroscope noise variance and drift was given that keeps the roll and pitch angle errors under 5'. This process should be useful for informing the selection of instruments used in the inertial measurement unit (IMU) of the AHRS, specifically when considering the costs of different instruments. Several issues arose during the implementation of the filter. First, it was interesting that a lack of sufficient measurements could lead to divergence in the non-linear filter. This was most likely due to the integration error not being properly corrected in the filter, either through lack of measurements, or improper noise selection (i.e. measurement trust vs. prediction trust). 56 Second, several assumptions were made on the model during the filter design process. It was shown, that when even when the true process deviates slightly from the assumptions, the filter still functions, although not quite as well. This is fairly standard for miss-modeling a problem, but was interesting nonetheless. Moving forward, it would be interesting to consider a slightly more complicated model that captures the effects of climb angle and side slip, maybe in terms of a multiple model like filter. Also, it might be interesting to add in more sources of measurements, such as GPS or magnetometers to see if the estimation error can be reduced. 57 58 Chapter 5 Conclusion 5.1 Discussion Throughout the thesis, the concept of estimation with regards to stochastic hybrid systems was thoroughly investigated. A broad outline of the topics of estimation and multiple model based systems was provided, along with several realizations of practical filters and algorithms. A literature review of the concepts underlying the development of stochastic hybrid estimation from early studies and filters, to more advanced concepts was provided. Specifically, the often made assumption of a time invariant transition probability matrix, which classifies a system as jump Markovian was looked at. Recent work relaxing this assumption was explored, from the concept of semi-Markov jump systems, to more advanced Bayesian realizations and particle filter implementations of continuous-state-dependent mode transition probabilities. More practical linear filters using Gaussian assumptions and continuous-state-dependent mode transitions were also outlined, along with the concept of a finite set of transition probabilities used to model time-invariance. A brief outline of the derivations and equations needed to implement both simple Kalman filters and more advanced extended Kalman filters was provided. In addition, a general framework for multiple model estimation was provided, along with the equations needed to implement an interacting multiple model algorithm in practice. Also, some of the challenges involved in the realization of a continuous-state-dependent 59 mode transition probability matrix were discussed, including the multivariate integral that arises from the derivation. Two approximations to the mode transitions in the form of Gaussian pdfs and cdfs were provided. Finally, a case study on the implementation of an extended Kalman filter algorithm for the application of attitude heading and reference systems was provided. This highlighted several issues that can appear when using non-linear filters, including divergence of the estimate, and the usefulness of additional sources of information in the form of measurements. It was also shown that if the true system process deviates from the model used in the filter, additional problems can manifest. 5.2 Future Work Many avenues exist for extending the concepts presented in this thesis. One interesting area is to explore additional modeling problems, and implement time varying transition probabilities in other systems and applications. Also, several other concepts often explored in the framework of filtering such as smoothing and prediction deserve further attention. In addition, the underlying nature of the non-constant mode transition probability matrix deserves further consideration. Here, only two concepts of sub-optimal implementations were presented; continuous-state-dependent mode transition probabilities, and time invariance in the context of a finite set of possible mode transition probabilities. 60 Bibliography [11 Y. Bar-Shalom, S. Challa, and H. Blom. IMM estimator versus optimal estimator for hybrid systems. IEEE Transactions on Aerospace and Electronic Systems, 41(3):986-991, 2005. [2] Y. Bar-Shalom, X. Li, and T. Kirubarajan. Tracking and Data Association, volume 179 of Mathematics in Science and Engineering. Elsevier Science, 1 edition, 1988. [3] Y. Bar-Shalom, X. Li, and T. Kirubarajan. Estimation with Applications To Tracking and Navigation. John Wiley & Sons, Inc, 2001. [4] L.F. Bertuccelli. Robust multiple model filtering with uncertain transition models. AIAA Guidance, Navigation and Control Conference and Exhibit, 2008. [51 J.D. Birdwell, D.A. Castanon, and M. Athans. On reliable control system designs with and without feedback reconfigurations. In Decision and Control including the 17th Symposium on Adaptive Processes, 1978 IEEE Conference, pages 419426, San Diego, CA, January 1979. [6] H. Blom. Hybrid state estimation for systems with semi-Markov switching coefficients. Proceedings of the 1st European Control Conference, Grenoble, pages 1132-1137, 1991. [7] H. Blom and Y. Bar-Shalom. The interacting multiple model algorithm for systems with Markovian switching coefficients. IEEE Transactionson Automatic Control, 33(8):780-783, 1988. 18] H. Blom and E. Bloem. Exact Bayesian and particle filtering of stochastic hybrid systems. IEEE Transactions on Aerospace and Electronic Systems, 43:55-70, 2007. [9] H. Blom and Bloem E. Particle filtering for stochastic hybrid systems. In In Proceedings of the 43rd IEEE Conference on Decision and Control, pages 32213226, Atlantis, Paradise Island, Bahamas, August 2004. [10] W.L. Brogan. Modern Control Theory. Quantum Publishers, 1974. 61 [111 L. Campo, P. Mookeriee, and Y. Bar-Shalom. State estimation for systems with a sojourn-time-dependent Markov switching model. IEEE Transactions on Automatic Control, 36(2):238-243, 1991. [121 A. Doucet, N. Gordon, and V. Krishnamurthy. Particile filters for state estimation of jump Markov linear systems. IEEE Transactions on Signal Processing, 49(3):613-624, 2001. [13] P. Hanlon and P Maybeck. Multiple-model adaptive estimation using a residual correlation Kalman filter bank. IEEE Transactionson Aerospace and Electronic Systems, 36(2):393-406, 2000. [14] A. Hero, D. Castanon, D. Cochran, and K. Kastella. Foundations and Applications of Sensor Management. Springer-Verlag, 2008. Hybrid estimation of complex systems. [15] M. Hofbaur and B. Williams. IEEE Transactions on Systems, Man, and Cybernetics-Part B: Cybernetics, 34(5):2178-2191, 2004. [161 L. Hong, N. Cui, M. Bakich, and J.R. Layne. Multirate interacting multiple model particle filter for terrain-based ground target tracking. IEEE Proceedings on Control Theory and Applications, 153(6):721-731, 2006. [171 I. Hwang and C. Seah. An estimation algorithm for stochastic linear hybrid systems with continuous-state-dependent mode transitions. In In Proceedings of the 45th IEEE Conference on Decision and Control, San Deigo, CA, December 2006. 1181 Michael Kasperski. Noise bounds for pitch and roll estimation using attitude heading and reference systems. 16.322 Stochastic Estimation and Control: Term Paper, 2014. [19] T. Kirubaraian, Y. Bar-Shalom, K.R. Pattipati, I. Kadar, B. Abrams, and E. Eadan. Tracking ground targets with road constraints using an IMM estimator. In IEEE Aerospace Conference, volume 5, pages 5-12, Snowmass at Aspen, CO, March 1998. IEEE. [201 X. Koutsoukos, J. Kurien, and F. Zhao. Monitoring and diagnosis of hybrid systems using particle filtering method. In Symposium on Mathematical Theory of Networks and Systems, 2002. [21] X. Li and Y. Bar-Shalom. Design of an interacting multiple model algorithm for air traffic control tracking. IEEE Transactions on Control Systems Technology, 1(3):186-194, 1993. [22] Y. Long and G-H. Yang. Fault detection for a class of nonhomogeneous Markov jump systems based on delta operator approach. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 223(1):129-141, 2013. 62 [231 H. Lopes. Attitude Determination of Highly Dynamic Fixed-wing UAVs A MEMS-AHRS/GPS Integration. Masters Thesis, Lisboa, Instituto Superior T6cnico, 2011. 124] V.J. Mathews and J.K. Tugnait. Detection and estimation with fixed lag for abruptly changing systems. IEEE Transactions on Aerospace and Electronic Systems, AES-19(5):730-739, 1983. [251 E. Mazor, A. Averbuch, Y. Bar-Shalom, and J. Dayan. Interacting multiple model methods in target tracking: A survey. IEEE Transactions on Aerospace and Electronic Systems, 34(1):103-123, 1998. [26] T. Menke and P. Maybeck. Sensor/actuator failure detection in the Vista F-16 by multiple model adaptive estimation. IEEE Transactions on Aerospace and Electronic Systems, 31(4):1218-1229, 1995. [271 R.L. Moose, H.F. van Landingham, and P.E. Zwicke. Digital set point control of nonlinear stochastic systems. IEEE Transactions on Industrial Electronics and Control Instrumentation, IECI-25(1):39-45, 1978. [28] M. Morelande, C. Kreucher, and K. Kastella. A Bayesian approach to multiple target detection and tracking. IEEE Transactions on Signal Processing, 55(5):1589-1604, 2007. [29] J. Nascimento, J.S. Marques, and J. Sanches. Estimation of cardiac phases in echographic images using multiple models. Proceedings of IEEE Conference on Image Processing, 2:149-152, 2003. 130] D. Ruan and D. Castanon. Real-time tumor tracking with interactive multiple model filter. CENSSIS, 2003. [31] C. Seah and I. Hwang. Hybrid estimation algorithm using state-dependent mode transition matrix for aircraft tracking. In In Proceedings of AIAA Guidance, Navigation and Control Conference, Keystone, CO, August 2006. [321 C. Seah and I. Hwang. State estimation for stochastic linear hybrid systems with continuous-state-dependent transitions: an IMM approach. IEE Transactions on Aerospace and Electronic Systems, 45, 2009. [331 N. Shimkin. Multi-model state estimation. Lecture notes: Estimation and Identification in Dynamical Systems, 2009. [341 D.D. Sworder. Control of systems subject to sudden change in character. Proceedings of the IEEE, 64(8):1219-1225, 1976. 1351 H.F. van Landingham and R.L. Moose. Digital control of high performance aircraft using adaptive estimation techniques. IEEE Transactions on Aerospace and Electronic Systems, AES-13(2):112-119, 1977. 63 Detection of interfer[36] N.A. White, P.S. Maybeck, and S.L. DeVilbiss. ence/jamming and spoofing in a DGPS-aided inertial system. IEEE Transaction on Aerospace and Electronic Systems, 34(4):1208-1217, 1998. [371 A.S. Willsky. A survey of design methods for failure detection in dynamic systems. Automatica, 12:601-611, 1976. [381 J.L. Yepes, I. Hwang, and M. Rotea. An intent based trajectory prediction algorithm for air traffic control. In AIAA Guidance, Navigation, and Control Conference and Exhibit, San Francisco, CA, August 2005. [39] L. Zhang. 7i,, estimation for discrete-time piecewise homogeneous Markov jump linear systems. Automatica, 45(11):2570-2576, 2009. [40] S. Zhao and F. Lie. Bayesian estimation for jump Markov linear systems with non-homogeneous transition probabilities. Journal of the Franklin Institute, 350(10):3029-3044, 2013. 64