# Linear dynamical mo dels,

Linear dynamical models, Kalman ltering and statistics. Lecture notes to IN-ST 259 Erik Blviken Nils Christophersen Geir Storvik University of Oslo, October 1998 ii Preface This course addresses students in both computer science (mathematical modeling) and statistics at the advanced undergraduate level. The background comprises a knowledge of linear deterministic dynamical systems (as given in MA-IN 126), linear algebra (MA 104), and the basic concepts of probability and statistics (ST 101). In MA-IN 126, linear dynamical systems were studied in continuous time using dierential equations but here we shall work exclusively with discrete linear systems represented by dierence equations. This has several advantages: When random variables are introduced to turn deterministic models into their stochastic counterparts, the resulting theory is much simpler in the discrete case than in the continuous one. The use of digital computers naturally leads to discrete mathematical formulations and the collection of observations in discrete time. Some dynamical systems, for example in economics, are dened only at discrete points in time. In any case, discrete time models may approximate continuous ones. In the rst part of these notes, we show how continuous linear, deterministic systems may be \discretized", i.e. how dierence equations may be derived from the continuous formulations. The emphasis is on state space models and the important system theoretic concepts of observability and reachability are dened. After describing what may be termed the deterministic \skeletons" in our context, stochastic models are introduced in the second and major part of these notes by adding \noise" (i.e. random variables) to the deterministic part. The Gaussian distribution in multidimensional space plays an important role when analyzing such models. A brief introduction to the theory of Gaussian random variables and processes is therefore included. After the discussion of the Gaussian distribution, an account of linear stochastic state space models and the Kalman lter is given. This topic is also treated in numerous other books and articles in the literature. Most of these accounts have iii iv been written in engineering contexts, although there has been a steadily growing interest in statistics, and many books on so-called time series analysis now include an exposition of Kalman ltering. In deciding to write out these notes in spite of the large body of written texts, we have been motivated by our need for addressing students in both mathematical modeling and statistics at the advanced undergraduate level. This suggests a text combining the main aspects of conventional Kalman ltering with an exposition of the unifying role these techniques can play within statistics. Apart from the elegant formulation of linear, dynamical statistical models obtained thereby, the applied statistician gains enormous modeling exibility by mastering this tool. He or she may, for example, easily include serially correlated errors in a regression, t all sorts of unbalanced models for analysis of variance, allow parameters to uctuate dynamically in time, or work with Bayesian versions of standard linear models. Through the Kalman lter he or she has a unied way of tting such models and to make predictions or forecasts. Other benets are automatic handling of missing data or, more generally, irregular patterns of observations. Students specializing in mathematical modeling will, on their part, in a work situation often nd themselves confronted with problems belonging to statistics. It is undoubtedly worthwhile to teach them how to use the Kalman lter apparatus to t statistical models such as regression or dynamic regression. Adding stochastic simulation to assess uncertainty (and even to test statistical hypotheses should the need arise), mathematical modelers, mastering, say Matlab , should, with some training, be able to do work often thought to belong to the realm of specialist statisticians. With the present course, we attempt to a take a step in this direction. To illustrate concepts and techniques beyond the simplest conventional textbook examples, one needs computer simulations. Modern software, such as Matlab , enables the students to get a deeper understanding of the main ideas and it becomes feasible to work with realistic examples. Matlab will be an integral part of the course and many exercises involve experimentation in this powerful software. In many cases the same examples are used in subsequent exercises. We therefore recommend the student to write their Matlab code as functions m-les, making it easy to utilize and modify in subsequent exercises. A separate appendix listing the most important commands and functions is included in order to make the Matlab implementations easier to work out. We strongly discourage using the software simply as a black box. As when using a calculator, one must be able to check that the results are reasonable. (Students having IN 227 may have an advantage here, as this course provides a good general background in numerical linear algebra.) The present notes, prepared for the autumn of 1998 is an extended version of the one from autumn 1997. A chapter discussing non-linear models is included. Further, many errors and misprints have been corrected. We are grateful to collegues and students for all the comments we have recived. We will in particular thank Bent Natvig for v careful proof-reading on most of the chapters and many valuable comments. Further, we will thank Anders Lland for nding a huge number of misprints! vi Contents 1 Introduction 1 2 Linear deterministic dynamical models. 5 2.1 Discretization of continuous systems . . . . . . . . . . . . . . . . . . . . 5 2.2 Stability, reachability and observability . . . . . . . . . . . . . . . . . . 7 2.3 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 3 Random (Gaussian) variables and processes 3.1 3.2 3.3 3.4 3.5 3.6 3.7 Random variables . . . . . . . . . . . . . . . . Random vectors: Pairs . . . . . . . . . . . . . Covariance matrix and Gaussian random pairs Random vectors: Higher dimension. . . . . . . Gaussian vectors . . . . . . . . . . . . . . . . Random processes . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . . . . 4 The general stochastic state space model 4.1 4.2 4.3 4.4 Introduction . . . . . . . . . . . . . . . . The general stochastic state space model Remarks . . . . . . . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . 5 Estimation of random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 15 17 19 23 25 28 31 35 35 38 39 41 43 5.1 Optimal estimation in general. . . . . . . . . . . . . . . . . . . . . . . . 44 5.2 Optimal estimation in the Gaussian case . . . . . . . . . . . . . . . . . 45 vii viii CONTENTS 5.3 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 6 The discrete time Kalman lter 6.1 6.2 6.3 6.4 6.5 6.6 6.7 Preliminaries . . . . . . . . . . . . . . . . . . The Kalman lter under Gaussian conditions . Derivation of the lter . . . . . . . . . . . . . Examples . . . . . . . . . . . . . . . . . . . . The Kalman lter under non-Gaussian noise . The Kalman lter for non-zero input variables Problems . . . . . . . . . . . . . . . . . . . . . 7 Examples from statistics 7.1 7.2 7.3 7.4 Regression with one explanatory variable Multiple linear regression . . . . . . . . . Time series models . . . . . . . . . . . . Problems . . . . . . . . . . . . . . . . . . 8 Additional aspects of the Kalman lter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1 Stability of the Kalman lter algorithm . . . . . . . . . . . . . . . . 8.2 Equivalent forms of the variance recursion . . . . . . . . . . . . . . 8.2.1 The information lter . . . . . . . . . . . . . . . . . . . . . . 8.2.2 More versions of the variance recursions . . . . . . . . . . . 8.3 Assessing modeling errors . . . . . . . . . . . . . . . . . . . . . . . 8.3.1 Updating the covariance matrix for an arbitrary gain matrix 8.3.2 Misspecied covariance matrices . . . . . . . . . . . . . . . . 8.3.3 General modeling errors . . . . . . . . . . . . . . . . . . . . 8.4 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 Prediction and smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 52 54 55 57 64 66 66 71 71 73 74 78 79 80 82 82 83 84 85 86 87 88 93 9.1 Predicting future . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94 9.2 Fixed-point smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 9.3 Fixed-interval smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . 96 ix CONTENTS 9.4 Fixed-lag smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 9.5 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 9.6 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101 10 Filter adaption 10.1 10.2 10.3 10.4 Observing the System process . . . . Main method . . . . . . . . . . . . . A safe approach: The EM algorithm Problems . . . . . . . . . . . . . . . . 11 Non-linear state estimation . . . . . . . . . . . . . . . . 11.1 Problem specication . . . . . . . . . . . . . 11.2 The linearized and extended Kalman lters . 11.3 Monte Carlo ltering . . . . . . . . . . . . . 11.3.1 The update step . . . . . . . . . . . 11.3.2 Monte Carlo prediction . . . . . . . . 11.4 Examples of non-linear ltering . . . . . . . 11.5 Problems . . . . . . . . . . . . . . . . . . . . A Matlab commands A.1 A.2 A.3 A.4 A.5 . . . . . . . . . . . . . . . . . . . . . . Properties of deterministic systems . . . . . . . Matlab commands for the Gaussian distribution Code concerning the random walk example . . . Code concerning the moving body example . . . Nonlinear ltering . . . . . . . . . . . . . . . . . A.5.1 Rejection sampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 . 107 . 109 . 113 . 117 119 . 120 . 121 . 123 . 124 . 128 . 130 . 135 137 . 137 . 138 . 138 . 141 . 145 . 145 x CONTENTS Chapter 1 Introduction Linear models of dynamical systems exist in various forms and may be categorized in dierent ways. In this course, we will separate between continuous and discrete representations and either of these may again be deterministic or stochastic. Dynamic linear models of these types are applied in many dierent elds. From a course in dierential equations, we know that a diverse set of dynamical phenomena are well described by linear deterministic dierential equations. Such phenomena include voltages and currents in electrical circuits, motions governed by Newton's laws, and biological systems that can be divided into linear compartments. By introducing random variables into such models, we are able to describe dynamic patterns that uctuate in an unpredictive way due to noise or errors, or due to a complexity that eludes our ability to describe the systems in deterministic terms. In general, linear models are widely used to describe dynamical behavior throughout the natural sciences and also in other elds such as economics. However, this does not imply that modeling of a dynamical system is necessarily a simple matter. In some cases, for example in physics, formulation of a model may be rather straightforward because the governing equations are known. But in other cases involving \softer" disciplines, model development may be a tedious process where various approximations must be assessed before one arrives at an adequate model representation. In this course, we will assume that a model of the system has been derived, and that observations (generally noise-corrupted) of some of the variables or combinations of variables in the system are available. From the model and the observations, the task is to estimate variables and parameters that are not directly observable. A huge body of theory has been developed to treat various aspects of such problems, including realization theory, system identication, ltering theory, time series analysis, and dierent types of statistical estimation theory. Here we can only focus on some of these aspects, and so-called state estimation will play a prominent role. To make the presentation more concrete, we start with an example that will be used repeatedly. 1 2 CHAPTER 1. INTRODUCTION u(t) m x(t) Figure 1.1: A body of mass m moving in one dimension under the inuence of an external force u(t) Example 1 (A moving body) Consider the system in Figure 1.1, where a body of mass m moves in one dimension under the inuence of an external force u(t). From Newton's 2nd law of motion, one obtains a linear second order dierential equation for the displacement (position) x(t). (See for example (Boyce & DiPrima 1977).) mx(t) = u(t): (1.1) Given the initial conditions x(0) and x_ (0), the motion of the body is completely determined once the input function u(t) for t 0 is known. The position and the velocity taken together as a vector will be called the state vector because these variables contain all information from the past that is necessary to predict the future behavior of the body. (Technically, u(t) must be at least piecewise continuous to secure a unique solution of the dierential equation.) The solution x(t), which is the sum of the homogeneous part (involving the initial conditions) and a particular part (determined by u(t)), can only be expressed in closed form for certain input functions. However, it is a simple matter to solve equation (1.1) numerically for an arbitrary u(t). 2 Eq. (1.1) is a model of the physical system in Figure 1.1. The model is derived from socalled rst principles (Newton's laws in this case), and gives an accurate description of the motion provided factors such as air resistance and relativistic eects are negligible. A representative problem of the ones we will consider in this course is the following: Suppose that the force u(t) is the sum of a known deterministic part and an unknown part that can only be described in statistical terms (e.g. wind gusts). Furthermore, the position of the body is measured at discrete times with fairly large errors. Our goal is to estimate the speed of the body as well as improving the raw position data. To do this, we use the model, properly implemented on a computer, in combination with the observations. (If we consider a satellite or a missile instead of our innocent body, such a task is of more obvious interest.) The scheme used is the Kalman lter named after Rudolf E. Kalman who published it in 1960. This is called state estimation. If one also wants to estimate the mass of the body from the same observations, that will be classied as parameter estimation. Before deriving the Kalman lter, we need to be able to discretize eq. (1.1). This is carried out in Chapter 2, which also contains a brief exposition of the linear system 3 theoretic concepts of observability and reachability. These properties, dened here for deterministic systems, play a role when analyzing the performance of the Kalman lter. Because most of the subsequent theory will rely on stochastic theory, Chapter 3 discuss random variables and process in general and Gaussian processes in particular. In Chapter 4, the stochastic state space model is introduced, and some of its properties are discussed. The derivation of the Kalman lter rely heavily on general theory for estimation of random variables based on a set of estimation. This will be treated in Chapter 5. In Chapter 6 the important features of the Kalman lter are laid out and several examples and additional properties are provided in Chapter 8. Prediction and so-called smoothing (where both past and future observations are used in estimating the state vector) are treated in Chapter 9. When putting up a model, many parameters have to be specied. In practice, several of these will be unknown, but can be estimated from data. This issue will be discussed in Chapter 10. In Chapter 7 it is explained how many seemingly dierent problems in statistics fall within the Kalman lter framework. Finally, Appendix A contains a list of useful Matlab commands as well as the Matlab code used to program many of the examples. Bibliographic notes As noted, the Kalman lter was published in 1960 (Kalman 1960). Since then an enormous body of literature has been produced on various aspects of the Kalman lter. (A few years ago, a literature count gave 200,000 references.) The lter was originally designed to give a recursive solution to the classical linear least squares estimation problem in signal processing and control theory. Recent references based on this engineering tradition include Brown & Hwang (1992); Minkler & Minkler (1992); Grewal & Andrews (1993). The last book contains a historical overview. These books also contain additional material on the linear system theoretic properties of observability and reachability (our Chapter 2), Kalman lter stability, various covariance recursions, modeling errors (our Chapter 8) and smoothing (our Chapter 9). Over time, the Kalman lter has taken root in many other areas. But it took over 20 years before the lter became part of mainstream statistics, although the lter is readily interpreted as a recursive Bayesian estimator. This illustrates an unfortunate 'cultural gap' which sometimes exists between disciplines. Today, the Kalman lter is treated in full from a statistical viewpoint in books such as Harvey (1989) and West & Harrison (1989). The latter emphasizes in particular on the Bayesian approach to dynamical systems, see also Harrison & Stevens (1976). Many of the mainstream textbooks in statistical time series also cover Kalman ltering. 4 CHAPTER 1. INTRODUCTION Chapter 2 Linear deterministic dynamical models. We will now study what may be called the deterministic \skeletons" of our dynamic linear models. This simplies the exposition of discretization as it allows us to draw directly on material from courses in ordinary dierential equations. Furthermore, the concepts of observability and reachability are most easily introduced in a deterministic setting. 2.1 Discretization of continuous systems Instead of considering 2nd order, or more generally nth order, dierential equations, it is frequently advantageous to transform the model into a set of coupled rst order dierential equations, i.e. to the so-called state space form . As noted in the previous chapter, the state vector contains the internal variables needed at a certain point in time to solve the model equations. Consider again our motion of body example: Example 1 (A moving body, cont.) For our 2nd order example, the two state variables are x (t) = x(t) and x (t) = x_ (t). Eq. (1.1) can then be written as: 1 x_ 1(t) = x2(t); x_ 2(t) = u(t)=m: 2 (2.1a) (2.1b) In this form, the equations are readily solved in some numerical software, see Appendix A. Assume that the position z(t) = x (t) is measured (free of errors) at discrete times tk ; k = 1; 2; : : : . The complete model, comprising the dynamical part in eq. (2.1) and 1 5 6 CHAPTER 2. LINEAR DETERMINISTIC DYNAMICAL MODELS. the measurement, may be written in vector-matrix form as x_ (t) = 0 1 x (t) + 0 u(t); x_ (t) 0 0 x (t) 1=m z (tk ) = 1 0 xx ((ttk )) ; k 1 1 2 2 (2.2a) (2.2b) 1 2 where x = x x 1 2 T 2 is the state vector. In general, we will consider a linear continuous nth order system given by: x_ (t) = A(t)x(t) + B (t)u(t); (system) (2.3a) z (tk ) = H (tk )x(tk ); (observations) (2.3b) where x(t), z(t) and u(t) (the input function) are vectors having dimensions n; m and p, respectively. The time interval between measurements is denoted the sampling time and is not necessarily uniform. Note that the system may be time-varying since the dening matrices depend on t. Physically, this corresponds to the fact that the body will not come to rest at the origin unless it is released there with zero velocity. Eq. (2.3) forms a natural starting point for the discretization procedure. Recall the general solution to eq. (2.3a) (Boyce & DiPrima 1977): x(t) = (t; t0)x(t0) + Z t t0 (t; s)B(s)u(s)ds (2.4) where (t; t ) is the so-called transition matrix. We will denote this solution the superposition integral since the complete solution is a superposition of the homogeneous part, including the initial conditions, and the particular solution including the input function u(t). In the time-invariant case we have (t; t ) = eA t t0 (cf. Boyce & DiPrima (1977)). Take t = tk and t = tk . Then 0 ( 0 0 ) 1 x(tk ) = (tk ; tk )x(tk ) + 1 1 Z tk tk 1 (tk ; s)B(s)u(s)ds: (2.5) If u(t) is a constant u(tk ) over the interval [tk ; tk ), we may write the integral term as (tk ; tk )u(tk ) where 1 1 1 1 (tk; tk ) = 1 Z tk tk 1 (tk ; s)B (s)ds: (2.6) If u(t) is not a constant over the sampling interval, it is still customary to use the same notation although now only the composite term (tk ; tk )u(tk ) can be obtained 1 1 7 2.2. STABILITY, REACHABILITY AND OBSERVABILITY from the continuous formulation. Simplifying the notation writing k instead of tk and using only one time argument in the and matrices (i.e (tk ; tk ) = (k 1)), we arrive at the discrete state space model that will be employed throughout the course: 1 x(k ) = (k 1)x(k 1) + (k 1)u(k 1); z (k ) = H (k )x(k ): (2.7a) (2.7b) Here k = 1; 2; : : : . Note that the system equation (2.7a) is a dierence equation. Numerical software such as Matlab have general functions for performing such a transform (see Appendix A). Example 1 (A moving body, cont.) For our 2nd order example, the discrete system matrices will be time-invariant, i.e. (k 1) = and (k 1) = for all k, and are easily computed symbolically as: =2m ; = 10 T1 ; = TT=m where T is the sampling time. (Can you explain why the elements in these matrices make sense physically?) 2 2 Stability in the discrete case is dened in a similar way as for continuous systems. Obviously, a discretized system is stable if the continuous version is so. In the timeinvariant case where = eAT , the eigenvalues di (i = 1; : : : ; n) of are given by di = ei T , where i are the eigenvalues of A. If all i lie in the left half of the complex plane, the eigenvalues of will be within the unit circle. This is the stability criterion for discrete systems. (These results are treated in more detail in the Exercise 2.1.) 2.2 Stability, reachability and observability An important question regarding dynamical systems is their equilibrium points and whether these are stable or not (Boyce & DiPrima 1977). An equilibrium point xe satises x_ e(t) = 0. For the homogeneous part of the dierential equation in eq. (2.3a) (discarding the second term on the right hand side), the origin is clearly such a point. The origin is called globally asymptotically stable if jjx(t)jj ! 0 as t ! 1 for any x(0). A linear time-invariant system is stable in this sense if and only if all eigenvalues of A lies strictly in the left part of the complex plane. The solution then comprises sums of damped exponential functions. Considering the eects of input functions u(t), a system with a globally asymptotically stable homogeneous part will have a bounded solution (jjx(t)jj remains nite) provided u(t) is itself bounded (Boyce & DiPrima 1977). 8 CHAPTER 2. LINEAR DETERMINISTIC DYNAMICAL MODELS. For Example 1, eq. (2.2a), both eigenvalues are equal to 0 and are not strictly in the left half plane. The system is therefore not globally asymptotically stable. Reachability and observability are linear system theoretic concepts that are unrelated to stability. They lead to conditions that in a sense guarantee the model to be well posed for state estimation and identication. We will only consider time-invariant discrete systems (the sampling time T and the matrices ; and H are xed), but the ideas may be extended to more general situations Denition 2.1 A system is reachable if there exists a nite sequence of input vectors u(k ), k = 0; 1; : : : ; k0 transferring the system from any initial state x(0) to any nal state x(k0 + 1). Intuitively, this implies that the state vector may be controlled from any point to any other point by selecting a proper sequence of inputs. The necessary and suÆcient condition for reachability is derived from eq. (2.7a) by writing out the recursion as follows: nx(0) + n u(0) + + u(n 1) = nx(0) + W cU : x(n) = 1 (2.8) Here n is the dimension of the state vector and = n ; 0 1 u(n 1) B u(n 2) C B C U =B C: . . @ . A u(0) Wc 1 We see that any vector x(n) can be reached if and only if the n pn matrix W c has n independent columns, i.e. the rank is n. Note that for p > 1 there is no unique sequence of input vectors. There is no reason to consider k0 > n 1, since it can be shown (by the Caley - Hamilton theorem) that the rank of W c will be maximal for k 0 = n 1. Denition 2.2 The system is observable if any initial state x(0) can be determined from z (k ); k = 1; : : : ; k00 for some nite k00. This condition implies that the state vector can be reconstructed from a nite sequence of observations, since by knowing x(0) (and u(k)), the state vector can be computed for all times. Assume for the time being that all inputs are 0. The necessary and 9 2.3. PROBLEMS suÆcient condition for observability is derived by considering the set of equations: = Hx(1) = H x(0) z(2) = Hx(2) = H x(0) ... z (n) = H n x(0); z(1) 2 or in a more compact form: 0 H B H B B .. @ . H n 1 C C C A 0 z(0) B z(1) B x(0) = B .. @ . z(n 1 C C C: A 1) Given that = eAT is always nonsingular (since the eigenvalues are always non-zero), this set of linear equations in x(0) has a unique solution if and only if the nm n matrix 1 0 H B H B W o = B .. @ . H n 1 C C C A 1 has n independent rows, i.e. the rank is n. We might, of course, have used the matrix W o but this would not have complied with the literature standard. The condition will be the same when known inputs aect the system (check that yourself) and, as with reachability, there is no need to consider the case k00 > n. If the system is not observable, the state vector cannot be determined even from noise - free measurements. It seems reasonable that this may have undesirable eects on a method that attempts to estimate the state vector from noisy observations. As we will see in Chapter 8, observability is one of the necessary conditions that must be satised in order to secure that the estimation problem is well posed in a certain sense. The other condition turns out to be reachability extended to include stochastic inputs. 2.3 Problems Exercise 2.1 (Eigenvalues and matrix exponentials) Matrix exponentials are important for discretization of continuous dynamic systems (that is the transition from dierential equations to dierence equations). We will in this exercise consider the eigenvalues of matrix exponentials. 10 CHAPTER 2. LINEAR DETERMINISTIC DYNAMICAL MODELS. Consider the following matrices a 0 a 1 A = A = 0 a ; 0 a 1 2 and the Hilbert matrix H n of order n. (The Hilbert matrix is the n n matrix with (i; j )'th element hij = 1=(i + j 1).) This matrix is known as being diÆcult to handle numerically. The Hilbert matrix may be computed directly in Matlab with the command hilb(n). (a) What are the eigenvalues of A and A ? (You may see this directly from the matrices because of their particular form.) (b) The matrix exponential for an n n matrix A is given by 1 2 eA = I + A + A2 =2! + Calculate by hand eA1 and eA2 . (c) More diÆcult: Can you prove what we now might think? It looks like the general connection is (eA )i = e(A)i : (Hint: Similarity-transformations are useful if you remember this from your linear algebra course.) (d) Check the answer you got in (a) in Matlab for a = 2. (Use eig.) (e) Check the answer you got in (b) in Matlab (use expm) for a = 2. (f ) Use Matlab to check if the results in (c) is correct for H n with n = 2; 3; : : : . Is it correct if you try with the 4 4-matrices A3 = A1 0 0 A2 andA = 4 A1 1 1 A2 where 0 is the 2 2 matrix with only zeros, while 1 only contain ones? (Hint: Look at the commands ones and zeros.) Exercise 2.2 (A moving body) (a) Show that the system in Example 1 (the discretized version) is reachable and explain why this is reasonable physically. (b) Verify that our example is observable if the position is measured, but not if only velocity is measured. Why is this? 11 2.3. PROBLEMS Exercise 2.3 (A deterministic physical system) A pendulum with small angular movements, (t), can be described by the equation L(t) + g(t) = u(t) where L is the rod length, g is the gravitational acceleration and u(t) is the acceleration we can incur. (a) Put the system in state space form (x (t) = (t) and x (t) = _(t)) and show that we get: x_ (t) = 0 1 x (t) + 0 u(t): x_ (t) g=L 0 x (t) 1=L 1 1 1 2 2 2 (b) Intuitively, do you think this system is stable? Show that the eigenvalues are pure imaginary. (c) Discretize the system with T = 0:5 s, that is, calculate = eAT . (d) Set L = 1 m, g = 10 m/s , (0) = 0:1, _(0) = 0 and u(t) = 0, t > 0. Solve the dierential equations in Matlab by use of the routine ode45 and make plots of (t) and _(t). (You should try to write a so-called m-le which is a good training-exercise.) (e) Solve the same system again, but now with the routine lsim. Again make plots of (t) and _(t). (f ) Discretize the system numerically by use of the routine c2d in Matlab . (g) Simulate the discretized system with the routine dlsim. Compare with the numerical solutions of the dierential equation. 2 Exercise 2.4 (Eigenvalues and stability in the cont. and discrete case) Origo is a stable equilibrium point for the time-invariant system x_ (t) = Ax(t) if and only if the eigenvalues of A lies in the left half of the complex plane exclusive the imaginary axis (from here on called the left half-plane). (a) Consider the pendulum of Exercise 2.3. We now introduce air resistance which is assumed to inuence the pendulum with a power proportional to its anglevelocity. Show that we then get the following dierential equation for the movement of the pendulum: L(t) + c_(t) + g(t) = u(t); where c > 0 is the friction coeÆcient specied by the size and surface of the pendulum ball. 12 CHAPTER 2. LINEAR DETERMINISTIC DYNAMICAL MODELS. (b) Put the system in state space form and show that the eigenvalues to the system matrix now is in the left half-plane. (c) What happens with the eigenvalues when the value of c increase from 0? (d) The general linear system x_ (t) = Ax(t) has the solution x(t) = eAtx(0). Can you from this and from what you know about the eigenvalues of eAt explain why all terms in the solution to a stable model will contain exponentially damped functions and therefore converge to zero? (e) Suppose L = 1 m, g = 10 m/s . Use Matlab for simulating the system for c = 0:5 m/s and some smaller and larger values. Does it t with your physical intuition? (f ) Under the discretization with sampling time T we have from Exercise 2.1 the connection i(eAT ) = ei A T , i = 1; : : : ; n (when A is n n). Explore how the eigenvalues of A which are on the real or the imaginary axes are transformed under this transformation. (Put T = 1 for simplicity. Matlab calculates with complex numbers and makes plots in the complex plane.) 2 ( ) Exercise 2.5 (Reachability and observability) (a) Consider the pendulum model in Exercise 2.4. Put again L = 1 m, g = 10 m/s and c = 0:5 m/s. Use Matlab and discretize the model with T = 0:5 s and show that the discrete system is reachable. (b) It is possible to bring the system from any initial state to an arbitrary end state in two time step. Find u(0) and u(1) such that the system is brought from x(0) = 0:1 0 T to x(2) = 0:1 0:05 T . 2 (c) Assume x(0) = 0 0 T . Which states can be reached in one time step? (d) Show that the discrete system is observable both if only the angle is measured and if only the angle velocity is measured. Why is this reasonable? (Remark: Remember that for Example 1 (one-dimensional movement without friction), the system was not observable if only the velocity was measured.) (e) Suppose the angle z(k) = x (k) = (k) is measured and that we have observed the angles z(0) = 0:15, z(1) = 0:0098, z(2) = 0:1169 and z(3) = 0:0065. Calculate from this the initial velocity. 1 Exercise 2.6 (Spring system) Consider the motion of a mass m suspended at the end of a spring and subject to an external force acting in the vertical direction. Hooke's Law will be helpful(Boyce & 2.3. PROBLEMS 13 DiPrima 1977). Hooke's Law: The force exerted by a spring has magnitude proportional to the displacement of the spring from its equilibrium position. A spring motion can be described as mx(t) + ax(t) = u(t) where a > 0 is spring constant and u(t) is external force. (a) Put the system in state space form. (b) Do you think the system is stable? Find the eigenvalues. (c) Find the corresponding discrete system with T = 1, i.e. compute . Since no physical spring can sustain simple harmonic motion, a frictional force must be introduced. A frictional force with frictional constant c is proportional to the velocity. (d) Find the dierential equation in this case and write the system in state space form. (e) Analyze stability of the system for dierent values of c. (f ) Assume m = 10. What requirements for a and c are needed for the system to be reachable? 14 CHAPTER 2. LINEAR DETERMINISTIC DYNAMICAL MODELS. Chapter 3 Random (Gaussian) variables and processes The purpose of this chapter is to present an elementary, informal and brief introduction to the theory of Gaussian random variables and processes with the specic need of Kalman ltering in mind. This means that the students are only assumed to have a background from an elementary course in probability theory corresponding to the work of half a semester. There is usually little emphasis on joint Gaussian distributions in such courses. The chapter will go from scratch, starting with the concept of univariate random variables. No proofs will be given. The reader is here referred to introductory textbooks in statistics and probability. Some possible references are Taylor & Karlin (1993) (stochastic processes) and Mardia, Kent & Bibby (1979) (random vectors, Gaussian distribution). 3.1 Random variables A random variable x is a quantity assigned values through a probabilistic mechanism dened by a probability density (or the equivalent terminology distribution function ) p(x). Any non-negative function integrating to one over the real line may be a density in this sense . To a density is associated its mean or expectation 1 = E [x] = and its variance Z 1 1 xp(x) dx; var[x] = E [(x ) )] = 2 (3.1) Z 1 1 (x ) p(x) dx: 2 1 This is not strictly true. The functions must also be what mathematicians call need not bother with this technicality. 15 (3.2) measurable. We 16 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 −5 −4 −3 −2 −1 0 1 2 3 4 5 Figure 3.1: The univariate Gaussian distribution for dierent values of and . The solid line is for = 0 and = 1, the dashed line is for = 0 and = 2 while the dotted line is for = 1 and = 1. Their meaning as average and spread of the distribution is obvious from their denitions. They need not necessarily exist, i.e they could be innite. This will in practice be no problem for the situations we will consider. The unit of the variance is the square of the unit in which x is expressed. As a result its numerical value does not admit direct interpretation. In spite of this, the variance is an immensely useful concept. The variance may be transformed to a measure on the original unit by taking the square root: p stan[x] = var[x]: (3.3) This measure is denoted the standard deviation of the random variable x. The most famous example, and the only one we shall consider in this chapter, is the Gaussian (or normal) density 1 1 (x ) : p(x) = p exp (3.4) 2 2 Here and > 0 are the parameters of the distribution. Their interpretation is immediate in that = E [x] and = stan(x). To specify that x has a Gaussian distribution with expectation and variance , we will use the notation x N (; ). Figure 3.1 shows p(x) for dierent values of and . Note the symmetry around , while the width of the curve is determined by . 2 2 2 2 both 2 and that has been used as notation for expectation in (3.1) as a parameter of the normal density in (3.4). The symmetry of the Gaussian density implies that the two usages are identical. 2 Note 17 3.2. RANDOM VECTORS: PAIRS R E [(x)] = 11 (x)p(x) dx E [ax + b] = aE [x] + b E [x1 + ::: + xn ] = E [x1] + ::: + E [xn ] var[ax + b] = a2var[x] var[x] = 0 ) x = constant Condition None None None None None Table 3.1: Rules for operating means and variances. x1 ; :::; xn are random variables, a and b real numbers (i.e. constants) and a real function. Expectation, variance and standard deviation are simple concepts that facilitate the interpretation of random variables. They are what the mathematicians call functionals, i.e. they associate a real number to a given density function. There are a number of simple rules to operate them, which are convenient in probabilistic calculations. Those most needed in the following are listed in Table 3.1. 3.2 Random vectors: Pairs The extension to pairs of random variables is straightforward (at least at rst glance). A random vector (x ; x ) is dened through its joint density function p(x ; x ). The only dierence from the univariate case is that the density is now dened on the plane instead of on the real line. As before, any non-negative function integrating to one over both x and x may be used as a mathematical model for a random pair . Univariate density functions for both variables may be deduced from their simultaneous one p(x ; x ). If p(x ) and p(x ) are the densities of x and x marginally, i.e. without regard to the other , then 1 1 1 2 p(x2 ) = 2 1 2 3 2 1 Z 1 1 4 2 1 2 p(x1 ; x2) dx1 ; (3.5) and similar for p(x ). The point in introducing joint densities is, however, a dierent one. When dealing with dynamic systems, there is a need for a concept that can capture stochastic dependence between dierent variables. Dependence is in the present context the same as covariation . To detail what this means, consider two observations that are close in time. If one is accidentally low (or high), then so is often the other. They covariate positively. Negative covariation is the opposite, one variable tend to be high when the other is low. The covariation is not in any xed, deterministic sense. The dependence is modied by randomness. A powerful way 1 3 Subject to measurability, see preceding footnote. 4 Densities are throughout generically denoted p, the the argument indexing of dening for which variables. For example, p(x1) is the density of x1 and p(x1; x2) the joint density of (x1; x2). 18 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES of expressing the idea, is through the concept of conditional density. The formal denition is p(x ; x ) p(x jx ) = ; (3.6) p(x ) where we have introduced the vertical bar to indicate which variables we condition on (those on the right side of the vertical bar). Note that the conditional densities become dierent functions as x is varied. It is easily veried that the integral with respect to x (over the real line) is one (do it!). It is therefore a valid density, and it enables us to express how the random variation in x changes with the value of x . It is perfectly legitimate to compute the mean and variance of this conditional distribution, for example 1 1 2 2 2 2 1 1 2 E [x1jx2] = Z 1 1 x1 p(x1jx2 ) dx1 ; (3.7) which is known as the regressor of x on x . It will play a central role in the derivation of the Kalman lter, because it turns out that it denes how an unobserved x ideally should be estimated from knowledge of x . Dependence, as dened above, is a rich and complicated concept, and many applications need a more modest version. The covariance achieves just that. Let = E [x ], = E [x ], P = var[x ] and P = var[x ] be the mean and variances of the two random variables. Then covariance and correlation , which reduces dependence down to single numbers, are dened through P = cov[x ; x ] = E [(x )(x )] (3.8) and (3.9) corr[x ; x ] = (P PP ) = = p cov[x ; x ] : var[x ]var[x ] Covariance is not really a dependence measure, since it is inuenced by the measurement scales used (multiply everything with, say 10 and the covariance goes up by the same number). However, the denominator in (3.9) makes correlation scale-free and thus a (necessarily imperfect) way of expressing dependence as a single number. Another important concept is that of independence between two random variables. That occurs when p(x jx ) = p(x ); for all x and x : (3.10) This says that the density (or variation ) of x is the same whatever value of x . No covariation between the two is then present. Eq. (3.10) implies that p(x ; x ) = p(x )p(x ); 1 2 1 2 1 2 2 11 12 1 1 1 1 2 22 2 1 1 2 11 22 1 2 1 12 2 1 2 2 1 1 2 2 1 1 2 1 2 1 2 2 19 3.3. COVARIANCE MATRIX AND GAUSSIAN RANDOM PAIRS E [E [x1jx2 ]] p = E [x1] jE [x1x2]j E [x21]E [x22] E [(x)jx = a] = (a) E [x1jx2] = E [x1] E [x1x2] = E [x1]E [x2] var[xP1] = E [var[ P x1jx2 ]] + var[ PE [x1jx2]] var[ j xj ] = j var[xj ] + j6=k cov[xj ; xk] var[x1 + ::: + xn] = var[x1] + ::: + var[xn] cov[x1; x1] = var[x1] cov[x1; x2] = cov[x2; x1] cov[ax1 + b; cx2 + d] = ac cov[x1; x2] cov[x1 + x2; xp 3 ] = cov[x1 ; x2 ] + cov[x1 ; x3 ] cov[x1; x2] var[x1]var[x2] jcorr[x1; x2]j 1 corr[ax1 + b; cx2 + d] = corr[x1; x2] corr[x1; x2] = 0 Condition None None None Independence Independence None None x ; :::; xn uncorrelated None None None None None None None Independence 1 Table 3.2: Rules for dependent random variables . The independence conditions (when present) are suÆcient, not necessary. and p(x2 jx1) = p(x2 ); see (3.6). The concept of independence is thus symmetric in x and x , as it should. Reversing the roles, i.e. starting out from x given x instead of x given x would lead to the same denition. Mathematical properties and rules of the concepts introduced in this section, are summarized in Table 3.2. Most of these properties are easily calculated, given the denition of expectation, variance and covariance. The second property is usually called Schwarz's inequality. Note in particular the last property, which says 1 2 1 2 1 2 Proposition 3.1 Two random variables which are independent are also uncorrelated. 3.3 Covariance matrix and Gaussian random pairs We shall in the next section deal with many random variables simultaneously and consider covariances between all pairs. It is convenient to organize these quantities in 20 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES matrix form. Let us rst see how this looks when there are just two variables x and x . Dene 1 2 x= x1 x2 (3.11) with mean vector = 1 2 (3.12) and covariances P = P11 P12 P21 P22 (3.13) : known as a covariance matrix of x, is a convenient algebraic tool. It is always symmetric, since P = P . Gaussian distributions for pairs are neatly dened from it. The density for x is then dened as 1 (3.14) p(x) = (2 ) jP j = expf (x )T P (x )g: 2 Here x = (x ; x )T is a column vector, and P, 1 12 21 1 1 2 1 2 jP j = PP1121 PP1222 = P11P22 P12 P21 is the determinant of P . The exponent in (3.14) is a quadratic form in x , i.e. an expression containing squares and products of x and x . The reader can write it out, and convince him(her)self that the expression is rather complicated. Clearly this must be even more so when additional variables are added. By contrast, the matrix form is dense and, with experience, even nice and transparent. The Gaussian model contains ve parameters. There are two in the mean vector , two variances P and P and the additional covariance P = P . It may not be obvious that the parameters in (3.14) actually have this interpretation (and that (3.14) is a density, for that matter). However, this turns out to be so. Figure 3.2 shows p(x) for 0 1 = P = 0 ; 1 ; with = 0; 0:5; 0:9. The function is symmetric around zero for = 0, while for increasing , the density is concentrated more and more around the line x = x . This means that the density gives high probability for x being almost equal to x . Suppose x = x x )T is Gaussian in a joint sense, i.e. with density of the form (3.14). It can then be proved that each of x and x are Gaussian marginally . 1 11 1 22 2 12 2 21 1 1 1 2 1 2 2 2 21 3.3. COVARIANCE MATRIX AND GAUSSIAN RANDOM PAIRS 0.16 0.14 0.12 0.1 0.08 0.06 0.04 0.02 0 3 2 3 1 2 0 1 0 −1 −1 −2 −2 −3 −3 0.2 0.15 0.1 0.05 0 3 2 3 1 2 0 1 0 −1 −1 −2 −2 −3 −3 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 3 2 3 1 2 0 1 0 −1 −1 −2 −2 −3 −3 Figure 3.2: The bivariate Gaussian distribution for dierent correlation values . The upper panel corresponds to = 0, the middle panel to = 0:5 and the lower panel to = 0:9. In all cases both x1 and x2 have mean 0 and variance 1. 22 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES Also note the structure of the density when the covariance P = 0. It is then seen from (3.14) that the joint density becomes the product of two univariate densities. x and x are therefore independent in this case. But from Table 3.2 (last line) independence always implies that the correlation (and hence the covariance) is 0. Thus two Gaussian variables which are uncorrelated are also independent. Another useful property of Gaussian models is that the conditional distribution of one variable given the other remains Gaussian. The parameters of this conditional distribution is important. Take, for example, x given x . Then 12 1 2 1 E [x1jx2] = 1 + P12 (x P22 2 2 2 ); (3.15a) and var[x jx ] = P 1 2 P122 : P22 11 (3.15b) These results will be proven in a more general setting in Theorem 3.2. Both formulae have intriguing interpretations. The rst says that the expected value of x , taking the value of x into account, is modied from what it would have been without this knowledge, by a factor proportional to x . There is no adjustment if x turned out to be what we expected (= ). Neither does the second factor contribute if the covariance P = 0. The two variables are then independent, and the second one does not contain relevant information about the rst. The adjustment also becomes 0 if P ! 1. Now the information in x is so unreliable that it is of no value. As to (3.15b), observe that the right hand is always less than P , which is the variance of x without knowledge of x . Variation (or uncertainty ) always goes down when extra information is added. That is in the ideal world. It can be dierent in practice when imperfect knowledge of underlying mechanisms prevent us from using information properly (cf. Chapter 8). The formulae will be proven in a more general setting in Theorem 3.2. 1 2 2 2 2 2 12 22 2 11 1 2 Example 2 A small illustrating example with signicance for dynamic systems is the following. Suppose x is Gaussian with mean 10 and standard deviation 1. Let " be another Gaussian variable, independent of the rst and with mean 0 and standard deviation p 1 a (jaj < 1). Dene 1 2 x2 = 10 + a(x1 10) + ": (3.16) and assume that x follows x in time. From one of the results in the next section, it can be shown that x is also Gaussian. From the rules in Table 3.1, we get E [x ] = 10 and var[x ] = a var[x ] + var["] = a + (1 a ) = 1, showing that x has exactly the same distribution as x had. Clearly we can continue this way and dene a sequence 2 2 2 1 2 2 1 1 2 2 2 23 3.4. RANDOM VECTORS: HIGHER DIMENSION. of random variables with the same property. The sequence is then stationary in distribution (see Section 3.6 for more on this concept). The variables x and x are dependent. By simple calculations corr[x ; x ] = a; i.e. the degree of dependence varies with a. Eq. (3.16) is one of many stochastic models for describing covariation in time sequences. 2 1 1 2 2 3.4 Random vectors: Higher dimension. It is easy formally to extend the concept of joint densities (or multivariate distributions ) to the case of n variables x = (x ; :::; xn)T . The density is a non-negative function p(x) in x with integral equal to one when all n variables are integrated over the real line. Much interest in the following has to do with subvectors of x. Let 1 x= x1 x2 gr gn (3.17) r; where the rst subvector x = ( x ::: xr )T consists of the rst r variables in x. The second subvector is x = ( xr ::: xn)T . Extensions of (3.5) and (3.6) are now 1 1 2 p(x2 ) = Z x1 2Rr +1 p(x1 ; x2)dx1; (3.18) which denes the marginal density of x by integrating over the rst r variables (those in x ) and p(x ; x ) ; (3.19) p(x jx ) = p(x ) which is the conditional density of x given the value of x . We can dene independence between vectors as well. As in Section 3.2 this should mean that the conditional density does not depend on the set of conditioning variables, or equivalently p(x ; x ) = p(x )p(x ). The n variables have means j = E [xj ], variances Pjj = var[xj ] and covariances Pjk = cov[xj ; xk ]. It is convenient to organize them in vector and matrix form, giving the mean vector and the covariance matrix, i.e. as 2 1 1 1 2 2 2 1 1 2 1 0 B = E [x] = @ 1 ... n 2 2 1 C A; 0 P = E [(x )(x B )T ] @ P11 P1n Pn1 Pnn ... ... ... 1 C A; (3.20) 24 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES Condition None None None None None Independence Independence T VAR[Bx + a] = BfVAR[x]gB None VAR[x + ::: + xn] = VAR[x ] + ::: + VAR[xn ] x ; :::; xn uncorrelated VAR[xP] = E [VAR[ x jx ]] + VAR[ E [x jx ]] None P P VAR[ j xj ] = j VAR[xj ] + j6 k COV[xj ; xk ] None COV[x ; x ] = VAR[x ] None COV[x ; x ] = COV[x ; x ] None T COV[Ax + b; Cx + d] = AfCOV[x ; x ]gC None COV[x + x ; x ] = COV[x ; x ] + COV[x ; x ] None E [Ax + b] = AE [x] + b E [x1 + ::: +R xn ] = E [x1 ] + ::: + E [xn ] E [(x)] = 11 (x)f (x) dx E [(x)jx = a] = (a) E [E [x1 jx2]] = E [x1] E [x1jx2 ] = E [x1 ] E [x1x2 ] = E [x1]E [x2 ] 1 1 1 1 1 2 1 2 = 1 1 1 1 2 2 1 1 1 2 2 1 3 1 2 2 1 3 Table 3.3: Rules for operating mean vectors and covariance matrices. x ; :::; xn are random 1 vectors, a a xed vector, B a xed matrix and a real function of n variables. and also to consider blocks. Introduce, consistent with the subdivision in (3.17), = 1 2 gr gn r; P = P 11 P 12 P 21 P 22 gr gn (3.21) r Here and are the mean vectors of x and x and P and P their covariance matrices. The two remaining sub-matrices in (3.21) are P and P . They represent covariances between the elements in x and x . As mentioned in Section 3.3, all covariance matrices are symmetric and this makes P and P the transpose of each other, i.e. P = P T . Independence between x and x implies that P = P T = 0. The opposite implication is not true, although it does hold for Gaussian distributions, as we shall see in the next section. Random vectors for which the cross covariance matrix P is 0, are called uncorrelated. It will be convenient to write E [x] = and VAR[x] = P , the capital letters in 'VAR' marking the covariance matrix of a vector. Further, we will denote COV[x ; x ] the matrix containing as its (i; j )'th element the covariances between the i'th component of x and the j 'th element of x . With this convention, analogies to the operational rules in Tables 3.1 and 3.2 are set down in Table 3.3. Note the formula requiring uncorrelatedness. A suÆcient condition for this is full independence, as noted earlier. The result about double expectation is of tremendous importance when the Kalman lter is to be derived. We will therefore prove this result explicitly. 1 1 2 1 2 1 21 22 12 21 2 12 12 11 21 2 12 21 12 1 1 2 2 25 3.5. GAUSSIAN VECTORS Theorem 3.1 For any two random vectors x1 ; x2 , we have (under some \general conditions") that E [E [x1 jx2 ]] = E [x1 ]: Proof of Theorem 3.1 E [x1 ] = Z x1 p(x1 )dx1 Z Z (3.18) = x1 p(x1 ; x2 )dx2 dx1 x1 x2 Z Z (3.19) = x1p(x1 x2 )p(x2 )dx2 dx1 x1 x2 Z Z () = x1p(x1 x2 )dx1 p(x2 )dx2 x2 x1 Z = E [x1 x2]p(x2)dx2 x2 =E [E [x1 x2]]; x1 j j j j where the \general conditions" ensure the transition marked with a (). 3.5 Gaussian vectors The only n-variate density we shall consider is the Gaussian one. The mathematical form of the density is almost as dened in (3.14). The only dierence is that the constant is (2) n= instead of (2) , and that now is an n-dimensional vector and P an n n matrix. Similar to the univariate case, we will use the notation x N (; P ) to denote that x follows a Gaussian distribution with mean and covariance matrix P . Two main properties of the Gaussian model are listed in the theorem below: 2 1 Theorem 3.2 Suppose x is Gaussian. Then (a) Y = Ax + b is also Gaussian. This applies to any matrix A and any vector b for which the operations are dened. (b) Divide x into subvectors x1 and x2 . Then the conditional distribution of x1 given x2 (and x2 given x1 ) is also Gaussian with mean and covariance matrix given by E [x1 jx2] = 1j2 = 1 + P 12P 221 (x2 2); (3.22a) 26 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES and VAR[x1 jx2 ] = P 1j2 = P 11 (3.22b) P 12P 221 P 21: Note that the formulae (3.22) are generalizations of the formulae (3.15). The reder should verify that (3.15) are special cases. Both results need lling out. Take the rst one. The parameters of the new Gaussian vector Y are E [Y ] = A + b and VAR[Y ] = AfVAR[x]gAT , both directly obtained from Table 3.3. There are many interesting special cases to that result. Here are two: If A = (1; 1; 1; :::; 1) is a row vector of 1's and b = 0, then Y = x + ::: + xn, i.e. the sum of Gaussian variables are always Gaussian. Secondly, take as A 1 A= Ir 0 ggrn r; where I r is the identity matrix of size r r (i.e. the matrix with 1 on the main diagonal and 0 elsewhere) and the 0's are matrices consisting only of 0's. If b = 0 as before, then Y = x where x is the subvector of x dened earlier. But we have now proved that subvectors of Gaussian vectors must themselves be Gaussian. In order to prove Theorem 3.2, we will need the following lemma: 1 1 Lemma 3.1 Assume (x1 ; x2 ) are jointly Gaussian distribution. If x1 and x2 are uncorrelated, they are also independent. Proof of Lemma 3.1 The two variables x ; x are independent if p(x ; x ) = p(x )p(x ). Now, since P = 0, we have jP j = jP jjP j. Further 1 1 2 P 1 12 = 0 P 111 0 2 11 1 2 22 P 221 which leads to (x )P 1 (x )T = (x 1 )P 111 (x1 1 1)T resulting in that p(x ; x ) = p(x )p(x ). 1 2 1 2 + (x 2 2)P 221 (x2 2 )T ; 27 3.5. GAUSSIAN VECTORS Proof of Theorem 3.2 For the rst part of the theorem, we refer to Mardia et al. (1979). Concerning the second result, we will give a proof thereof, based on the rst result. The subsequent argument is taken from Mardia et al. (1979). Recall that each of the subvectors x and x are Gaussian, as proven above. Introduce 1 x1j2 =x P = I r; 1 12 P 221 x2 P 12P 1 22 2 x1 x2 (3.23) : Since x j is a linear transformation of x = (x ; x )T , it must be Gaussian. The mean vector and covariance matrix are, according to Table 3.3 1 12 E [x1j2 ] = 1 2 (3.24) P 12 P 221 2 and VAR[x j ] =(I r ; P 12 P 221 )P (I r ; P 12P 221 )T P 11 P 12 1 P 12 P 22 ) P 21 P 22 12 =(I r ; since (P )T = P 1 22 1 22 VAR[x j ] = P 12 Ir P 221 P 21 ; and P T = P . Thus 12 21 (3.25) P 12 P 221 P 21; 11 after blockwise matrix multiplication. The cross covariance matrix COV[x j ; x ] is interesting. Indeed, 12 COV[x j ; x ] = E [(x j 12 2 12 E [x1j2])(x2 2 2)T ]; and after x j and E [x j ] are replaced by (3.23) and (3.24) 12 12 COV[x j ; x ] =E [((x =E [(x 12 2 1 1 =P =0: 1) P 12 P 221 (x2 1 )(x2 2 )T ] P 12P 221 E [(x2 12 2 )(x2 2 ))(x2 2)T ] 2 )T ] P 12P 221 P 22 Hence: Lemma 3.2 x1j2, dened in pendent. (3.23) is uncorrelated with x , thus in the Gaussian 2 case also inde- 28 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES The lemma yields a simple derivation for the conditional distribution of x given x . Turn (3.23) (rst line) around so that it reads x =P P x +x j : (3.26) Suppose x is xed. The rst term on the right in (3.26) is then simply a constant, while the other term x j , which is independent of x has the Gaussian distribution stated above, i.e. with mean and covariance matrix as given in (3.24) and (3.25). Thus x must, for xed x , also be Gaussian distributed. The mean vector is P P x + E [x j ] and the covariance matrix is equal to the covariance matrix of x j . Hence, after a trivial rearrangement, inserting (3.24) for j , we have proven the second part of Theorem 3.2. An important corollary of Theorem 3.2 is the following 1 1 1 22 12 2 2 12 2 2 12 1 2 12 12 1 22 2 12 12 Corollary 3.1 Assume x = (xT1 ; xT2 ; xT3 )T where corr[x2 ; x3 ] = 0. Then E [x1 jx2 ; x3] =E [x1 jx2] + P 13P 331 (x3 3 ) VAR[x1 jx2 ; x3] =VAR[x1jx2 ] P 13 P 331 P 31 where i = E [xi ] and P ij = COV[xi; xj ]. Proof Note rst that E [x jx ; x ] = + (P 1 2 3 1 12 ; P 13 ) P 22 0 0 1 P 33 x2 x3 2 3 3 3 = + P P (x ) + P P (x ) However, from Theorem 3.2, the rst two terms on the right hand side is equal to E [x jx ], proving the rst part of the Corollary. The second part is shown similarly: 1 1 22 12 1 2 1 33 13 2 2 VAR[x jx ; x ] =P + (P ; P ) 0 P =P + P P P + P P =VAR[x jx ] + P P P again by Theorem 3.2. 1 2 3 11 12 11 12 1 13 1 22 2 21 13 13 1 33 0 P 22 33 1 33 1 P 21 P 31 P 31 31 3.6 Random processes A random process is a collection of random variables fx(t)g, indexed in time. If plotted against t, the process will describe a stochastic curve with random disturbances 29 3.6. RANDOM PROCESSES superimposed on local and global trends. The practical meaning entails that the individual variables x(t) and x(s) are stochastically dependent. In particular, if t and s are close in time, we would expect their values to be highly correlated. The purpose of this section is to introduce the most important concepts for random processes. Suppose x(t) is dened on an interval from 0 to T . It is possible to regard this as an extension of the situation in the preceding section where n random variables were considered jointly. Now, however, the situation has become even more complicated since there are an innite number of random variables to take care of . We can not specify the distribution for more than a nite number of variables. Fortunately this is enough for a consistent denition. The distribution of a random process is dened by taking points t < t < ::: < tn and specifying a distribution for the corresponding variables x(t ); x(t ); :::; x(tn). This has to be done for all n = 1; 2; :: and all collections of t ; :::; tn inside [0; T ]. It is not obvious that this can be done at all. At least there must be certain consistency requirements. For example, the density of x(t ) follows from the joint one for x(t ); x(t ) (see Section 3.2), and the two densities can not be specied independent of each other. The problem is solved in probability theory, and necessary and suÆcient conditions for consistency have been worked out. We shall take this for granted. Usually the process is dened through some random mechanism. An example is given in (3.29) below. An important concept is that of stationarity. A process fx(t)g is called stationary if the joint distribution of x(t + s); x(t + s); ::::; x(tn + s) does not depend on s. This is to hold for all n and all t ; ::::; tn. The denition says that the probability distribution does not change in time. If the time dierences are the same in a future collection of random variables, the joint distribution remains the same. In particular, all x(t) must for a stationary random process have the same distribution. The mean = E [x(t)] is therefore for such processes a constant and the same goes for the variance. Also note the implication for covariances. If c(t ; t ) = cov[x(t ); x(t )] is the covariance of x(t ); x(t ), then c(t ; t ) = c(t + s; t + s) under stationarity. Take, in particular, s = t , Then c(t ; t ) = c(0; t t ) and the covariance for a stationary process depends on the time dierence only. In practice, a process can only be observed at discrete time points, for example at tk = kÆ , k = 0; 1; 2; ::. The stochastic process is then fx(kÆ )g which we shall denote fx(k)g from now on. The longer the increment Æ, the more information is lost by going from the continuous to the sampled case, but we shall not go into this. Consider the process fx(k)g. Let (k) = E [x(k)] be its mean and dene the covariance to be c(k; l) = cov[x(k); x(l)]. If the process is stationary, (k) = is a constant and c(k; l) = c(l k ) depends only on l k . Stationarity is in practical applications often used in this less stringent sense . 5 1 1 2 2 1 1 1 2 1 2 1 1 1 2 1 1 1 2 1 2 2 1 2 2 2 1 6 countable number is not even since [0; T ] is an interval. former denition is often referred to as or stationarity. If it is only demanded that (k) = and that c(k; l) = c(k l), the process is weakly stationary. 5 The 6 The strict strong 30 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES The function c( ) = cov[x(k ); x(k + )]; valid under stationarity; (3.27) is called the autocovariance function of the process fx(k)g. The function c( ) describes the covariance for variables that are time units apart. Note that c(0) = var[x(k)]. Hence c( ) (3.28) ( ) = c(0) is the corresponding correlation (or autocorrelation). f( )g is known as the autocorrelation function. Although dependence (or correlation) usually is an important ingredient in random processes, uncorrelated processes are important as building blocks for other models. A process containing uncorrelated identically distributed variables is called a white noise process , in contrary to a process with dependent variables which is called a colored process. A random process where x(t ); x(t ); :::; x(tn) is a Gaussian vector for all n and t ; :::; tn is called a Gaussian random process . Such processes will be the only ones we will consider. 1 2 1 Example 2 (cont.) A natural extension of the example in Section 3.3 is, x(k ) = ax(k 1) + w(k 1); k = 1; 2::: (3.29) Here a is a xed coeÆcient and fw(k)g is a white noise process. The term means that E [w(k)] = 0 for all k, the variance is constant and that all autocovariances are 0. Gaussian white noise is a process of independent random variables. Its autocorrelation function is (0) = 1 and ( ) = 0 for 6= 0. Take in (3.29) var[w(k)] = (1 a ) and suppose fw(k)g is Gaussian. Also assume, at the start, that x(0) is normal with mean 0 and variance . It then can be shown that fx(k)g is a stationary Gaussian process with mean 0 and autocovariance function 2 2 2 c( ) = 2a ; = 0; 1; 2; ::: (3.30) The corresponding autocorrelation function becomes ( ) = a ; = 0; 1; 2; ::: (3.31) This example is known as the autoregressive process of order one and also as the Gauss-Markov process. We shall derive the formulae (3.30) and (3.31) in one of the exercises. 2 31 3.7. PROBLEMS 3.7 Problems Exercise 3.1 (Proving some R 1 rules on expectation and variance) (a) Based on E [(x)] = var[ax + b] = a var[x]. 2 1 (x)p(x) dx, show that E [ax + b] = aE [x] + b and (b) Prove Proposition 3.1 Exercise 3.2 (Proving rules on covariance) (a) Show that cov[ax + b; cx + d] = ac cov[x ; x ]. 1 2 1 2 (b) Show that corr[ax + b; cx + d] = corr[x ; x ]. 1 2 1 2 Exercise 3.3 (Conditional Gaussian distribution) By using the denition of the conditional distribution, show the equations for conditional mean and variance in the Gaussian distribution given by (3.15). (Hint: Show that the conditional distribution is on the form (3.4).) Exercise 3.4 (Autocorrelation) Prove the formulae for autocovariance (3.30) and autocorrelation (3.31) assuming the model (3.29). Exercise 3.5 (Random numbers?) Simulation of random variables is important for exploring the properties of a model. An important part of the simulation approach is the ability to perform simulations from a given distribution. Dependent variables can in many cases be constructed by transformation from independent variables, making the generation of independent variables an important building block. Generation of exact independent random variables is an impossible task. One therefore has to stick to pseudo-random variables, which are only approximately following the desired distribution and only approximately independent. In particular, when generating many variables, the variables are constructed in sequence following a deterministic rule. We will in this exercise explore the behavior of our random generator for Gaussian distributions. (a) Simulate 100 variables x ; :::; x from the standard Gaussian distribution (the Gaussian distribution with expectation zero and variance equal to 1). Make a histogram of the variables. Does it look reasonable? (You may also try higher number of variables.) 1 100 (b) Make a plot of xk against xk . Do you see any dependence between xk and xk ? Why is this a good check on the behavior of the random generator? +1 +1 32 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES (c) The number generators in Matlab which generate random numbers use a socalled seed. This seed can be specied by the command randn('seed',hnumi) in Matlab . Generate a vector of length 100 with numbers from randn so that each number is generated with its own index as seed. That is x(k) is generated with randn('seed',k ). Plot this vector. Comment on the result. Exercise 3.6 (Estimation of expectation and variance) Assume x N (; ). In most cases we are in a situation where (the expecta2 tion) and (the variance) are unknown parameters. Assume however that we have observed x(1); :::; x(N ) which all follow the same Gaussian distribution as x above. There is in this case possible to estimate the unknown parameters based on the observations. In particular, estimates for and are given by the formulae N N X X 1 1 ^ = x(k ); ^ = (x(k) ^) : 2 2 N 2 k=1 N 2 k=1 How these estimators are actually obtained will be discussed further in the following chapters. (a) Write a routine in Matlab which has as input ; and N , and rst simulates N independent variables from N (; ), and thereafter calculates estimates of and based on the formulae above. (b) Run the routine you wrote in (a) 20 times and store the estimates for and in two vectors of length 20. Use these vector's to evaluate the properties of the estimators (i.e are they unbiased, what are their uncertainty). (c) Using the denitions and rules for expectations and variances, calculate the expectation and variance of ^. (d) Try also to calculate the expectation of ^ . (Hint: Rewrite x(k) ^ to x(k) + ^ in the formula for ^ .) 2 2 2 2 2 Exercise 3.7 (Estimation of the autocorrelation function) A time series is a sequence of stochastic (random) variables x(k) for k = 1; : : : ; N . The variables may very well be dependent. The autocorrelation function ( ) for x(k) is a measure of the linear dependence between x(k) and x(k + ) for = 1; : : : ; N k. Autocorrelation is the same as correlation, the name is dierent only to explicitly mark that the correlation is between two variables in the same time series. The denition is given in (3.28). Assuming stationarity, the autocorrelation does not depend on which positions, k and k + , of the time series it is measured, but only on the distance ( ) between the two variables. The autocorrelation is estimated by PN N )(x(k + ) xN ) k (x(k ) x N ; = 0; : : : ; N 1 ^( ) = PN N ) k (x(k ) x N 1 =1 1 =1 2 33 3.7. PROBLEMS where xN = N 1 PN k=1 x(k ). (a) Implement a routine in Matlab for calculating the autocorrelation function for a given time series. Try to utilize the possibilities of vectorization. (b) Generate a white noise process fx (k)g where x (k) N (0; 1) for k = 0; :::; 100. Then construct two other processes fx (k)g and fx (k)g by the transformations 7 1 p 1 2 x2(1) = x1 (0); x2(k ) = 0:5x2(k p x3(1) = x1 (0); x3(k ) = 0:5x1(k p 3 1) + 0:5x (k 1); k = 2; :::; 100 p 1) + 0:5x (k): k = 2; :::; 100 1 1 fx (k)g is an example of a so-called autoregressive (AR) process, while fx (k)g 2 3 is an example of a moving average (MA) process. Both types of process will be considered further in Chapter 7. (c) Estimate the autocorrelation functions for each of the time series and plot these as well as the series themselves. Comment on the results. (d) Plot x(k + 1) against x(k) for k = 1; : : : ; N 1 (that is x(k + 1) as a function of x(k)). Comment on the results. (e) For all the three processes, calculate analytically the autocorrelation functions and compare with your results from (c). 7 Rather than using a for-loop for calculating a sum of products, you can f. ex. use sum(x(a:b-1).*x(a+1:b)). Write help arith in if you haven't done this before. The -code will then both be easier to read and faster to execute. Matlab Matlab 34 CHAPTER 3. RANDOM (GAUSSIAN) VARIABLES AND PROCESSES Chapter 4 The general stochastic state space model 4.1 Introduction In Chapter 2, a discrete time deterministic state space model was introduced in (2.7). In this chapter, we will discuss the stochastic version of the state model. Consider a collection of random variables x(k), k = 0; 1; 2; :: associated with time points tk , which may be equidistant (so that tk = kÆ for some common Æ) or they may not. Such a set of random variables is a random process as dened in Section 3.6, the idea being that there exist underlying dynamic forces linking x(k) and x(j ) at dierent time points tk and tj , although not in a completely foreseeable way. There will in engineering frequently be possible to construct dynamic relationships on physical grounds, but there remains almost always some part of the problem that can not be modeled. That part automatically enters the error terms which are often formulated stochastically as we shall do here. Example 3 (random walk) We start by introducing the simplest linear stochastic state space model conceivable. Assume the very special model x(k ) = x(k 1) + w(k 1); (4.1) where w(k 1), k =; 1; 2; ::: are uncorrelated random variables with zero mean and variances var[w(k)] = Q(k): (4.2) The process x(k) dened in (4.1) is called a random walk model and is much used in statistics. The name stands from the fact that the process at tk is the same as it 35 36 CHAPTER 4. THE GENERAL STOCHASTIC STATE SPACE MODEL 10 8 6 4 2 0 −2 −4 0 10 20 30 40 50 60 70 80 90 100 Figure 4.1: Simulation of the random walk process. Q(k 1) = 1 for k = 1; :::; 50 and = 0:1 for k = 51; :::; 100. Observations are simulated with R(k) = 0:5 for all k. The process is displayed by the solid line (linear interpolation). The observations are superimposed as dots. was at tk , except for the random term w(k 1) which is as likely to add to as to subtract from the former value x(k 1). There is no drift in the process, and it can be proved by the results in Chapter 3 that, if all w(k) are stochastically independent, then the mean of x(k) if we know x(k 1) is x(k 1). The process has been simulated in gure 4.1 (solid line) for two values of the variance Q(k ) (Matlab commands are given in Appendix A). Note that the trace may uctuate wildly or be quite smooth. Large values of Q(k) gives birth to unstable processes, diering a lot from one time point to the next one, whereas small values lead to smoothness. A special case worth mentioning is when Q(k) = 0. Eq. (4.1) then reduces to x(k) = x(k 1), and the \process" fx(k)g is a constant. Although this case may at this point seem trivial, we shall see later that many important statistical models are actually conceived in this way. The preceding x-process (and its extension in the next section) is one of two ingredients in a linear state space model. The other element is a relationship connecting x(k) to its actual observation z(k), allowing for error or noise in the measurement. Suppose, for example, that z (k ) = x(k ) + v (k ); (4.3) where v(k) is the error term. It is assumed that v(k) is a process of the same type as the w-process in (4.1), i.e uncorrelated with zero mean and variance var[v(k)] = R(k): (4.4) It is assumed in addition that the two processes fw(k)g and fv(k)g are mutually uncorrelated (i.e. corr[w(k); v(j )] = 0 for all k and j ) or, sometimes a little stronger, 1 4.1. INTRODUCTION 37 stochastically independent. The implication is that the random disturbances aecting the measurements have nothing to do with the randomness in the process fx(k)g itself. This is surely plausible under many circumstances, since it means that the dynamics and the measurements are inuenced by dierent, independent factors. An additional commonly assumed assumption is that w(k) and v(k) should be uncorrelated with x(0). Measurements (simulated) are marked with dots in Figure 4.1. Note how the variation around the solid curves are linked to the size of the variance R(k). We shall see later, when trying to recover the x-process from the measurements, that what really matters is the ratio of the two variances (4.2) and (4.4). This restoration problem is a central one. 2 We shall refer to w(k 1) as the error or noise at time tk , since it adds to the dynamics (here the trivial x(k) = x(k 1)) an element that can only be understood in probabilistic terms. The total collection fw(k)g of noise variables is called a noise process. Such noise processes are essential tools in describing the randomness in the phenomena under study. The symbols w(k) and v(k) will be used to designate noise processes that have mean 0, i.e. for example E [w(k)] = 0, are uncorrelated, i.e. corr[w(k); w(j )] = 0 for k 6= j and have magnitude described by their variances, i.e. var[w(k)] = Q(k). A noise process with uncorrelated variables is usually called a white noise process. In the example above, we assumed there was no structure in the error sequences fw(k)g and fv(k)g. This is a standard assumption made for such models. In some cases there may however be some structure present meaning that errors at neighboring points in time have a tendency to be similar (or, less common, dissimilar). We shall see in Chapter 7 how such eects, often called autocorrelations, can be formulated mathematically and put into the class of linear state space models which utilize as building blocks uncorrelated processes in the sense dened above. Uncorrelatedness can be regarded as an independence concept, although weaker than full stochastic independence that demands all w(k) to be independent. The two notions become equivalent in case of Gaussian processes (see Chapter 3), an assumption we shall make from time to time throughout. 38 CHAPTER 4. THE GENERAL STOCHASTIC STATE SPACE MODEL 4.2 The general stochastic state space model The general stochastic state model is obtained from the deterministic model in eq. (2.7) by addition of process and measurement noise. x(k ) = (k 1)x(k 1) + (k 1)u(k 1) + w (k 1); (system), (4.5a) z(k ) = H (k )x(k ) + v (k ); (observations). (4.5b) In much of the mathematical derivations to follow, the presence of the input functions will complicate the matter without giving extra insight into the problem. We will therefore mainly be working with the more simple model neglecting deterministic inputs: x(k ) = (k 1)x(k 1) + w (k 1); (system); (4.6a) z (k ) = H (k )x(k ) + v (k ); (observations) (4.6b) where k = 1; 2; : : : and (k) and H (k) are deterministic coeÆcient matrices. The state vector x(k) and the observation vector z(k) are vectors of size n and m, respectively, implying that (k) and H (k) are matrices of dimensions n n and m m. The main problem we will consider is how to estimate the state vector based on the observations. The noise processes fw(k)g and fv(k)g are assumed to have the following properties: (k); k = k0 0 COV[w(k); w(k )] = Q (4.7a) 0 k 6= k 0 (k); k = k0 0 COV[v(k); v(k )] = R (4.7b) 0; k 6= k 0 which means that both fw(k)g and fv(k)g are white noise process, and COV[w(k); v(k0)] =0 (4.7c) COV[x(0); v(k)] =0 (4.7d) COV[x(0); w(k)] =0 (4.7e) which state that the two noise processes are uncorrelated with each other and uncorrelated with the initial state. The general model (4.6) reduces to a time-invariant if the parameters f(k)g, fH (k)g, fQ(k)g and fR(k)g do not depend on k. An interesting result, relating stationarity to stability is the following Theorem 4.1 Suppose (k 1) = , Q(k 1) = Q and that is asymptotically stable. Then the distribution of fx(k )g as dened in (4.6a) will, as k increases, converge towards a stationary distribution, that is a distribution not depending on k . 4.3. REMARKS 39 The proof, which is beyond the scope of these notes, is given in Anderson & Moore (1979). In order to nd this stationary distribution, note that when stationarity has occurred, x(k 1) and x(k) follow the same distribution. In particular, their expectations and covariance matrices must be equal. Denote and P the expectation and covariance matrix, respectively, in the stationary distribution. Using (4.6a) and the rules for expectations and variances, we then must have =; (4.8a) P =P T + Q: (4.8b) The rst equation (involving n unknowns) may be solved directly by recognizing that a non-zero value of is only obtainable if has 1 as eigenvalue, in which case is the corresponding eigenvector(s). The second equation is more diÆcult. Taking into account that P is symmetric, there are a total of n(n + 1)=2 unknown. For small n's we may write this out as a set of linear equations which can be solved by standard techniques. In the more general case, a possible algorithm is to start with an arbitrary symmetric matrix P which is put into the right hand side of the equation, giving a new matrix P . This new matrix may again be put into the right hand side to give yet another matrix. Continuing in this manner will in the end give the approeriate matrix. 4.3 Remarks The general model contains f(k)g,fH (k)g,fQ(k)g and fR(k)g as input quantities, in sum a huge number of coeÆcients. Many of them will in practice be determined by the physical situation and often some of them are the same for all k (the variances for example), but that is not to say that the dependency on k is not important. On the contrary, the fact that the theory goes through for arbitrary parameter sequences is of the highest importance for practical modeling and data processing, as we shall see. Usually, especially in statistics, there are a few unknown parameters that must be found empirically by statistical estimation. We discuss this aspect in Chapter 10. In Chapter 7 we will present a number of specic examples that can be put into the linear state space framework, but a few remarks on modeling is best made in this general set-up. One issue is initialization. This means that x(0) must be assigned a value, although not necessarily a xed one. The standard approach is to assume a probability distribution for x(0), usually in terms of a E [x(0)] = (0) (4.9) and a covariance matrix VAR[x(0)] = P (0j0): (4.10) 40 CHAPTER 4. THE GENERAL STOCHASTIC STATE SPACE MODEL To discuss the meaning of these assumptions, consider Example 1 in Chapter 2 where x (k ) was the position and x (k ) the speed at time tk . We might actually know the position and speed at the start. The right hand side of (4.9) is then set equal to these given values whereas the variance matrix on the right of (4.10) would be 1 2 0 0 P (0j0) = 0 0 Since there is no uncertainty regarding the initial conditions, the variances of the initial state is simply zero. The opposite case would be one where nothing is known (a so-called diuse or noninformative prior ). Ideally this means that 1 P (0j0) = 1 ; (the o{diagonal is immaterial), but since innity can not be represented in a computer, a suÆciently large number will do. There are innumerable intermediate positions between these two extremes. For example, if the position was known perfectly, but the speed was completely in the dark, take 0 ; P (0j0) = 1 (again the o-diagonal elements do not matter). There is clearly a great deal of exibility. We shall see in the exercises how the reconstruction of the x-process is inuenced by this initial information. In some situations, we would like to specify P (0j0) somewhere in between these two extremes. This happens when some information is available about x(0), but not in an exact manner. One possibility is for instance that a similar system have been analyzed before, giving some indication on what x(0) should be. Alternatively, experts in the eld may have knowledge about the system which can be used to make some (necessarily imprecise) statement about the initial state. This would correspond to the Bayesian world in statistics, in contrast to the frequentist view in which we put the diagonals to innity. Another point is missing observations . Gaps in an observation sequence is a quite common phenomenon. Sometimes the measurement equipment is deliberately switched o to be used for other purposes, or the technology involved are occasionally subject to bugs and errors. The most natural way of entering missing information into a model is through innite variances in the measurement equation. To be specic, consider the observation of position in the second order model, that is z(k) = x (k) + v(k). One way to express mathematically that observation z(k) is missing, is to specify an innite (or very large) variance for the error v(k). This says that the uncertainty is so great that it overshadows the information in z(k), and the eect of z(k) vanishes 1 41 4.4. PROBLEMS from the computation. An alternative, and perhaps a more subtle way, is to rewrite the measurement equation as z (k ) = H (k )x1 (k ) + v (k ) where ( 1; if z(k) is observed; 0; if it is not. H (k ) = Note that z(k) = v(k); if H (k) = 0, but now there is no relation to the x-process, and z (k ) becomes valueless. This second option (which is exact whereas the rst one is only approximate) is at least as good as the other. Note that by using these techniques any observational pattern, however complicated, can conveniently be expressed and fed to the computer. The Kalman lter described in Chapter 6 is available to estimate a general x-process. 4.4 Problems Exercise 4.1 (Random walk, extended) Consider a slight generalization of eq. (4.1) giving the system equation: x(k ) = ax(k 1) + w(k 1); where we will assume w(k 1) is Gaussian distributed with expectation equal to zero and variance . 2 (a) Write a routine in Matlab with input a, and N which simulates the system for k = 1; :::; N with x(0) = 0. (Hint: The routine randn(N) generates N Gaussian distribution variables with zero expectation and variance equal to 1. By multiplying the variables you get with , you obtain new variables with variance equal to .) 2 (b) Run the routine in (a) for N = 1000; = 1 and a = 0:3; 0:9; 1:0 and 1:01, and plot the series. Comment on the results. (c) Try to relate the results in (b) to the stabilization concept in Chapter 2. (d) Adjust your routine from (a) so that x(0) is drawn from a Gaussian distribution with mean zero and variance P (0j0), where P (0j0) is large (for example equal to 10000). Simulate again the process for dierent values of a. Comment on how x(1000) depends on the initial state. 42 CHAPTER 4. THE GENERAL STOCHASTIC STATE SPACE MODEL Exercise 4.2 (Simulation of two-dimensional systems) Consider a stochastic system model of the form x1 (k ) x2 (k ) = x1(k x2(k 1) + 0 1) w(k 1) where fw(k)g is a white noise process with w(k 1) N (0; ). 2 (a) Write a Matlab routine with input , , T and N which simulates the system process for k = 1; :::; N . Specify the initial conditions yourself. (b) Consider now a stochastic version of the (discretized) moving body example (Example 1) from Chapter 2 given by x (k ) 1 T x (k 1) 0 x (k ) = 0 1 x (k 1) + w(k 1) : Use the routine written in (a) to simulate the process for T = 0:5 and k = 1; :::; 100. Use dierent values of , and plot the results. (c) Repeat (b) but now with the stochastic version of the (discrete) pendulum movement system in Exercise 2.3, that is = expfA T g, where 0 1 A= g=L 0 : Use g = 10:0; L = 1:0 and T = 0:5. (d) Repeat (b) once more but now including air resistance into the pendulum model (Exercise 2.4), that is = expfA T g, where now 0 1 A= : 2 1 1 2 2 2 g=L c=L Use again g = 10:0; L = 1:0; c = 0:5 and T = 0:5. (e) Comment on the results above. In particular, try to relate the results to the stabilization concept in Chapter 2. (f ) From Theorem 4.1, we then know that the systems which are stable will converge to a stationary distribution. Use (4.8) to nd the stationary distribution for this (or these) system(s). (Hint: It is not possible to nd P through matrix manipulations. You will have to write down the linear equations for the components in P and solve this system. Chapter 5 Estimation of random variables Recovering the x-process from measurements is the essential problem for models as described in Section 4.2. This estimation problem dier in an important aspect from the more standard parameter estimation problems considered in introductory courses in statistics. In this case the quantity of interest, x(k) is a random variable, and not a xed constant as is the case for parameters. In principle, there are many ways of solving the particular estimation problem involved for the dynamic linear model. One possibility is to base an estimation process on the observation model (4.6b) alone. For instance, if we have the univariate model n = m = 1 with H (k ) = 1, we could estimate x(k ) by x^(k ) = z (k ). Such an estimate is unbiased, since E [^x(k ) x(k )] = E [z (k ) x(k )] = E [v (k )] = 0; but the variance is as large as that of the original observation. Usually x(k) will be a smooth process, and there is an enormous benet in utilizing the dynamics in the x-process to aid the reconstruction through feedback, which the former, naive attempt did not do. We discuss how this idea is carried out in Chapter 6 by introducing the Kalman lter that in a certain sense is the best procedure that can be constructed. The Kalman lter applies to linear stochastic state space models and organizes the computations in an elegant recursive manner. We introduce this class of models in the next section. In this chapter we will discuss estimation of random variables in a more general setting, which will give us the basis for deriving the Kalman lter. Section 5.1 consider how to construct optimal estimators in general, while Section 5.2 specialize the results from Section 5.1 to Gaussian distributions. 43 44 CHAPTER 5. ESTIMATION OF RANDOM VARIABLES 5.1 Optimal estimation in general. When deriving the Kalman lter, we will be concerned with estimation of an unobserved random vector x 2 Rn based on some observed random vector z 2 Rm . Let p(x; z ) = p(xjz)p(z ) (5.1) be the joint density of (x; z), which is factorized into the product of the conditional density of x given z and the marginal density of z. Consider the conditional mean (or regressor) j ^ = E [x z] = x Z j xp(x z)dx; (5.2) which (obviously) is a function of z. We shall sometimes write (when we want to emphasize this dependency) x^ = x^ (z). The conditional mean has the following important properties: Theorem 5.1 ^ = x^ (z) is given by Assume x (a) E [^x(z) (b) E [(^x(z) E [(^x(z ) (c) E [(^x(z) Then the following holds: 0 x] = . x)T g (z )] = 0 for any function g (z ) : ~ (z) : x)~ g (z)T ] = for any function g 0 R !R m (5.2). x)T (^ x(z ) n. x)] E [(h(z) Rm ! Rn and Rm ! Rp x)T (h(z ) x)] for any function h(z ) : The interpretation from an estimation point of view is immediate. Suppose x^ (z) is used as an estimate for x. Then (a) implies that the estimate is unbiased, i.e. the estimation error is zero in the mean. The error is also uncorrelated with the observation vector z (from (b)), and the average error is smaller than for any other alternative method according to (c). Proof of Theorem 5.1 We will use the double expectation rule. Start by observing that E [(^x(z ) x)T g (z )] =E [E [(^ x(z ) j x)T g (z ) z]] E [x z])T g (z )] =E [(^x(z) j =E [(^x(z) x^ (z))T g(z)] =0; 5.2. OPTIMAL ESTIMATION IN THE GAUSSIAN CASE 45 and the rst part of property (b) of the theorem follows. The second part is shown similarly, only replacing g with g~. (a) is simply a specialization by taking g~ (z) 1. To establish (c) note that (h(z) x)T (h(z) x) =(h(z) x^ (z))T (h(z) x^ (z)) + (^x(z) x)T (^x(z) x) + 2(^x(z) x)T (h(z) x^ (z)): Take expectations on both sides. Then the last term on the right vanishes (use ^ (z) in (b)). Hence g (z ) = h(z) x E [(h(z) x)T (h(z ) x)] = E [(h(z) x^ (z))T (h(z) x^ (z))] + E [(^x(z) x)T (^x(z) x)] E [(^x(z) x)T (^x(z) x)]; since the rst term on the right of the rst line can not be negative. 5.2 Optimal estimation in the Gaussian case Theorem 5.1 provides a strong argument for estimating a random quantity through its conditional expectation given the observations. This may be extremely diÆcult computationally with the dynamic model in Section 4.2, even with complete knowledge of the statistical properties of the error processes. However, there is one type of situation where the problem simplies nicely, i.e. when both error processes in the state space model (4.6) are Gaussian. The nal results along this line will be proved in the next chapter, but we shall go a long way in the present one by reviewing some basic results about conditional distributions for Gaussian random variables. In Section 3.5 we showed that if (x; z)T is Gaussian with mean vector = x z and covariance matrix P = P xx P xz P zx P zz ; then the distribution of x given z is also Gaussian with mean vector xjz = x + P xz P zz (z z ) and covariance matrix P xjz = P xx P xz P zz P zx : 1 1 (5.3) (5.4) 46 CHAPTER 5. ESTIMATION OF RANDOM VARIABLES Both the identities (5.3) and (5.4) are important from an estimation point of view. Suppose x is some unknown vector to be estimated from observations z. Then Theorem 5.1, in combination with (5.3) shows that the optimal estimator x^ = E [xjz] = xjz is in the Gaussian case a linear function of z . Applied to the ltering problem of Section 4.2 this implies that the optimal estimator for x(k) must be a linear function of z(1); :::; z(k) when the noise processes in (4.6) both are Gaussian. The other identity reveals the remarkable property that the conditional covariance is independent of the given observations z . Consider this in the estimation context. We know that VAR[x xjz jz] = E [(x xjz)(x xjz)T jz] = P xjz by denition. Note that (x xjz ) coincides with the estimation error ^ x: e=x Hence E [eeT jz ] = P xjz : Then, from the double expectation rule (Theorem 3.1) E [eeT ] = E [E [eeT jz ]] = E [P xjz ] = P xjz : (5.5) Since, by property (a) of Theorem 5.1 e has expectation 0, this is equal to the covariance matrix for e , VAR[e ] = P j ; and we have in the realm of Gaussian models a particularly simple expression for the (unconditional) estimation errors, which can be computed ahead of the actual data, as can the conditional one. Note that although for our linear models, the unconditional variances of the estimation errors coincides with the conditional ones, this is not a general result. Furthermore, their interpretations are dierent. The conditional variance reects the uncertainty involved for those particular data that we have observed, while the unconditional variance denes the uncertainty involved if we were able to repeat our experiments several times. 1 1 1 12 5.3 Problems Exercise 5.1 (Gaussian estimation) Let be an unknown quantity. Assume there exists one single measurement y where y = h + v: Here h is a parameter known from the measurement technology while v is a noise term which we assume is Gaussian distributed with expectation 0 and variance R. Also assume that is a Gaussian random variable with expectation and variance P . 47 5.3. PROBLEMS (a) We will assume in the whole exercise that there is no dependence between and v . Discuss the validity of this assumption. (b) Show that y is Gaussian distributed with expectation y = h and variance Py = h P + R. (c) Calculate cov[; y]. Specify also corr[; y]. (d) Show that the conditional distribution for given y is Gaussian with expectation 2 E [jy ] = + hP (y h P + R 2 h ) and variance var[jy] = P h PR+ R : 2 (e) Write down the \best" way of estimating based on y. We will in the following interpret the expressions for E [jy] and var[jy] based on the estimation problem dened in the beginning of the exercise. (f ) Analyze how the expressions depend on P . Try to explain the structure of this dependence. (g) What happens when P ! 1? Try to explain which result this is a special case of. (h) How do the expressions for E [jy] and var[jy] depend on R? Explain. (i) What happens with the two expressions when R = 0? Explain again. (j ) And how is the dependence on h? Explain once more. Exercise 5.2 (Prior information from earlier experiments) In Exercise 5.1 it was assumed that some knowledge about was available before the measurement y was collected (we knew the expectation and variance of ). Assume now that we do not know anything about . Then we get to know the following: A measurement z = + u, where u is Gaussian noise with expectation 0 and variance P , has resulted in the value z = . Explain why we after measuring z (but before measuring y) have exactly the same type of a priori knowledge about as in Exercise 5.1. (A priori because we have no realizations of y yet.) Exercise 5.3 (A simple state space model (AR(1)) { part I) Consider the model x(k + 1) = ax(k) + w(k) for k = 1; : : : ; N . Here x(1) N (0; ). The random variables w(1); w(2); : : : ; w(N ) are independent and identical distributed N (0; Q). They are also independent of x(1). The constant a is assumed to be in the interval ( 1; 1). 2 48 CHAPTER 5. ESTIMATION OF RANDOM VARIABLES (a) Explain why x(k) for all k = 1; : : : ; N are Gaussian distributed and show that (k ) = E [x(k )] = 0. (b) Derive a set of dierence equations for px (k ) = var[x(k )] = E [(x(k ) (k )) ] = E [x(k ) ] by taking the variance on each side of the equality sign in the model. Show that we get px(k + 1) = a px(k) + Q. Why is this deterministic dierence equation stable? (c) When k ! 1, then px(k) will converge towards a limit px. Show that px = Q=(1 a ). (d) Assume = 1 and Q = 4. Use Matlab for plotting px (k) for all k = 1; : : : ; 20 for a = 0:7 and a = 0:4. Try to make some conclusions on your results. (e) Let a = 0:7 and make a large number of realizations of the process. Plot the empirical distributions at time points k = 1, k = 3 and k = 15 in Matlab . In the same plots, place the theoretical Gaussian distributions for x(1), x(3) and x(15). The similarity should be good. Be sure you understand why this comparison makes any sense. (Hint: The Matlab routine hist draws a histogram (empirical distribution) of a dataset. The hold routine may be useful in making several gures in the same plot.) 2 2 2 2 Exercise 5.4 (A simple state space model (AR(1)) { part II) We will consider the same model as in Exercise 5.3, but now use Q = (1 a ). 2 2 (a) Explain why all x(k) now have the same distribution. Make a plot of the distribution for = 1. Which values would you use for estimating an unknown x(k)? (b) Find the conditional distribution for x(k) given x(k 1). Make a plot also for this distribution and compare with the one you found in (a). Let a = 0:7 and assume x(k 1) = 0:5. How would you now estimate x(k)? (c) Show that the conditional distribution for (x(k); x(k + 1)) given x(k 1) is a Gaussian distribution with expectation vector a a2 x(k 1) and covariance matrix 1 a a 1 + a (1 a ) : 2 2 2 5.3. PROBLEMS 49 Make a plot of the joint conditional distribution for (x(k); x(k+1)) given x(k 1). How would you estimate (x(k); x(k + 1)) if x(k 1) was known? (Hint: The routines meshgrid and mesh may be useful.) (d) Find the joint unconditional distribution for (x(k); x(k + 1)). Make a plot of the distribution and compare with the one you made in (c). How would you estimate (x(k); x(k + 1)) if x(k 1) was unknown? (e) Use the result from (c) to nd the joint distribution for (x(k 1); x(k); x(k +1)). (f ) What is the conditional distribution for x(k) given (x(k 1); x(k + 1))? Try to explain why this conditional distribution is equal to the conditional distribution for x(k) given x(1); : : : ; x(k 1); x(k + 1); : : : ; x(N ). 50 CHAPTER 5. ESTIMATION OF RANDOM VARIABLES Chapter 6 The discrete time Kalman lter Our objective in this chapter is to construct estimators x^ (k) for x(k) at time k with error estimates. This is usually called state estimation. Since we are assuming that each observation z(k) is becoming known at exactly time instant k (or rather some tk ), the data available for the estimation of x(k) is z(1); :::; z(k). The Kalman lter provides the best estimate of x(k) that can be obtained through linear estimators (6.1), i.e. it is of the form ^ (k) = ak (k)z(k) + ak (k 1)z(k 1) + :::: + ak (1)z(1); x (6.1) where fak (l)g are coeÆcients to be adapted from knowledge of the model and some initial information about x(0), the start vector of the system process fx(k)g. The coeÆcients will obviously vary from one point in time to another, explaining the index k on the a's. The estimation error when x(k) is estimated by x^ (k) is ^ (k) x(k): e(k ) = x (6.2) The uncertainty of the estimation error is of vital interest when applying the Kalman lter. We will see that uncertainty measures based on variances are to be computed as part of the scheme. There are many derivations of the Kalman lter in the literature. Often ad hoc assumptions (such as linearity of the lter) are introduced a priori to simplify the computations. The derivation given here has the advantage of being rigorous and self{contained in addition to give a useful background in general estimation theory. Throughout the discussion, we will for simplicity assume that the input sequence u(k ) = 0 for all k . Non{zero input functions can easily be incorporated, but would only make the derivations to follow more complex without gaining any extra insight into the main ideas. The extension of the Kalman lter to non-zero input-vectors will therefore only be presented without proof at the end of the chapter. 51 52 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER Throughout this chapter the sequences f(k)g, fH (k)g, fQ(k)g and fR(k)g are regarded as fully known. The Kalman lter is a linear estimation method. Some notation and concepts will be introduced in Section 6.1. The Kalman lter will then be derived under Gaussian conditions in Section 6.2. In Section 6.4 we illustrate the use of the Kalman lter through a few examples. Section 6.5 demonstrates by a very simple argument that the Kalman lter actually also obtains the best linear estimate in the general non-Gaussian case. Finally in Section 6.6 we will generalize the lter for non-zero deterministic inputs. 6.1 Preliminaries Go back to the problem of Section 4.2, i.e. that of estimating x(k) as accurately as possible on the basis of 0 1 z (1) B z (2) C C ~z (l) = B (6.3) B .. C : @ . A z(l) Dene ^ (kjl) = E [x(k)j~z(l)] x (6.4) for general k and l. The solution to the estimation problem above is given by Theorem 5.1 as the conditional mean x^ (kjk) = E [x(k)j~z(k)]. This quantity can in the Gaussian case, at least in principle, be computed through the results of the preceding chapter. The point is that when both error processes fw(k)g and fv(k)g in (4.6) are Gaussian, then, according to Theorem 3.2, so is x(k) and ~z(k) (and jointly too), and the formula for conditional expectations (5.3) yields x^ (kjk) after the identication of the appropriate cross covariance matrices. In practice this is not the way we do it, a major drawback being that the whole computation has to be done all over again for each single k. Even worse, the approach requires the inversion of the covariance matrix of ~z(k) (corresponding to P in (5.3)), which turns into a huge matrix as the time is running and k becomes larger and larger. Surely we should seek some recursion that refers as much as possible of the computations at k back to what has already been done at k 1 and earlier. This is exactly what is accomplished by the Kalman lter, although the ensuing derivation will show that it is really more than merely a convenient computational tool. Introduce rst some more mathematical notation. We shall need the auxiliary conditional mean ^ (kjk 1) = E [x(k)j~z(k 1)] x (6.5) 22 53 6.1. PRELIMINARIES which is the predictor using information up to k 1 only. (Estimation of future random variables is usually denoted prediction .) The estimation error when estimating x(k) by x^ (kjl) is ^ (kjl) x(k): e(k jl) = x (6.6) The variability of this term is important in order to evaluate the quality of the estimate. A popular variability measure is the covariance matrix. Recall from Theorem 5.1(a) that x^ (kjl) x(k) has zero mean. Hence the error has covariance matrix P (k jl) = VAR[e(k jl)] = E [(^ x(k jl) x(k ))(^ x(k jl) x(k ))T ]: (6.7) This covariance matrix measures the variability we will expect using x^ (kjl) without taking the actual values of ~z(l) into account, that is a measure of performance of a procedure. When the estimation is to be performed at time l, the observations ~z (l) are available, and in this case it is more natural to consider the conditional covariance matrix VAR[e(kjl)j~z(l)]. In general, VAR[e(kjl)j~z(l)] 6= VAR[e(kjl). However, as shown in Section 5.2, these covariance matrices coincide under Gaussian assumptions. For the derivation of the Kalman lter, we will in particular be interested in the situation where l = k, while the case l = k 1 will be important as part of the algorithm. For future prediction the case l < k in general will be of interest and is discussed in Section 9.1. In Chapter 9, considering so-called smoothing, also the case l > k will be treated. Another important concept, is the so-called innovation process, dened through ~ (kjk 1) = z(k) H (k)^x(kjk 1): z (6.8) To motivate the name recall from (4.6b) that E [z(k)jx(k)] = H (k)x(k) so that the term H (k)^x(kjk 1) would be our natural guess for z(k) before it was actually observed, i.e. at time k 1. The dierence z(k) H (k)^x(kjk 1) may thus be interpreted as the unexpected part of the observation z(k), that is an innovation we were not able to foresee one time unit earlier. The innovation process fz~ (kjk 1)g plays a central role, not only for the Kalman ltering theory to be derived here, but for linear ltering in general. Lemma 6.1 summarizes its main properties. Lemma 6.1 The innovation sequence dened by (6.8) has the following properties: E [~z (k jk 1)] = 0; COV[~z(k jk 1); z (k 0 )] = 0 for all k 0 < k; S (k ) = VAR[~ z (k jk 1)] = H (k )P (k jk 1)H (k )T + R(k ): where R(k ) is the covariance matrix of v (k ), cf. (4.7b). (6.9a) (6.9b) (6.9c) 54 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER The innovation at time k has thus mean zero and is uncorrelated with all observations prior to k (property (6.9b)). The latter observation is extremely important. It will in the present sequel enable us to simplify the argument leading to the Kalman lter. Later (in Chapter 10) we shall see how it yields a neat decomposition of the joint density functions of the observations. Proof of Lemma 6.1 Start by inserting (4.6b) for z(k) into (6.8). Then ~ (kjk 1) = H (k)(x(k) x^ (kjk 1)) + v(k); z (6.10) and after replacing x^ (kjk 1) by (6.5) ~ (kjk 1) = H (k)fx(k) E [x(k)j~z(k 1)]g + v(k): z (6.11) We know from Theorem 5.1(a) that x(k) E [x(k)j~z(k 1)] has mean zero. This yields property (6.9a) of the lemma, since E [v(k)] = 0. To establish (6.9b), observe that the rst term on the right of (6.11) is uncorrelated with ~z(k 1) (by Theorem 5.1(b)) as is also the second term v(k). Hence the sum must be uncorrelated too, which is the statement to be proved. The formula for VAR[~z(kjk 1)] is almost immediate. Indeed, the two terms in (6.10) are by model assumptions uncorrelated. Hence VAR[~z(kjk 1)] =VAR[H (k)(x(k) x^ (kjk 1))] + VAR[v(k)] =H (k)VAR[x(k) x^ (kjk 1)]H (k)T + VAR[v(k)] =H (k)P (kjk 1)H (k)T + R(k): Here the second line is a consequence of the rules for operating covariance matrices, as given by Table 3.3, and the third line follows after recalling (6.7) and (4.7b). 6.2 The Kalman lter under Gaussian conditions The main structure of the Kalman lter is the double recursion ^ (kjk 1) = (k 1)^x(k 1jk 1) x (prediction) (6.12a) ^ (kjk) = x^ (kjk 1) + K (k)(z(k) H (k)^x(kjk 1)) (updating); (6.12b) x where K (k) is a deterministic weight matrix known as the Kalman gain. Note that the second term in (6.12b) equals K (k)~z(kjk 1), where z~ (kjk 1) is the innovation dened in (6.8). Thus the updating of x(k) when z(k) becomes available, amounts to modifying the former estimate x(kjk 1) by a quantity proportional to the innovative part of the new observation. The computation of the Kalman gain K (k) is done jointly with the error covariance matrices P (kjk) and P (kjk 1) in one single recursion, also involving S(k), the 55 6.3. DERIVATION OF THE FILTER covariance matrix for the innovation, as dened in (6.9c) which is of interest in its own right (see Chapter 10). The most common version of the algorithm, the covariance lter, is as follows: P (k jk 1) = (k 1)P (k 1jk 1)(k 1)T + Q(k 1) (6.13a) T S (k ) = H (k )P (k jk 1)H (k ) + R(k ) (6.13b) K (k ) = P (k jk 1)H (k )T S (k ) (6.13c) P (k jk ) = (I n K (k )H (k ))P (k jk 1): (6.13d) Clearly this is a recursion, since P (k 1jk 1) at the right of the rst equation is transferred to an expression for P (kjk) in (6.13d). In the literature the scheme is often written with fewer equations. This has no advantages and, moreover, all the quantities computed in (6.13) have a separate role to play. In (6.13c) the inversion of S(k) is required. A suÆcient condition for this inverse to exist is that R(k) is positive denite, which we will normally assume. In cases where this condition not is satised, some more care has to be taken, although the inverse might exist even in many of these cases. Several other algebraically equivalent versions of the recursion (6.13) exist (cf. Chapter 8). These are of interest because they may provide additional insight or have improved numerical properties. Also note that the observations do not go into this second part of the Kalman algorithm at all. In fact, we practically foresaw that earlier after having derived the formula (5.4) for the covariance matrix of conditional Gaussian distributions. The deeper reason for the data independence can be sought in that result. The property has great practical interest for on-line applications, since it means that the Kalman gains, the hardest part of the algorithm, can be computed and stored prior to the actual data processing. 1 6.3 Derivation of the lter We will prove the equations (6.12) and (6.13) through four steps. Step 1 Consider rst the prediction step (6.12a) and the prediction error covariance matrix (6.13a). Clearly ^ (k x jk 1) = E [x(k)j~z(k 1)] = E [(k 1)x(k 1)j~z(k 1)] + E [w(k 1)j~z(k 1)] =(k 1)E [x(k 1)j~z(k 1)] + E [w(k 1)] =(k 1)^x(k 1jk 1) + 0; (6.5) (4.6a) 56 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER which is (6.12a). A central ingredient of the argument is that w(k 1) is uncorrelated with everything going on before k, in particular ~z(k 1). Similar reasoning yields (6.13a). Start out with ^ (k x jk 1) x(k ) = (k 1)(^x(k 1jk 1) x(k 1)) w (k 1): Take covariance matrices on both sides, recalling that P (kjk 1) = VAR[^x(kjk 1) x(k )]. Then jk 1) =VAR[(k 1)(^x(k 1jk 1) x(k 1)) w(k 1)] =(k 1)VAR[^x(k 1jk 1) x(k 1)](k 1)T + VAR[w(k 1)] =(k 1)P (k 1jk 1)(k 1)T + Q(k 1); P (k which is (6.13a). Step 2 Note that (6.13b) coincides with property (6.9c) of Lemma 6.1, and it remains to prove (6.12b), (6.13c) and (6.13d)). The argument relies on the formula (3.22a) for Gaussian conditional expectations. Observe rst that ~z(k) and (~z(k 1); z~ (kjk 1)) mutually determine each other; see (6.8). Thus x^ (kjk) = E [x(k )j~z (k )] can also be written ^ (k x jk) = E [x(k)j~z(k 1); z~ (kjk 1)]: (6.14) Consider the random vector 0 1 x(k ) @ ~z(k 1) A ~ (kjk 1) z with mean vector 0 1 x (k ) @ ~z (k 1) A 0 and covariance matrix 0 P xx (k ) P x~z (k ) P xz (k ) @ P ~zx (k ) P ~z~z (k ) 0 P zx (k ) 0 P zz (k ) ~ ~ 1 A: ~~ Here x(k) and ~z (k 1) are the means of x(k) and ~z(k 1) respectively while P xx (k ) is the covariance matrix of x(k ), P x~z (k ) the cross covariance matrix between ~ (kjk 1) x(k ) and ~z (k 1) and so on. Note the zero mean for the innovation process z 57 6.4. EXAMPLES and the zero cross covariance between z~ (kjk 1) and ~z(k 1) are consequences of Lemma 6.1. Apply Corollary 3.1 to the random vector with x = x(k); x = ~z(k 1) and x = z~ (kjk 1) to see that 1 2 3 ^ (k x jk) =E [x(k)j~z(k 1)] + P xz (k)P zz (k)~z(kjk 1) =E [x(k)j~z(k 1)] + K (k)~z(kjk 1); ~ 1 ~~ (6.15) But we have now arrived at (6.12b) with K (k ) = P xz~ (k )P z~ z~1 (k ): (6.16) Step 3 The next step is to nd a suitable expression for P (kjk), dened in (6.7). From (5.5) it is clear that P (kjk) = VAR[x(k)j~z(k)] = VAR[x(k)j~z(k 1); z~ (kjk 1)]. Again we can apply Corollary 3.1 to see that P (k jk) = P (kjk 1) P xz~ (k )P z~ z~ (k ) 1 P z~ x (k ): (6.17) Step 4 For the termination of the argument we have to derive P xz (k), P zx(k) and ~ P z~z~ (k ) ~ in (6.16) and (6.17). The latter has also been called S(k) and is taken care of already. For P xz (k): ~ ~ (k P xz~ (k ) =COV[x(k ); z jk 1)] =E [x(k)~z(kjk 1)T ] = E [x(k)(H (k)(x(k) x^ (kjk 1)) + v(k))T ] =(E [x(k)(x(k) x^ (kjk 1))T ])H (k)T + E [x(k)v(k)T ] =fE [(x(k) x^ (kjk 1))(x(k) x^ (kjk 1))T ] + E [^x(kjk 1)(x(k) x^ (kjk 1))T ]gH (k)T + E [x(k)v(k)] = [P (kjk 1) + 0]H (k)T + 0; (6.18) (6.10) (6.7) where the rst zero is a consequence of the prediction error x^ (kjk 1) x(k) being uncorrelated with any function of ~z(k 1) (Theorem 5.1(b)), x^ (kjk 1) in particular, and the second zero is by model assumption directly. When the identities P xz (k) = P (kjk 1)H (k)T , P zz (k) = S(k) and P zx(k) = P Txz are inserted into (6.16) and (6.17), (6.13c) and (6.13d) follow. ~ ~~ ~ ~ 6.4 Examples In a few situations, explicit analytical expressions can be obtained from the Kalman lter 58 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER Example 4 (iid variables) Consider the simple (but important) example x(k ) = x(k 1) (6.19a) z (k ) = x(k ) + v (k ) (6.19b) where v(k) N (0; ). This actually corresponds to a situation where, for given x(k), all z(k) are independent and identically distributed (iid) N (; ) where = x(k). Note that there is no noise in the system equation in this case. Consider now rst the updating equations for the covariance matrix P (kjk). By (6.13a), P (k jk 1) = P (k 1jk 1) which should be obvious because of (6.19a). Further, S (k ) = P (k 1jk 1) + ; P (k 1jk 1) ; K (k ) = P (k 1jk 1) + and P (k 1jk 1) P (k jk ) = ; P (k 1jk 1) + where the last equation gives a recursive equation for calculating P (kjk). We may rewrite the equation as 2 2 2 2 2 2 2 2 = 1 + P (k jk ) P (k 1jk 1) which shows that P (kjk) < P (k 1jk 1). Thus, the error variance decreases mono- tonically as each new observation is processed. Now assume P (0j0) = 1, corresponding to no prior information about . Then it is easy to see that 2 2 = k and P (k jk ) = : P (k jk ) k (6.20) Further, the Kalman gain is in this case 2 k = k1 : K (k ) = 2 k + giving that 1 x^(k jk ) =^x(k 1jk 1) + (z (k ) x^(k 1jk 1)) k = k 1 x^(k 1jk 1) + 1 z(k) 1 1 k 2 k (6.21) 59 6.4. EXAMPLES For k = 1, we obviously get x^(1j1) = z(1), and by induction it follows that in general k X 1 x^(k jk ) = z (i) = z(k ): k (6.22) i=1 Now, of course, in this simple situation we would normally not go through the whole Kalman lter framework, but rather work on the problem more directly (as is done in elementary statistics courses). An interesting consequence of applying the Kalman lter to this problem is however equation (6.21) which gives a recursive algorithm for calculating the mean. Such a recursive calculation is more numerical robust than the more direct method of rst summing the variables and then dividing by k. In Chapter 10 we will consider methods for estimating the variance and also in this case we will see that a recursive equation appears. 2 2 Here the Kalman lter is seen to provide the classical solution to a well known problem. The Bayesian version is also easily accommodated. We will consider this issue in one of the exercises The above example can actually be considered as a special case of linear regression which also can be formulated as a stochastic state space model, and for which also analytical expressions for the estimates exist. We will consider this further in Chapter 7. The situations for which simple analytical expressions are available are in general rather the exception than the rule. Even for one-dimensional examples the calculations quickly become intractable to carry out by hand. Example 3 (random walk, cont.) Consider the (small) generalization of Example 4 where we now add noise to the system equation giving the new model x(k ) = x(k 1) + w(k z (k ) = x(k ) + v (k ): 1); (6.23a) (6.23b) This is the random walk model considered in Section 4.1. In this case analytical expressions for x^(kjk) and P (kjk) become rather messy. Still however, their values are easily calculated by programming the Kalman lter equations into the computer. Assume, as before, that Q(k 1) = 1:0 for k = 1; :::; 50 and = 0:1 for k = 51; :::; 100 while R(k) = 0:5 for all k. Using the simulated data displayed in Figure 4.1, estimates of x(k) can be obtained. These are shown as dashed lines in the top panel of Figure 6.1. Because we have used simulated data, we can in this case compare the estimates with the true x-process, which is shown as a solid line. The lower panel of the same gure shows the estimation error standard deviations for dierent k's. For the rst k's, the variance is relative large. This is because the estimate of x(k) in this case is based on 60 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER 10 8 6 4 2 0 −2 −4 0 10 20 30 40 50 60 70 80 90 100 0.75 0.7 0.65 0.6 0.55 0.5 0.45 0.4 0 10 20 30 40 50 60 70 80 90 100 Figure 6.1: In the upper panel, estimates of the random walk process obtained from the Kalman lter based on simulated observations (shown as stars) are displayed. The Kalman lter estimates are shown as a dashed line, while the true system process is shown as a solid line. The lower panel shows the estimation error standard deviations P (kjk). The simulated data in Figure 4.1 was used. p only a few observations. After 3-4 time steps, the variance seems to stabilize. This is a typical behavior, and will be discussed further in Chapter 8. The large decrease around k = 50 is due to that Q(k) decreases from 1 to 0.1 at this point. 2 Example 1 (A moving body, cont.) Assuming only random disturbances acting on the body in Figure 1.1, we get the following discrete representation x1 (k ) x2 (k ) = 1 T 0 1 x1(k x2(k 1) + w (k 1) 1) w (k 1) (6.24a) 1 2 where T is the sampling time. The system noise may represent forces due to wind gusts and turbulence and is assumed to be white and satisfy the other criteria in the state space model (4.6). In the special case Q (k) = 0; Q (k) = 0, w (k) = 0; w (k) = 0 and so x (k) = a + kT b, where a = x (0) and b = x (0) are the initial position and speed at the start. The presence of non-zero random errors means uctuations and deviations from this deterministic pattern. Such a model, but with movements in 1 1 1 For 11 1 22 1 2 2 discretized the body we are actually talking about continuous noise processes that are . The details in this case are much more involved than discretization of deterministic inputs and are beyond the scope of these notes. Practical approaches to noise discretization are presented IN 358. A thorough theoretical understanding requires stochastic calculus. 61 6.4. EXAMPLES two dimensions simultaneously (so that there are, say x and x -processes as well) is actually used in sea navigation. The random terms would then be due to, for example, uctuations in wind or current. If position is measured, we have 3 z (k ) = 1 0 4 x1(k ) + v (k ): x2(k ) (6.24b) where v(k) (with variance R(k)) is the error in measuring x (k). If only x (k) is measured, the measurement matrix becomes instead H = (0 1) and the measurement error is now, of course, the error in measuring x (k). Figure 6.2 shows simulations of the model with Kalman lter estimates. The simulated observations are indicated by asterisks, while the states and their estimates for convenience are displayed by solid and dotted lines, respectively. (Of course, both the states and their estimates are only dened at each integer k.) Both initial estimates are zero since in this case x^ (0j0) = E x(0) = 0. The x estimate follows the real state quite well, while the estimate for x seems to lag behind the true process to some extent. Such lags between estimates and actual state vector components occur frequently. There are two reasons for this. Firstly, there are typically fewer observations than state vector components at each time point. The lter then obtains its reconstructions by combining observations over several time points. Secondly, the innovations in the updating step have to be treated conservatively and be weighted properly by the matrix K (k) because of the measurement noise. The results in Figure 6.2 represent one realization of the x and z processes, which again lead to one realization of the lter estimates. The necessary Matlab code for this example is available in the Appendix A and we recommend strongly that you play with this example and get a feeling for how variable the realizations may be from case to case. Also alter Q and R and try to get an intuitive understanding for how the patterns change. Figure 6.3 shows the error standard deviations for the model in (6.24a) and (6.24b). Note the initial transient phases in the standard deviations before stationary levels are reached. For completeness, the lower panel of Figure 6.3 shows the Kalman lter gains. As one would expect, since the gains are functions of the standard deviations, these elements also display a transient phase before reaching a steady state. It was noted that the error covariance matrices, and therefore the standard deviations, do not depend on the z process. Therefore the covariance recursion (6.13) can be carried out and the lter assessed before any actual or simulated measurements are processed. This simplies the design of a Kalman lter in, for example, an engineering context. There, one typically has to select a certain measurement strategy from many possible ones to meet specied performance and cost criteria. This can be achieved through the covariance recursion trying out various combinations of measurements 1 2 2 1 2 62 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER 10 0 −10 −20 −30 −40 −50 −60 −70 0 5 10 15 20 25 1 0 −1 −2 −3 −4 −5 −6 −7 0 5 10 15 20 25 Figure 6.2: One realization of the solution of the model in (6.24a) and (6.24b) with Kalman lter estimates. The true x1 (upper panel) and x2 (lower panel) are the solid lines, while the lter estimates are dashed. The measurements are indicated by asterisks. Here Q11 = 1; Q22 = 2; Q12 = Q21 = 0, and R = 10. (i.e. dierent H ) each with potentially dierent precision (i.e. dierent R(k)). As an exercise you may want to run the Matlab code in Appendix A to try to achieve stationary standard deviations for the errors in both x and x below 1. Before leaving this example, you may wonder what the Kalman lter has achieved. How much have we gained compared to a procedure based solely on estimation without any measurement updating? It turns out that this corresponds to running the covariance recursion with K (k) = 0 (see Section 9.1). The standard deviations for the errors in this case are plotted in Figure 6.4. (Note that now P (kjk) = P (kjk 1)). Convince yourself that what we are now looking at are the errors for an estimator ^ (k) = (k)^x(k 1) = 0 since in this particular case E [x(0)] = 0.) Compared to x Figure 6.3, the errors are much larger and they do not reach stationary values but continue to grow. Clearly, the Kalman lter achieves quite a lot. 2 1 2 Note that this model is sometimes also used in other elds such as economics where it is denoted the linear growth model. Suppose, for example, that x (k) is the current value of some economic indicator or variable, say the revenue of a rm or a certain branch of rms. Eq. (6.24a) then says that the sales volume at time tk is likely to grow by an amount x (k)T until the next time point tk , a random disturbance w (k) coming in addition. The growth term itself is in turn subject to random uctuations. 1 2 +1 1 63 6.4. EXAMPLES 3.5 3 2.5 2 1.5 1 0.5 0 2 4 6 8 10 12 14 16 18 20 2 4 6 8 10 12 14 16 18 20 2 4 6 8 10 12 14 16 18 20 2 1.8 1.6 1.4 1.2 1 0.8 0.6 0.4 0.2 0 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 Figure 6.3: Standard deviations for the errors in x^ (kjk) (solid line) and x^ (kjk 1) (dashed line), for x1 (k) in the upper panel and for x2 (k) in the middle panel. In the lower panel, the Kalman lter gains K11 and K12 are displayed. Here P11(0j0) = 5, P22(0j0) = 2, Q11 = 1; Q22 = 2; Q12 = Q21 = 0, and R = 10. The formulation makes sense economically since it allows us to express mathematically that growth (or decline) in sales are expected, though not in a quite foreseeable way, as in the real world. When it comes to the observations, it would in navigation be possible to measure position x (k) and/or speed x (k). In the economic sphere, probably only the sales volume x (k) could be observed. In the latter case, the measurement equation becomes as in (6.24b). It is worthwhile to note in passing that the model in eqns. (6.24a) and (6.24b) reduces to an ordinary regression line if Q (k) = Q (k) = 0. As remarked above, we may then write x (k) = a + kT b, where a = x (0) is initial position and b = x (0) is the (constant) speed. Hence, z(k) = a + kT b + v(k), and the observations are thus 1 2 1 11 1 22 1 2 64 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER 70 60 50 40 30 20 10 0 0 5 10 15 20 25 20 25 5 4.5 4 3.5 3 2.5 2 1.5 0 Figure 6.4: K (k) = 0. 5 10 15 Standard deviations for the estimation errors in x1 and x2 obtained by setting measurements around a regression line. 6.5 The Kalman lter under non-Gaussian noise Although the algorithm of Subsection 6.2 was derived under Gaussian assumptions, it remains valid for other types of noise in a weaker sense, with the global optimality lost. Underlying this statement is the fact that the rst and second order moments of sums of uncorrelated random vectors are determined by the rst and second order moments of the individual terms with distributional assumptions beyond this being immaterial. To make the idea precise, let lz (~z (k )) = k X r=1 a(r)z (r); (6.25) b(r)x(r) (6.26) and lx(~x(k )) = k X r=1 be linear forms in the observations and the system process respectively (~x(k) in (6.26) is the vector obtained by collecting x(1); :::; x(k) in a way similar to ~z(k) in (6.3)). Suppose the coeÆcients a(r) and b(r) are matrices with the same number of rows so that the dierence lz (~z(k)) lx(~x(k)) is well dened. Then: Lemma 6.2 The mean and covariance matrix of lz (~z(k)) lx(~x(k)) are determined by the parameters f(k)g, fH (k)g, fQ(k)g and fR(k)g of (4.6) and the mean/covariance matrix 65 6.5. THE KALMAN FILTER UNDER NON-GAUSSIAN NOISE of the start vector x(0) of the x-process, all other distributional assumptions being irrelevant. This almost immediate result (which is proved at the end of the section) implies that the Kalman lter algorithm of section 6.2 still works with the same error estimates, and with the innovation process having the same properties, except for \uncorrelatedness" replacing \independence". This must be so since the statistics considered there is precisely of the linear form in Lemma 6.2. The only dierence when we go to the non-Gaussian case is that the lter is no longer globally the best one, although it is still the best procedure available of the linear type. To see this, consider, for simplicity a scalar x-process. Let E represent expectation with respect to some general noise processes whereas Eg is with respect to Gaussian noise with the same fQ(k)g and fR(k)g sequences as in E . Then, with l(~z(k)) a linear function of z(1); ::; z(k) E [(^x(k jk ) x(k ))2] =Eg [(^x(k jk ) x(k ))2] Eg [(l(~z(k)) x(k))2] =E [(l(~z(k)) x(k))2]; utilizing that x^(kjk) is optimal under Gaussian models (second line) and that E and Eg , according to lemma 6.2, have the same eect on linear statistics (rst and third line). Hence, x^(kjk) cannot be worse than any other linear method lz (~z(k)) as estimator for x(k). This argument can clearly be applied to any element of a vector process x(k). Proof of Lemma 6.2 Substitute H (r)x(r) + v(r) for z(r) in the expression for lz (~z (k )). Then lz (~z (k )) lx(~x(k )) = k X r=1 a(r)v (r) + k X r=1 b(1) (r)x(r); where b (r) are new coeÆcients, determined by the old b(r), a(r) and H (r). Now, eliminate successively x(k) (by inserting (k 1)x(k 1) + w(k 1)), then x(k 1) (using (k 2)x(k 2)+ w (k 2) as replacement) and so forth until all of x(k); x(k 1); :::; x(1) have disappeared, and we are left with (1) lz (~z (k )) lx(~x(k )) = k X r=1 a(r)v (r) + k X r=1 b(2) (r)w (r 1) + b (0)x(0); (2) where fb (r)g is another set of coeÆcients, related to the former b (r)'s and the (r)'s. The conclusion of the lemma follows from this. Take, for example, the covariance (which is more complicated than the mean). Since all the random variables on (2) (1) 66 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER the right are uncorrelated by assumption, the covariance matrix of the sum follows by summing the covariance matrices of the individual terms. Hence, VAR[lz (~z(k)) lx(~x(k))] = k X r=1 a(r)R(r)a(r)T + (b + k X r=1 (b (r))Q(r 1)(b (r))T (2) (2) VAR[x(0)](b (0))T (2)(0) (2) showing that only the covariance matrices fQ(r)g and fR(r)g matter for the covariance of lz (~z(k)) lx(~x(k)), as stated. 6.6 The Kalman lter for non-zero input variables As mentioned in the beginning of this chapter, the derivation of the Kalman lter becomes more simple notationally when there are no deterministic input terms available. The mathematics is, however, just as easy as for non-zero input vectors. In this section we will present the recursive algorithms for this case. The stochastic state model is now dened by (4.5) Note rst that since the input terms u(k) are deterministic and known, they do not inuence on the uncertainty involved. The updating equations (6.13) for calculating the covariance matrix P (kjk) for the estimation error therefore remains unchanged. Concerning the estimate itself, the modication needed in this case is to add an additional term (k 1)u(k 1) to the prediction estimate x^ (kjk 1). However, because the input term does not go into the observation model (4.5b), the ltering (or updating) part remains unchanged, giving the following recursive equations: ^ (k x jk 1) = (k 1)^x(k 1jk 1) + (k 1)u(k 1) (prediction) (6.27a) ^ (kjk) = x^ (kjk 1) + K (k)(z(k) H (k)^x(kjk 1)) (updating) (6.27b) x 6.7 Problems Exercise 6.1 (Properties of AR(1) models) We will in this exercise study a simple state space model with a state variable x(k) and an observation z(k) for k = 1; : : : . Consider the non-observable x-process rst. Assume that x(k) = ax(k 1) + w(k 1) for k = 1; : : : where a is a parameter and w(k ) is white noise. It is assumed that var[w(k )] = Q. Also assume that E [x(0)] = 0 and Var[x(0)] = Px (1). From Exercise 5.3 we know that E [x(k)] = P 0 and Px (k) = Var[x(k)] is given by Px (k ) = a Px (k 1) + Q. Further, Px (k ) = Q kj a j + a k Px (1). 2 =1 2( 1) 2 6.7. PROBLEMS 67 (a) What happens with Px(k) when k ! 1? Handle the cases jaj < 1, jaj = 1 and jaj > 1 separately. (b) Show that cov[x(k); x(k j )] = aj Px(k j ). Decide from this, for a xed j and jaj < 1, the limit limk!1 Cov[x(k); x(k j )], (c) Specify the correlation corr[x(k); x(k j )]. Also for the correlation, calculate the limit limk!1 Corr[x(k); x(k j )], for jaj < 1. (d) Assume the process fx(k)g has been running for a long time. How would you from the calculations above characterize the statistical properties of the process when jaj < 1? How does a inuence on the behavior of the process? (e) Simulate the process in Matlab for dierent values of a, f.ex. a = 2; 0:8; 0; 0:5, 0:8; 1; 2. Use Q = 1. Exercise 6.2 (Analytical calculations of Kalman lter) We will consider the same process as in Exercise 6.1. In practice, x(k) is only observed indirectly through the process fz(k)g. Assume z(k) = hx(k) + v(k) for k = 1; 2; : : : where v(k) is white noise with Var[v(k)] = R. The two noise processes fw(k)g and fv(k)g are assumed to be independent of each other. This implies that fv(k)g and fx(k)g are independent if fv(k)g is independent of x(1). We will make this assumption. (a) Calculate E [z(k)] and Var[z(k)]? What happens with Var[z(k)] when k ! 1? Discuss this for dierent values of a. A quantity of interest when analyzing series of data is the so-called signal to noise ratio. In our context it is dened as var[hx(k)] ; SR = var[v(k)] that is the ratio of the variances between the two terms which is contained in z(k). The nominator is the variance of the term \explained" by the underlying system model (the signal) while the second term is the variance of the \unexplained" or noise term. (b) What is the signal to noise ratio in the particular model under study? You may consider the case k ! 1 if you nd this relevant. Let x^(kjk 1) be the optimal estimate of x(k) based on the observations z(1); : : : ; z(k 1) and let x^(k 1jk 1) be the optimal estimate of x(k 1) based on the same observations. Assume all processes are Gaussian. 68 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER (c) Consider the following question: Which variables in the model is it natural to assume are Gaussian? (d) Show that x^(kjk 1) = ax^(k 1jk 1). (e) Find the innovation process. What are the most important statistics properties of this process? Discuss and give mathematical expressions where you nd it relevant. (f ) Apply the Kalman lter to calculate x^(kjk) based on x^(kjk 1) and the innovation. Find a recursive formula for the Kalman gain. We will assume there is no system noise in the following, that is w(k) = 0. (g) Try to solve the recursion for the Kalman gain by nding an expression for it. (h) Also nd an expression for the variance to the estimation error x^(kjk) x(k), that is P (kjk) = E [(^x(kjk) x(k)) ] 2 (i) Discuss how the expressions in (g) and (h) depend on a. (j ) Try now to vary Q and R and inspect how the stationary value for the Kalman gain changes. Do the results t with your intuition? Exercise 6.3 (Applications of the Kalman lter) We will consider further the system discussed in Exercises 6.1 and 6.2. In Exercise 6.2 (g) we ended up with the recursive formulae for the Kalman lter. (a) Try to make a block-diagram for the process and the lter (Hint: The simulink system in Matlab can be used for drawing block-diagrams.) (b) Explain which part that is the \real world" and which part that takes place in the computer. Why is it that the Kalman lter can be perceived as a process model which runs in parallel with the system? How is it \corrected" or updated in the lter? (c) Simulate the system without the Kalman lter in Matlab . Let N (the number of observations) be 100. Use a = 0:9, h = 1, Q = 1 and Px(1) = 1. Try both R = 0:1 and R = 1. Plot the x-process as a solid line and the z -process as point around the state. (d) Apply the Kalman lter to reconstruct the x process from the observations. Try for both values of R. Plot the true and estimated x-process in the same gure. You may also add the observations. 6.7. PROBLEMS 69 (e) Plot the Kalman gain and the variance of the estimation error P (kjk). Justify that P (kjk) is correct by generating many realizations of the estimation error and estimate its variance and mean at each time point for some k's. (f ) Now put Px(1) = 0 and Px(1) = 100. Again calculate the Kalman gain and corresponding P (kjk)'s. Compare and interpret what you see. (g) It looks like the Kalman gain and the variance of the estimation error approaches some stationary values for increasing n. Argue that this is reasonable. Find the stationary values analytically and compare with the numerical values. 70 CHAPTER 6. THE DISCRETE TIME KALMAN FILTER Chapter 7 Examples from statistics The purpose of this chapter is to demonstrate how linear state space modeling unify linear statistical techniques such as linear regression, analysis of variance and time series (ARMA) models. It is good training to work out how these models can be put in state space form. It is also practically useful in that the superior data processing capability of the Kalman lter becomes available. Missing observations is a case in point. Many papers in statistics have in the past been devoted to this issue for particular models, but the simplest and most general way is to let the Kalman lter take care of it. This requires the model to be written in state space form. We shall consider three main types of situations. Linear regression with one explanatory variable is treated in Section 7.1, while multiple regression is the topic of Section 7.2. The chapter is closed in Section 7.3 with a discussion of time series models. 7.1 Regression with one explanatory variable Basic formulation The traditional linear regression model relates response z(k) and explanatory variable x(k) through z (k ) = + x(k ) + v (k ); (7.1) where and are coeÆcients and v(k) independent errors with mean 0. (7.1) can be written in state space form. Formally, take (k) = and (k) = and note the trivial relationship (k ) = 1 0 (k ) 0 1 (k (k 1) + 0 1) 0 71 (7.2a) 72 CHAPTER 7. EXAMPLES FROM STATISTICS which can be used as the system equation. The measurement equation is (7.1), or when rewritten in state space form (k ) z (k ) = 1 x(k ) (k ) + v (k ): (7.2b) Note that fx(k)g in this case are known coeÆcients without errors. Regression data can then be t by running the Kalman lter. The estimates ^(N jN ) and ^(N jN ) at the end of the series coincides with the ordinary least squares estimates. This must be so since Kalman ltering is optimal in least squares sense and since = (N ) and = (N ). Although the exercise may seem pointless since there is already a satisfactory numerical procedure available, the state space based approach is much easier to generalize. Bayesian regression Recall that (7.2a) amounts to a constant sequence of (k), (k ), the values being dened by the initial values. The Kalman lter is fed informa- tion of the form E [(0)] = ; var[(0)] = E [ (0)] = ; var[ (0)] = dening what is known about the regression coeÆcients in advance . The means , and the variances , are under the control of the user. The traditional least squares solution is obtained by letting ! 1 and ! 1. For nite and we have Bayesian analysis where what is known about the regression coeÆcients in advance is combined with information from the data. 2 0 2 0 1 2 0 2 0 Autocorrelated errors Another possibility is to work with models where the error terms v(k) in (7.1) are serially correlated (colored noise), which is often of importance in economics and for industrial data. Autocorrelated errors will be discussed more generally in section 7.3, but let us here consider a simple example. Assume v(k) = av (k 1) + "(k 1), which is the autoregressive model of order 1. The regression model (7.1) with such a specication of the errors can be represented in state space form by adding a line to (7.2a), i.e. 0 1 0 10 1 0 1 (k ) 1 0 0 (k 1) 0 @ (k )A = @0 1 0A @ (k 1)A + @ 0 A v (k ) 0 0 a v(k 1) "(k 1) and replacing (7.2b) by 0 1 (k ) z (k ) = 1 x(k ) 1 @ (k )A : v (k ) 1A possible initial correlation corr[(0); (0)] is neglected in this case. 73 7.2. MULTIPLE LINEAR REGRESSION The state space approach also has a potential for allowing the regression coeÆcients to uctuate in time. This is very simply accommodated by replacing zeros on the right in the preceding system equation by random terms. The eect when estimating (k ) and (k ) from a record of observations is that less credence is assigned to old data than more recent ones. The state space approach thus yields a prescription for downgrading information. This aspect is much discussed in forecasting, and several methods proposed there are in reality special cases of Kalman ltering. 7.2 Multiple linear regression The construction in the preceding subsection extends immediately to p explanatory variables x (k); ::::; xp(k). Replace (7.1) by z (k ) = + x (k ) + : : : + pxp (k ) + v (k ); (7.3) which can be put into state space form in the manner described in the preceding subsection. To see this replace (7.2a) by 0 1 0 10 1 0 1 (k ) 1 0 0 0 (k 1) 0 B (k )C B0 1 0 0C B (k 1)C B0C B C B CB C B C + B. C : (7.4) B .. C = B .. .. .. . . CB . . . . @ . A @. . . A @.. A . .A @ . C p(k ) 0 0 0 1 p(k 1) 0 All the remarks in the preceding subsection applies to this multiple regression model as well. We shall here briey discuss another aspect. It is well known that multiple linear regression contains a number of special cases, of high interest, for example analysis of variance and analysis of covariance, which comes out when some (or all) of the explanatory variables fxj (k)g are binary. Consider an example where each unit on which the response z(k) has been observed, belongs to one of g groups. There is an additional explanatory variable, called xg (k) for reasons given shortly. A model often used in such a situation is z (k ) = i + xg (k )g + v (k ); if k belongs to group i. (7.5) There are separate parameters i characterizing the groups and an eect g xg (k) from the additional variable. By dening for each i ( 1 if unit k belongs to group i, xi (k ) = 0 otherwise, the model can be written z (k ) = x (k ) + : : : + g xg (k ) + g xg (k ) + v (k ); 1 1 1 1 1 +1 2 +1 +1 +1 1 2 leading 1 to an analysis often called +1 +1 analysis of covariance. +1 74 CHAPTER 7. EXAMPLES FROM STATISTICS clearly a special case of (7.3). This type of construction can be extended to several grouping variables so that any analysis of variance model can be handled by (7.3). Note that this, in turn, yields all the benets detailed in the preceding subsection. In particular, so{called model II cases (where the -coeÆcients are stochastic), can be handled, and this is advantageous since they may, in the traditional setting, be somewhat cumbersome computationally. 7.3 Time series models Much statistical modeling is concerned with data containing a time element. The examples to follow are special cases of a huge body of techniques known in statistics as time series models. A common view, especially in economics, is to regard the observed data, say z(k), as a superposition of three dierent eects. Thus assume z (k ) = T (k ) + S (k ) + v (k ); (7.6) where T (k) is a so{called trend, accounting for, in some sense, persistent growth (or decline), S (k) represents seasonality, i.e variations due to annual, monthly, weekly or even daily cycles, whereas v(k) stands for short-term uctuations caused by other factors. Although there is in a mathematical sense no clear-cut distinction between the three eects, they represent nevertheless a useful conceptualization for modeling. We shall now present several dierent formulations, and demonstrate how they can be put into a state space form. Growth Suppose rst that there are no seasonal eects so that (7.6) reads z (k ) = T (k ) + v (k ); (7.7) One way to represent the trend T (k) is to assume that T (k ) =T (k 1) + (k 1) + w1 (k (k ) = (k 1) + w2(k 1); 1) (7.8a) (7.8b) which coincides (in dierent notation) with the system process of the linear growth model in Section 6.4 (see the discussion after Example 1). If the short-term uctuations are modeled as an independent (white noise) process, we have the same linear growth model as in Section 4.1, and the discussion there is valid in the present sequel too. Suppose, however, that the v-process in (7.6) fails to satisfy the independence assumption. It may in practice easily happen that such serial correlations represent important eects. For example, short{term uctuations in economics are rarely independent from one point in time to the next one. One possible formulation is through 75 7.3. TIME SERIES MODELS an autoregressive process of order 1, usually denoted AR(1)). Then v(k) is given by the recursion v (k ) = av (k 1) + "(k 1): (7.9) Here the coeÆcient a satises jaj < 1 and "(k) is a white noise process as before. (7.9) is in engineering often called a Markov process. It can be proved that the process, after starting far back, reaches a stationary condition where the distribution is the same for all k (see Exercise 5.3). This is usually a reasonable modeling assumption to make. The mean is then 0, and it can under these circumstance also be established that corr[v(k); v(k + )] = a ; > 0 independent of k. The model can capture serial correlations by adjusting the coeÆcient a. Suppose we want to work with a model which is a combination of (7.7), (7.8) and (7.9). Such a model can be put into a state space form as follows. Take as the system equation 0 1 0 10 1 0 1 T (k ) 1 1 0 T (k 1) w (k 1) @ (k ) A = @0 1 0A @ (k 1) A + @w (k 1)A ; v (k ) 0 0 a v(k 1) "(k 1) and let 0 1 T ( k ) z (k ) = 1 0 1 @ (k )A v (k ) be the measurement equation. Note that v(k) is made part of the system process by this construction. Also note that there is no error term in the measurement equation. This is perfectly legal and corresponds to R(k) = 0 in the general model of Section 4.2. Note that even though R(k) is not positive denite in this case, the inverse of S (k) will exist. The variance matrix of the three error terms w (k), w (k) and "(k) could be 0 1 Q Q 0 @Q Q 0 A; 0 0 Q ignoring a possible dependence on k. Note the zeros which express a likely independence between v(k) and the error terms w (k) and w (k). 3 1 2 1 11 12 21 22 2 33 1 2 the condition that jaj < 1 is violated, the process either, if jaj = 1, wander over the whole real line, coming occasionally innitely far from the mean value 0, or, if jaj > 1 become explosive and turns to innity. 3 If 76 CHAPTER 7. EXAMPLES FROM STATISTICS Seasonality Consider seasonal eects next. As an extremely simple example, sup- pose there are only two seasons, say summer and winter or day and night. It seems reasonable to describe the eect of seasonality as opposite numbers for each season, i.e. ( S (k ) = Æ; if summer, Æ; if winter. This can also be written S (k) = S (k 1), or, if slight perturbations in this deterministic pattern is allowed S (k ) = S (k 1) + w (k 1); (7.10) 3 where w (k) is another error process. The model obtained when this seasonal eect is added to the former model (7.7{7.9), has a state space representation 3 0 1 0 10 T (k ) 1 B (k ) C B0 B C B @S (k ) A = @0 v (k ) 0 1 0 0 T (k B 1 0 0C C B (k A 0 1 0 @S (k 0 0 a v (k 0 1 T (k ) B (k )C C z (k ) = 1 0 1 1 B @S (k )A : v (k ) 1 0 1) w (k Bw (k 1) C C B 1) A + @w (k 1) "(k 1 2 3 1 1) 1)C C 1)A ; 1) In practice, there are often more than two seasons (for example four in quarterly data or twelve in monthly ones). These situations can also be expressed in state space form, although with more involved representations. Autoregression Take T (k) = ( constant), remove the seasonality and concentrate on the modeling of short{term uctuations. (7.6) now reads z (k ) = + v (k ) (7.11a) There is an enormous statistical literature on models for v(k). We have already met the AR(1){model in (7.9). An important extension is v (k ) = a1v (k 1) + a v(k 2) + : : : + apv(k p) + "(k 1) 2 (7.11b) which exhibits stationary behavior in the sense described earlier when the coeÆcients a ; : : : ; ap satisfy a mathematical condition that has been worked out and is available in standard textbooks on the subject (an alternative is to invoke Theorem 4.1). (7.11b) is known as an AR(p) model. A state space representation of (7.11a), (7.11b) can easily be constructed, for example for p = 4: 1 77 7.3. TIME SERIES MODELS 0 B B B B @ T (k ) v (k ) v (k 1) v (k 2) v (k 3) 1 0 1 C B 0 C B C=B 0 C B A @ 0 0 0 0 0 0 a1 a2 a3 a4 1 0 0 0 0 1 0 0 0 0 1 0 0 10 CB CB CB CB [email protected] T (k v (k v (k v (k v (k 1 T (k ) Bv (k ) C C B C: z (k ) = 1 1 0 0 0 B v ( k 1) B C @v (k 2)A v (k 3) 1) 1) 2) 3) 4) 1 0 0 C B "(k 1) C B C+B 0 C B A @ 0 0 1 C C C C A The system process has ve components, three of them being earlier numbers of the v {process. That is perfectly legal and so is the many zeros in the ve{dimensional error process (a zero means that the corresponding variance is 0). This exercise is useful in practice, since the Kalman lter is by far the most convenient way of lling out gaps in the series. Technically this is accomplished along the line discussed in Section 4.3. Moving average State space representations are probably even more useful for the other, main linear model for stationary time series, the so{called moving average (MA) model. Replace (7.11b) with v (k ) = "(k ) + b "(k 1) + : : : + bq "(k q ); (7.12) where "(k), as usual, is a white noise process. (7.12) is, for short, designated MA(q). The following state space representation is available for (7.12), say when q = 3: 0 1 0 10 1 0 1 T (k ) 1 0 0 0 0 T (k) 0 B"(k ) C B0 0 0 0 0C B"(k 1)C B"(k )C B C B CB C B C B"(k 1)C = B0 1 0 0 0C B"(k 2)C + B0 C B C B CB C B C @"(k 2)A @0 0 1 0 0A @"(k 3)A @0 A "(k 3) 0 0 0 1 0 "(k 4) 0 0 1 T (k ) B"(k ) C C B C: z (k ) = 1 1 b b b B " ( k 1) B C @"(k 2)A "(k 3) The point of the exercise is again that the Kalman lter is oered to perform computations. MA{models are harder to handle technically by other methods than AR-models were. Combination models containing both AR and MA terms (so{called autoregressive moving average (ARMA) models) are often useful, and extensively discussed in statistical literature. State space formulations are again useful, although omitted here. 1 1 2 3 78 CHAPTER 7. EXAMPLES FROM STATISTICS 7.4 Problems Exercise 7.1 (Linear regression with autocorrelated noise) Considerp the linear regression model with autocorrelated errors in Section 7.1 with x(k ) = k . Our aim is to estimate and . (a) Write a general routine for the Kalman lter for this model. (b) Why is there no use in performing smoothing on such a model? (c) Simulate a set of realizations of length N = 20 from the model with = 0:0; = 1:0; Q = var["(k)] = 1 and a = 1:0; 0:75; 0:5; 0:0; 0:25; 0:5; 0:75; 1:0. For each set of realizations, estimate and . (d) Make a plot of j^ j against a. Do you see any pattern? (e) Now make a plot against a of the standard deviations for the errors involved in estmating . Comment on the result. In particular, compare the behavior for a < 0 with a = 0. Try to understand this. (f ) Repeat (d) and (e) but now consider ^. Exercise 7.2 (AR modeling) Fitting autoregressive models to data serier is a large eld and Matlab is well equipped here, such as the command ar (try help ar and help theta to get some information on how to use this routine). We will consider the dierence equation z (k ) + cz (k 1) + dz(k 2) = w(k 1); k = 2; 3; : : : where c = 0:8, d = 0:15 and w(k 1) for a given k is a Gaussian distributed random variable N (0; 0:2). Let z(0) and z(1) be Gaussian N (5; 1). (a) Generate some realizations of z(k) for k = 0; : : : ; 100 and plot the results. (b) Write the model in state space form. (c) You should have ended up with a model with no measurement noise. In such cases we are not guaranteed that S(k) can be inverted (see Section 6.2). Make sure that there will be no problems in this case. (d) Show that the system is asymptotically stable. How do you think this property inuence on the behavior of the sequence fz(k)g. (e) Show that the system is observable and stochastic reachable. Chapter 8 Additional aspects of the Kalman lter In this chapter we will discuss various aspects concerning the Kalman lter. In Section 8.1, we will give a brief discussion of the important topic of stability of the Kalman lter algorithm. For a time { invariant system with constant noise covariance matrices (Q; R), stability means that the error covariance matrix P (kjk) approaches a unique matrix P ,independent of the initial condition, as k increases (note that this is a dierent matrix than the one discussed in Section 4.2). This is important in its own right and is also necessary for numerical errors not to accumulate during the recursions. Following on from this is a brief treatment of dierent but algebraically equivalent covariance recursions (Section 8.2). Several versions exist and some of them give additional insight into how the Kalman lter works. We also mention recursions that eectively compute the square root of the covariance matrix, thus limiting the numerical range of the matrix elements involved. Such recursions generally have better numerical properties and may be important in real time applications. So far, we have assumed that our model (comprising the state space equations and statistical assumptions) are suÆciently accurate to neglect model errors. However, in any real application the model forming the basis for the Kalman lter will dier from the \reality" giving rise to the observations. This may lead to the problem of lter divergence, where the diagonal terms of the lter error covariance matrix give a completely wrong picture of the performance of the lter. Typically, the diagonal terms would indicate that the lter works properly, while the estimates in reality have much larger errors. An assessment of the real lter performance is central in any application and some useful techniques will be discussed in the nal Section 8.3. 79 80 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER 8.1 Stability of the Kalman lter algorithm The stability results are cumbersome to prove and we will only show an example and state some main facts. For more throughout discussion, we refer to Harvey (1989). Example 1 (A moving body, cont.) Continuing with our two{dimensional moving body example, assume now that only the component x is measured (in contrast to our discussion in Section 6.4 where x was measured). Figure 8.1 shows the estimation error standard deviations in this case. Now only the standard deviation for the error in x reaches a stationary value, while the error in x diverges. (Verify this by trying more iterations yourself.) Although the lter still does a lot better than pure prediction from initial conditions alone (cf. Figure 6.4), the error divergence is undesirable; in a sense the ltering problem is ill{posed. 2 2 1 2 1 The reason for the problem in this example turns out to be that the system is not observable (cf. Section 2.2). It is interesting that the deterministic property of observability plays a role in stochastic ltering but, with hindsight, this may not be too surprising. It can be shown that if a deterministic system is unobservable, a component of the state vector within a certain subspace has no eect on the observations (in our example above, the state x could have been removed from the system equation without changing the distribution of the observations). Running the Kalman lter in the stochastic case, it seems reasonable that the uncertainty associated with the unobservable state component will depend on the initial uncertainty and possibly grow with time. We will see shortly that the deterministic reachability concept also comes into play. Consider the general state space model in eqns. (4.6a) and (4.6b). We will only state results for time{invariant systems; i.e. ; H ; R, and Q are all constant matrices independent of k. Assume that the system is observable in the sense of Denition 2.2; i.e. the matrix 1 0 H BH B W o = B .. @. H n 1 C C C A 1 has rank n. Further, if we formally apply the reachability criterion with w(k 1) as the n input, we have W c = I n which clearly has rank n. The system is therefore what is denoted stochastically reachable. Intuitively, this means that the noise aects the whole state space. (Note that there exist other formulations of the stochastic state space model that are not necessarily stochastically reachable.) We then have the following result: 1 81 8.1. STABILITY OF THE KALMAN FILTER ALGORITHM Error standard deviations for x1 16 14 12 10 8 6 4 2 0 2 4 6 8 10 12 14 16 18 20 14 16 18 20 Error standard deviations for x2 3 2.5 2 1.5 1 0.5 0 0 2 4 6 8 10 Time, k 12 Standard deviations for the errors in x^ (kjk) (solid) and x^ (kjk Here only x2 is measured. The parameters are as in Figures 6.2 and 6.3. Figure 8.1: 1) (dashed). Theorem 8.1 Assume the system in eqns. (4.6) is time{invariant , observable, and stochastically reachable. Furthermore, R and Q are assumed positive denite. Then: (a) For any symmetric and positive denite matrix P (0j0), P (k jk ) converges uniformly to a unique matrix P . This also implies that K (k ) converges to a constant matrix K . (b) Dene the steady state Kalman lter as the one using the gain matrix K . ^ (kjk) = x^ (kjk 1) + K (z(k) H x^ (kjk 1)) = ( KH )^x(k We have x 1jk 1) + Kz(k). This lter is asymptotically stable, i.e. all eigenvalues of ( KH ) lie within the unit circle. These results are important in practice since they provide necessary conditions for the error covariance matrices to converge independently of the initial condition P (0j0). They also guarantee numerical stability of the lter equations. For further details, see Minkler & Minkler (1992). 82 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER 8.2 Equivalent forms of the variance recursion In Chapter 6, the covariance equations (6.13) were derived. There are several alternative but algebraically equivalent sets of recursive equations. These may sometimes be useful in terms of improved numerical properties or they may be used to enhance the interpretation of the Kalman lter. 8.2.1 The information lter This section includes some matrix results which are useful to be aware of. To show these results requires a level of prociency in matrix manipulations that a serious student of Kalman ltering should master. Central is the so-called Matrix Inversion Lemma which states the following: Lemma 8.1 (Matrix Inversion Lemma) Let A (n n) be a nonsingular matrix and let B and C both be n m matrices. Suppose both (A + BC T ) and (I m + C T A 1 B ) are nonsingular. Then (a) (A + BC T ) B = A (b) (A + BC T ) = A 1 1 1 B (I m + C T A 1 B) 1. A 1 B (I m + C T A 1 1 B) 1C T A 1. The proof of the lemma is given in Exercise 8.1. Basically, it allows us to derive the following relationships from the standard variance recursion (see Exercise 8.2 on how to prove these): jk 1) = [I n B (k 1)]A(k 1) P (k jk ) = P (k jk 1) + H (k )T R(k ) A(k ) = (k ) P (k jk ) (k ) P (k 1 1 1 1 1 B (k ) = A(k )[A(k ) + Q(k ) 1 1 ] 1 1 H (k ) (8.1a) (8.1b) (8.1c) (8.1d) Eqns. (8.1) are relationships between inverse covariance matrices. It allows us to run a recursive algorithm for calculating the inverse of P (kjk 1). An inverse of a covariance matrix is usually named the information matrix. Small noise variances leads to more specic knowledge about the variables, that is more information. The recursions (8.1) are therefore called the information lter. The equations above are suÆcient for calculating (the inverse of) P (kjk). In order to calculate the estimates, the Kalman gain is needed. The relationship K (k ) = P (k is then available. jk)H (k)T R(k) 1 (8.2) 83 8.2. EQUIVALENT FORMS OF THE VARIANCE RECURSION A few remarks relating these recursions to the ordinary ones given in (6.13) are in place. In Section 4.3 we discussed the use of diuse priors where the diagonal elements of P (0j0) were put to innity. When using the ordinary lter, only an approximative solution is possible by substituting innity with a suÆciently large number. For the information lter, an exact solution is directly available by putting P (0j0) equal to zero. The information lter requires the inversion of A(k)+ Q(k) (an n n matrix), while the ordinary lter require the inversion of S (k) (an m m matrix). A comparison of the computational burden involved for the two lters will therefore depend on n and m. Because of the need for inversion (and thereby existence) of (k) and Q(k) , the information lter is usually less attractive than the ordinary lter. An important interpretation of the estimation procedure can however be found from (8.1b). Clearly, it shows that P (kjk 1) P (kjk) . It can be shown that this is equivalent to P (kjk) P (kjk 1) which tells us that trace(P (kjk)) trace(P (kjk 1)). In other words, the sum of the estimation error variances never increases as a result of a measurement update. This reects the fact that the uncertainty of an estimate decreases as the amount of data increases. Intuitively, one sees from (8.1b) that the more accurate the observations are (implying small elements in R(k) and large elements in R(k) ), the larger the trace of P (kjk) , and the smaller the trace of P (kjk). Eq. (8.2) is not very useful from a practical algorithmic point of view. But it tells us that if P (kjk) has somehow been obtained, the gain matrix can be viewed as a \ratio" between the estimation error variance and the measurement error variance. If, for example, P (kjk) increases due to increased system noise, then K (k) will also increase and the measurements are weighted more heavily in the lter measurement update step (for xed R(k)) so that the residuals become relatively more important. 1 1 1 1 1 1 1 1 1 8.2.2 More versions of the variance recursions Today, practical Kalman lters are often implemented using covariance recursions based on factorizations of the error covariance matrices. This oers advantages in terms of increased numerical precision without increasing the computation time signicantly. Actually, this is a rather well developed eld with many available sets of equations. One possibility is to compute a so{called square root M (k) of the error covariance matrix where P (kjk) = M (k)M (k)T . (Note that square roots of symmetric matrices are not unique.) This eectively doubles the numerical precision because 1 Given two matrices A and B of the same dimension, A semidenite. This in turn implies that trace(A) trace(B). B means that A B is positive 84 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER Reality or simulated reality System Computer algorithm z(k) Kalman filter x(k|k) Figure 8.2: The relationship between the Kalman lter and the real system or simulated versions of the real system. if the elements in P (kjk) range from 2N to 2 N , those of M (k) will range from 2N= to 2 N= . For more on this see Minkler & Minkler (1992); Grewal & Andrews (1993). 2 2 8.3 Assessing modeling errors Our experience is that this topic seems to be conceptually somewhat diÆcult to grasp. We will therefore take some eort to explain the main ideas. Figure 8.2 shows the relationship between the Kalman lter and the real system whose states the lter estimates. During a design phase, one typically does not process data from the real system but tests the lter on simulated versions of the real system. We imagine now that the system is described by the matrices ; H ; R; Q; (0) and P (0j0). Under optimal conditions, the lter is based on these same matrices. In this section, the problem is to assess a lter based on ; H ; R; Q, and P (0j0) processing measurements from the system. You may, of course, ask why anyone would want such a lter. In situations where the correct matrices are known, one then deliberately chooses a suboptimal lter rather than the optimal one. This could be the case, for example, if the optimal lter is computationally too demanding in a real time application. However, the most common situation is that the correct matrices are only more or less well known. To design a lter, one has to select a set ; H ; R; Q, and P (0j0). How then can the performance of this lter be assessed? Well, somebody once said that no amount of simulation trickery can replace missing information, so to really answer this question you simply have to try the lter in reality. But as ardent computer fans, we will still claim that simulations may be useful also in this case because you can pose so{called \what if" questions and get the answers by simulations. For instance, you may think that the diagonal elements of Q(k) and R(k) lie between certain boundaries. If this is the case and you base your lter on some Q(k) and R(k), the computer will give you the boundaries for the error variances. Doing this, you will hopefully end up entering the real world with 8.3. ASSESSING MODELING ERRORS 85 a lter that is less likely to fail . In the following, it is convenient to separate between to types of modeling errors. 2 The elements in error covariance matrices P (0j0); Q(k), and R(k) may be misspecied but the matrices (k) and H (k) describing the system are correct. All matrices may be misspecied and even the dimension of the lter may be dierent from that of the real system. 8.3.1 Updating the covariance matrix for an arbitrary gain matrix In some cases one could prefer to use a Kalman gain K (k) dierent from the optimal choice K (k). This may be because K (k) is easier to compute or in order to make the procedure more robust. Filters using such gain functions are called suboptimal Kalman lters. Consider the lter equations in eqns.(6.12) and assume that we apply some gain matrix K (k ) of our own liking instead of the optimal K (k ). Now using index to indicate that the estimates are based on K (k), the lter then becomes ^ (kjk 1) = (k 1)^x(k 1jk 1) x (prediction) (8.3a) ^ (kjk) = x^ (kjk 1) + K (k)(z(k) H (k)^x (kjk 1)) (updating): (8.3b) x As long as K (k) 6= K (k), this lter is suboptimal in the sense of Theorem 5.1. But K (k) may still be preferable because we have made it very simple to compute and are therefore willing to accept some destruction in performance compared to the optimal lter. For example, we could set K (k) constant and equal to the stationary gain matrix K . Denoting the real error covariance matrices involved using estimator ^ (kjk 1) by P (kjk 1), we now show that the error covariance matrices of the x lter in (8.3) are governed by the recursion P (k jk 1) =(k 1)P (k 1jk 1)(k 1)T + Q(k 1) (8.4a) T P (k jk ) =(I n K (k )H (k ))P (k jk 1)(I n K (k )H (k )) + K (k)R(k)K (k)T : (8.4b) Eq. (8.4a) follows immediately since the prediction step is the same as for the optimal lter. To see eq. (8.4b), note that ^ (kjk) = (I n K (k)H (k))(x(k) x^ (kjk 1)) K (k)v(k): x(k ) x 2 Entering a more philosophical mode, one may take the position that no model is ever really correct. For instance, truly linear systems may never exist. Our model is always an approximation. This is true, so what we have been discussing is the situation where modeling errors are negligible compared to the case where they are signicant . as judged by the modeler 86 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER Taking the covariance matrix on both sides, we obtain the desired relationship. In the next section we will utilize the recursion eqns. (8.4a) and (8.4b) to assess the performance of a suboptimal lter resulting from misspecication of the matrices P (0j0); Q(k ), and R(k ). Now consider the case where K (k) = K (k). Then we see, somewhat surprisingly, that (6.13d) can also be written as P (k jk ) =(I n K (k )H (k ))P (k jk 1)(I n K (k )H (k ))T + K (k)R(k)K (k)T : (8.5) Numerically, an advantage of this equation is that it will ensure a symmetric P (kjk). As an exercise you may want to start with eq. (8.4b) and derive eq. (6.13a). This will involve some matrix juggling and is good practice. 8.3.2 Misspecied covariance matrices The suboptimal lter estimates are computed according to ^ (kjk 1) = (k 1)^x(k 1jk 1); x (8.6a) ^ (kjk) = x^ (kjk 1) + K (k)(z(k) H (k)^x (kjk 1)): x (8.6b) Using the Kalman lter as a black box, we will as output also obtain some covariance matrices (denoted now P (kjk)) obtained through the ordinary recursions P (k jk 1) = (k 1)P (k 1jk 1)(k 1)T + Q(k 1) (8.7a) S (k ) = H (k )P (k jk 1)H (k )T + R (k ) (8.7b) T K (k ) = P (k jk 1)H (k ) S (k ) (8.7c) P (k jk ) = (I n K (k )H (k ))P (k jk 1) (8.7d) with initial condition P (0j0), but with K (k) replaced by K (k). This is the apparent error covariance matrices computed by the suboptimal Kalman lter. Referring to Section 8.3.1, it is clear that the real error covariance recursion for this lter is given by the formulae in that section: P (k jk 1) =(k 1)P (k 1jk 1)(k 1)T + Q(k 1) (8.8a) P (k jk ) =(I n K (k )H (k ))P (k jk 1)(I K (k )H (k ))T + K (k)R(k)K (k)T (8.8b) 1 Clearly, P (kjk); P (kjk), and P (kjk) are all dierent, where P (kjk) now denotes the error covariance matrix for the optimal lter, i.e. the one based on P (0j0); Q(k), and R(k). All we can say is that Pii(kjk) Pii (kjk). 0 0 0 87 8.3. ASSESSING MODELING ERRORS 4 0 P − optimal * P − computed by suboptimal KF P − real 3.5 3 2.5 2 1.5 1 0.5 0 0 2 4 6 8 10 12 14 16 18 20 Assessing suboptimal lter performance in estimating x1 (k). The real error standard deviation is shown together with the standard deviation computed by the suboptimal lter itself. The standard deviation for the optimal lter is as in Figure 6.3 and is given as a reference. Figure 8.3: Example 1 (A moving body, cont.) Returning to our two{dimensional example, Figure 8.3 shows three computed error standard deviations. Here the initial conditions are correct (P (0j0) = P (0j0)), while R = 1 and Q = 0:2I compared to Q = I and R = 10. Thus the lter becomes suboptimal because it is based on too small measurement and system noise variances. As a result, it is overly optimistic regarding its own performance with a stationary standard deviation somewhat below 1. In reality, the true standard deviation is around 2:5 and that is almost as low at it can be comparing with the optimal lter. Again you may want to play with this example using the Matlab code given in the Appendix. 2 8.3.3 General modeling errors There exists recursions to compute the real error covariance matrix when all matrices, including (k) and H (k) are misspecied. However, these are rather cumbersome and do not provide much insight. Brute force Monte Carlo simulations are often the best approach. To illustrate this, assume that the lter designer now includes air resistance into the model of the body in Figure 1.1 on which the Kalman lter is based. However, in reality this is negligible (as has been assumed up till now). The lter is based on the model x_ (t) = 0 1 x (t) + 0 u(t) x_ (t) 0 f x (t) 1=m 1 1 2 2 88 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER 8 0 P − optimal P* − computed by suboptimal KF P − real 7 6 5 4 3 2 1 0 0 2 4 6 8 10 12 14 16 18 20 Figure 8.4: Assessing suboptimal lter performance in estimating x1 (k) for a lter based on an erroneous matrix. The real error standard deviation is shown together with the standard deviation computed by the suboptimal lter itself. The standard deviation for the optimal lter is as in Figure 6.3 and is given as a reference. where the assumed air friction coeÆcient f = 5 provides \drag". For simplicity, we assume that u(t) = 0 for all t, but a noise term w(k 1) with covariance matrix Q is added to the deterministic version of the model. The lter is now assessed by producing a large number of realizations of the real state vector (without friction) and the associated measurements. The suboptimal lter processes these observation. For each simulation run, the state and the estimate from the suboptimal lter are compared and the error computed. Averaging over a large number of simulations allows us to estimate the mean and standard deviation of the estimation errors. The result is shown if Figure 8.4 which is based on averages over 5000 Monte Carlo simulations. Note that R = 1 and Q = 0:2I as above. Again the lter is overly optimistic compared to the optimal results but this time a serious situation occurs since the real standard deviation diverges leading to estimates that are eectively useless. You may wonder if the estimates are still unbiased (they may not). Play with the Matlab code and investigate this. 8.4 Problems Exercise 8.1 (Proof of Lemma 8.1) T (a) Dene D = A + BC . Show that A 1 =D +D 1 1 BC T A 1 89 8.4. PROBLEMS (Hint: Write I n = D D and insert for D. Then multiply by A .) (b) Based on the result in (a), show that D B = A B (I n + C T A B ) and use this to show part (a) of Lemma 8.1. (c) Show part (b) of Lemma 8.1. (Hint: Multiply the expression in (b) by C T A from the right and rearrange terms.) 1 1 1 1 1 1 1 Exercise 8.2 (Proof of information lter) We will in this exercise show equations (8.1b) and (8.2). The remaining equations in the information lter may be proved similarly (Anderson & Moore 1979). (a) Use (6.13) to show that P (k jk ) =P (k jk 1) P (k jk 1)H (k )T fH (k)P (kjk 1)H (k)T + R(k)g 1 H (k )P (k (b) Rearrange the expression in (a) further to show that P (k jk ) =P (k jk 1) P (k jk 1)H (k )T R(k ) fH (k)P (kjk 1)H (k)T R(k) + I g jk 1) 1 1 1 H (k )P (k jk 1) (c) Based on the result in (b), show (8.1b). (Hint: Apply Lemma 8.1 (b) using A = P (kjk 1) , B = H (k)T R(k) , and C = H (k )T .) (d) By using the same trick as in (a), (b), show that K (k ) =P (k jk 1)H (k )T R(k ) (H (k )P (k jk 1)H (k )T R(k ) + I ) 1 1 1 1 1 (e) Use now part (a) of Lemma 8.1 with A; B and C as in (c) to show that K (k ) =(P (k jk 1) + H (k )T R(k ) H (k )) H (k )T R(k ) : 1 1 1 1 (f ) Use (e) and (8.1b) to show (8.2). Exercise 8.3 (Better estimates with more observations) (a) From the equations in the information lter, we found that P (kjk 1) is always positive semidenite. Use this to show that var[aT x^ (kjk)] var[aT x^ (kjk 1)] where a is a vector of length n. Interpret the result. P (k jk) 90 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER (b) Use now Theorem 5.1 to show the result in (a) directly. Based on this, show then that P (kjk 1) P (kjk) must be positive semidenite. Exercise 8.4 (Divergence phenomenon) This exercise illustrates the divergence phenomenon, which occurs when either system noise or measurement noise or both are small. In such cases the Kalman lter may give completely wrong results. Consider the simple state space model x(k ) = x(k 1) + b; z (k ) = x(k ) + v (k ); where b is a very small bias, so small that, when we design our Kalman lter, we choose to neglect it. The noise term v(k) is assumed to be Gaussian with expectation zero and variance equal to R. Our Kalman lter is based on the following (approximative) model (equal to the iid variables Example 4 in Section 6.4) x(k ) = x(k 1); z (k ) = x(k ) + v (k ): (a) Show that for the approximative model, the Kalman gain is given by P (0j0) : K (k ) = kP (0j0) + R What happens with K k) as k ! 1? (b) What happens with the estimate x^(kjk) as k increases? (Hint: Consider the dierence x^(kjk) x^(k 1jk 1).) (c) Compute P (kjk), the covariance matrix for the error terms in the approximative model. Which impression does the approximative model give on the errors involved as k ! 1? (d) The Kalman lter only computes the error variance in the approximate model. The true error, e~(k jk ) = x^(k jk ) x(k ) may however have an error of a quite dierent magnitude. Show that e~(k jk ) = R P (0j0) e~(0j0) kP (0j0) + R kP (0j0) + R k=2)P (0j0) + kR + ((k 1) b: kP (0j0) + R What happens with e~(kjk) as k ! 1? k X i=1 v (i) 8.4. PROBLEMS 91 (e) Simulate the estimation error for your own choice of parameters. (f ) Summarize the results from this exercise to some general conclusions. Exercise 8.5 (Suboptimal ltering) Consider the same model as in Exercise 8.4. Put R = 1, P (0j0) = 1 and b = 0:02. (a) Assume we know the constant external force b and we want to make an optimal lter based on this. Compute and plot the error variance for this lter. (Hint: Use the lter given in Section 6.6.) (b) Assume still you know b, but we believe that R = 4 and we therefore are considering a suboptimal lter. Compute and plot the error variance in the suboptimal lter both for the apparent error (the one we get if we believe in the wrong error) and the real error. (c) Assume now that we only know about the appearance of b, but we do not know about its value. We still believe that R = 4. Construct a suboptimal lter where the error does not diverge by introducing ctive system noise. Compute and plot the apparent error variance for this lter. (d) Generate a large number of realizations of the real estimation error and nd its empirical distribution for some k's. Compare with the results from (a)-(c). Exercise 8.6 (Misspecied covariance matrice) Consider the example in Section 8.3.2. Compute P and P for R = r and Q = 0:2rI for dierent values of r. Discuss the results. 92 CHAPTER 8. ADDITIONAL ASPECTS OF THE KALMAN FILTER Chapter 9 Prediction and smoothing So far we have been concentrating on \on-line" estimation, that is estimating x(k) given z(1); :::; z(k). Estimation of x(k) given the observations z(1); :::; z(l) where l 6= k is however of interest in many cases. We have only seen one such case, where l = k 1, resulting in one step a head prediction, for which solutions are given directly from the Kalman lter. Prediction further into the future will be discussed in Section 9.1. In many cases, interest is on estimating x(k) given data succeeding time point k (or tk ). Such types of estimation is usually called smoothing. For example, if there is no need in doing the estimation \on-line", the data analysis can be performed after, say, z(1); :::; z(N ) is observed. Estimation of x(k) based on all data will then give more reliable results than only basing the estimation on the data up to time t. In fact, Theorem 5.1 in Section 5.1 shows that the optimal estimate for x(k) given that z (1); :::; z(N ) is available is ^ (kjN ) = E [x(k)jz(1); :::; z(N )]: x Of course, \on-line" calculation of these expectations is not possible, since it is necessary to have observed the later observations. However, the actual calculations of the estimators can be performed in a very similar manner to the Kalman lter. Note rst that x^ (N jN ) is given directly from the Kalman lter at time N . But given x^ (N jN ) we can nd an updating equation for x^ (N 1jN ). This can be repeated for nding ^ (N 2jN ) and so on, making the calculation of x^ (kjN ) possible by a backwards x recursion from N to 1. Finding x^ (kjN ) for k = 1; :::; N is called the xed-interval smoother. In other cases, we are perhaps more interested in x(k) at a specic time, and we want to update our estimate for each time a new observation is collected. That is, we want to calculate x^ (kjj ) for j = k; k +1; k +2; :::. Also these estimates are available through recursive calculations, but in this case the calculations is possible to perform on-line. Such an approach is called xed-point smoothers. Note that in principle, we could 93 94 CHAPTER 9. PREDICTION AND SMOOTHING perform such estimation by using the xed-interval smoother for N = j . The amount of calculation involved will however be huge, making this approach less attractive. The last kind of smoothers we will be considering is the xed-lag smoothers. In this case, we are interested in estimating x(k) given the data up to k + L, where L is a xed number. Such estimates could be important if for instance the real interest is in x^ (kjN ), but where it is reasonable to assume that observations more than L timesteps ahead have negligible inuence on x(k) given the data up to time k + L. Such estimates can then be calculated at time k + L instead of waiting for all observations to be collected. We will start in section 9.2 with a description on how to perform xed-point smoothing since this is the easiest part. Section 9.3 consider xed-interval smoothing while Section 9.4 deals with xed-lag smoothing. Section 9.5 closes the chapter with some examples. 9.1 Predicting future In many cases we are interested in predicting the future x(k + j ) based on the observations z(1); :::; z(k). From the Kalman lter algorithm, prediction of the system vector one step ahead is directly obtainable. Predicting the state further time steps apart is just as easy to obtain. There are many alternative ways of doing this, but perhaps the easiest approach is the one based on utilizing the already derived Kalman lter and the exibility in our state space modeling. When prediction of x(k + j ) is to be based on z(1); :::; z(k), we may consider the observations z(k + 1); :::; z(k + j ) as missing. In Section 4.3 we saw that missing data can be incorporated into the model by dening H (l) = 0 for l = k + 1; :::; k + j . Running the Kalman lter up to time point k + j by this modied model, the predictions ^ (k + j jk) = E [x(k + j )j~z(k)] x and the covariance matrix for the corresponding prediction errors ^ (k + j jk) x(k + j ) e(k + j jk ) = x are directly obtained. Example 1 (A moving body, cont.) Consider again the moving body example, and assume we want to predict the position for k + j = 22; 23; ::: based on the rst k = 21 (simulated) observations shown in Figure 6.2. The results are shown in Figure 9.1. The upper panel shows the position, while the lower panel shows the velocity. In both plots, the solid line shows the estimate xi(ljl) for l = 1; :::; 21 and xi(lj21) for l = 22; :::; 51. The dashed lines shows the standard deviations for the error added and subtracted from the estimates. Note (for l > 21) that the errors increase as predictions are made further in time. 2 95 9.2. FIXED-POINT SMOOTHING 50 0 −50 −100 −150 −200 −250 −300 −350 0 10 20 30 40 50 60 50 60 4 2 0 −2 −4 −6 −8 −10 −12 0 Figure 9.1: 10 20 30 40 Prediction estimates (solid line) for xi (k + j jk) with k = 21 and j = 1; :::; 30. The dashed lines show x^i (k + j jk) P (k + j jk). For l < k, the ordinary Kalman lter estimates x^ (ljl)) P (ljl) are shown. Upper panel shows the position while the lower panel shows the velocity of the moving body example. The computations are based on the (simulated) observations shown in Figure 6.2. p p In time-series analysis, prediction of future observations is of particular interest. Now ^ (k + j z jk) =E [z(k + j )j~z(k)] =E [H (k + j )x(k + j ) + v(k + j )j~z(k)] =H (k + j )^x(k + j jk) (9.1) and similar arguments result in var[^z(k + j jk) z (k + j )] = H (k + j )P (k + j jk)H (k + j )T + R(k + j ): (9.2) These formulae can be easily implemented into our lter, making prediction of observations equally straightforward. 9.2 Fixed-point smoothing Suppose we wish to estimate x(t) at some xed point t based on data ~z(k). For k < t, we are in an ordinary prediction situation, and the techniques in Section 9.1 applies. Our concern in this section will be situations where k > t. In particular, we would 96 CHAPTER 9. PREDICTION AND SMOOTHING like to update our estimate of x(t) as new observations z(k); k = t +1; t +2; ::: arrives. In the literature, special recursive equations are derived for performing this updating. An easier, although perhaps more heavy computational, approach is to utilize the exibility of the already derived Kalman lter by modifying our state space model. In particular, we will, for k > t, add x(t) to the state vector. Denote x(t) x (k ) = : (9.3) x(k ) The system equation in this case becomes x(k ) = In 0 0 (k 1) x (k 1) + 0 w(k 1) ; (9.4a) while the observation equation becomes z (k ) = 0 H (k ) x (k ) + v (k ): (9.4b) Denote the estimation error covariance matrices resulting by running the Kalman lter on this modied state space model by P (kjk). Then x^ (tjk) will be directly available as the rst n components of x^ (tjk), while P (tjk) is the upper left n n submatrix of P (kjk). 9.3 Fixed-interval smoothing The xed-interval smoother is particularly important when estimation of parameters is of concern, see Chapter 10. In this case we assume z(k) are observed for all k = 1; :::; N , and we want to calculate the estimates x^ (k jN ) for all k . While in principle, modications of the state space model could be constructed similar to the xed-point smoother, the dimension of the state vector would have to be increased by n at each time-step, making the calculations impractical. A more eÆcient alternative in this case is to derive some special purpose recursive equations. Note that the estimate for k = N , x^ (N jN ) is directly available from the Kalman lter. We shall see that based on this, it is possible to derive an expression for x^ (N 1jN ), which again can be utilized for calculating x^ (N 2jN ) and so on. The calculation of the xed-interval smoothers is therefore based on the Kalman lter, which is a forward recursive scheme and a new backward recursive scheme. The following Theorem describes the backwards recursive equations, which we state without proof: Theorem 9.1 The xed interval smoother is obtained by the following (backwards) recursive equations: jN ) = x^ (kjk) + A(k)[^x(k + 1jN ) x^ (k + 1jk)] P (k jN ) = P (k jk ) + A(k )[P (k + 1jN ) P (k + 1jk )]AT (k ) ^ (k x (9.5a) (9.5b) 97 9.4. FIXED-LAG SMOOTHING where A(k ) is given by A(k ) = P (k jk)T (k 1)P (k + 1jk): (9.5c) 1 We see that all the quantities involved either are known from the Kalman lter or is given by calculations at time-point k + 1, showing that we indeed have a backwards recursion algorithm for calculating the smoothers. Note that it is necessary to store ^ (kjk) and P (kjk), but that the observations are not needed! Further, although both x the prediction x^ (k + 1jk) and its error covariance matrix P (k + 1jk) are needed in the equations above, they do not necessarly have to be stored, because they are easily calculated by (6.12a) and (6.13a). 9.4 Fixed-lag smoothing For the xed-lag smoother, we would like to estimate x(k) based on ~z(k + L) for each k . The xed-lag smoother is usually being used as an approximation to the xedinterval smoother, the reason being that while the latter need all data to be collected before the estimates can be calculated, the former can be performed on-line (with a time-lag corresponding to the size of L). The way to calculate these xed-lag smoothers is similar to the approach for xedpoint smoothers. Also in this case, we will change our state space model by augmenting the state vector. Dene in this case 0 x(k ) = B B B B B @ x(k ) x1 (k ) x2 (k ) ... xL (k ) 1 0 C C C C= C A B x(k B B x(k B B ... @ x(k 1 x(k ) 1) 2) L) C C C C C A (9.6) where xj (k) = x(k j ) for j = 1; :::; L. Then, using the state model (4.6), we get 0 1 0 1 (k 1) 0 0 0 In B C B C In 0 0 0 C B B 0 C B C B 0 I n 0 0 C x (k 1) + B 0 C x(k ) = B C w (k ) B B . C . . . .C @ .. 0 .. . . . .. .. 0 In 0 A @ .. 0 A (9.7a) with the observation equation z (k ) = H 0 0 ::: 0 x(k) + v(k): (9.7b) 98 CHAPTER 9. PREDICTION AND SMOOTHING Applying the Kalman lter on this augmented model (with size (L + 1)n on the state vector), we obtain not only the estimates x^ (kjk) but also x^ i(kjk) = x^ (k ijk); i = 1; 2; :::; L. For i = L, the quantity of interest is obtained. Note that we in this case need to specify the distribution for the initial state vector ^ (kjk + L) x (0) = (x(0)T ; :::; x( L)T )T in the augmented model. Note further that x is the last element of x^ (k + Ljk + L), and similarly for P (kjk + L). Calculating xed-lag smoothers by this procedure has the advantage of utilizing the earlier derived Kalman lter. A disadvantage is however that the matrices involved can be quite large when L increases. There does exist other algorithms that are more eÆcient, although we will not this discuss this further here. 9.5 Example In this section we will discuss the eect of smoothing on the random walk example. Example 3 (Random walk, cont.) Consider the random walk model x(k ) = x(k 1) + w(k z (k ) = x(k ) + v (k ): 1) Assume rst we want to estimate x(25) based on data ~z(k) for dierent k's. Now, for k 25, x^(25jk ) =^x(k jk ); P (25jk ) =P (k jk ) + 25 X l=k+1 Q(l 1) which is directly obtained by noticing that x(25) = x(k ) + 25 X l=k+1 w(k ): Consider now the case k > 25. We then need to apply the method described in Section 9.2. First we augment our state variable to x(k ) = x(25) x(k ) : 99 9.5. EXAMPLE 4 3 2 1 0 −1 −2 −3 −4 0 10 20 30 40 50 60 70 80 90 100 12 10 8 6 4 2 0 10 20 30 p 40 50 Figure 9.2: 60 70 80 90 100 x^(25jk) (upper panel) and P (25jk) (lower panel) as a function of k based on the simulated data given in Figure 4.1. In both plots, the vertical dashed line indicate time point 25. giving the state space model x(k ) = 1 0 0 1 z (k ) = 0 1 x (k x(25) x(k ) 0 1) + w(k 1) ; + v(k): By running the Kalman lter on this state space model, we obtain x^(25jk) and P (25jk) for k > 25. In Figure 9.2, x^(25jk) and P (25jk) are plotted for k = 15; :::; 100 using the simulated data given in Figure 4.1. For k < 25, the variance is very high and the estimates depend heavily on the observations. For k > 25, only small corrections are made for the rst proceeding observations, after which no changes are made. Similarly, the variance is stabilizing, reecting that z(k)-observations for k much larger than 25 do not inuence much on x^(25jk). Consider next xed-interval smoothing. We then need to apply the backwards recursions given in Theorem 9.1. Figure 9.3 shows x^(kjN ) and P (kjN ) for N = 100 based on the same data as above. The upper panel shows both the smoothed estimates (solid line) and the ltered ones (dashed line). Note that the smoothed curve is in fact much smoother than the ltered one. This is a typical behavior, due to the use of succeeding points in the estimation. In the lower panel the error variances for the smoothed estimates (solid line) is compared to the error variances for the ltered estimates. Although both curves has the same type of shape, the smoothed one is lower than the ltered one, which again is due to that the smoothed estimates utilizes more 100 CHAPTER 9. PREDICTION AND SMOOTHING 10 8 6 4 2 0 −2 −4 0 10 20 30 40 50 60 70 80 90 100 90 100 6 5 4 3 2 1 0 0 10 Figure 9.3: 20 30 40 p 50 60 70 80 x^(kjN) (upper panel) and P (kjN) (lower panel) as a function of k based on the simulated data given in Figure 4.1. In both plots, the corresponding ltered estimates are shown as dashed lines. of the data. Consider nally xed-lag smoothing. We assume we want to estimate x^(kjk + L) with L = 2. In this case we need to augment the state vector to 0 1 x(k ) x(k ) = @ x(k 1) A x(k 2) giving the state space model 0 1 0 1 1 0 0 1 x(k ) = @ 1 0 0 A x (k 1) + @ 0 A w(k ) 0 1 0 0 z (k ) = 1 0 0 x (k ) + v (k ): Figure 9.4 compares the results from using the xed-lag smoothing (solid lines) with the results using xed-interval smoothing (dashed lines). The upper panel shows the estimates, while the lower panel shows the error variances. Note that for k < 50, the results are almost identical, showing that, given ~z(k + 2), later observations give no extra information about x(k). For k > 50, there are some small dierences between the two smoothers. The reason for this is that in this case there is a larger dependence between the states at dierent time points (the signal to noise ratio is larger). This implies that observations taken further apart from the time point of interest will contain more information, making the need for choosing a larger L. 2 101 9.6. PROBLEMS 8 6 4 2 0 −2 −4 0 10 20 30 40 50 60 70 80 90 100 0.7 0.65 0.6 0.55 0.5 0.45 0.4 0.35 0.3 0 10 20 30 40 50 Figure 9.4: 60 70 80 90 100 For the simulated data given in Figure 4.1, x^(kjk + 2) (solid line) and x^(kjN) (dashed line) is shown in the upper panel, P (kjk + 2) (solid line) and P (kjN) is shown in the lower panel. p p 9.6 Problems Exercise 9.1 (Predicting future in time-invariant models) (a) Consider a time-invariant state space model. Show that x(k + j ) = j x(k ) + j 1 X l=0 j l 1 w (k + l): (b) Show that ^ (k + j jk) = E [x(k + j )j~z(k)] = j x^ (kjk): x (Hint: Use the double expectation rule, conditioning on x(k).) (c) Show that the error term e(k + j jk) = x^ (k + j jk) x(k + j ) is given by e(k + j jk) = j [^ x(k jk) x(k )] j 1 X l=0 j l l w (k + l): (d) Use again the double expectation rule to show that the prediction error covariance matrix P (k + j jk) = VAR[e(k + j jk)] is given by P (k + j jk) = j P (k jk)( + j )T j 1 X l=0 j l 1 Q( j l 1 )T : 102 CHAPTER 9. PREDICTION AND SMOOTHING (e) Consider the univariate model x(k ) = ax(k 1) + w(k z (k ) = x(k ) + v (k ) 1) where fw(k)g and fv(k)g are white noise processes with zero expectations and var[w(k)] = Q, var[v(k) = R. What happens with P (k + j jk) when j ! 1? Discuss for dierent values of a. (f ) Implement the procedure for prediction in Section 9.1 for the model in (e) and check the conclusions you made in (e) with the ones obtained with the prediction lter. Exercise 9.2 (A moving body, cont.) Consider our two-dimensional moving body model (the discrete version given by eqns. (6.24)). (a) Simulate N = 50 observations from the model with your own choice of parameters. (b) Run the Kalman lter to obtain x^ (kjk) and P (kjk) for k = 1; :::; 50. (c) Run the xed interval smoother for obtaining x^ (kjN ) and P (kjN ). Compare with your results in (b). (d) Run the xed lag smoother with dierent values of j . Compare with the results in (c). (e) Assume you are particular interested in the time point k = 25. Run the xed point smoother in combination with the prediction lter to obtain estimates of x(25) at all time points 1; :::; 50. (f ) Repeat (a) (e) for dierent values on the variances for the noise. Try to make some general conclusions. Exercise 9.3 (A simple hydraulic model) We will in this exercise consider a simple hydraulic model for the river \Overow". The purpose of the model is to use it for prediction of possible overow situations on the lowest part of the watercourse. We then need a Kalman lter and observations from the river. The rainfall area is modeled as a series of n reservoirs, each representing partial rainfall areas. The water level in reservoir j at time point t is xj (t) and the water ow out of reservoir is aj xj (t). (Such a reservoir is called linear.). Reservoir number j (j > 1) receive water from the above reservoir j 1 in addition to water 103 9.6. PROBLEMS fall uj (t) + wj (t) which falls down in the sub-area. We then have the following set of linear dierential equations: x_ (t) = a x (t) + u (t) + w (t); x_ j (t) = aj xj (t) + aj xj (t) + uj (t) + wj (t); 1 < j n: In order to get a manageable problem, we will assume that n = 4. All volumes are measured in millimeter (mm) water distributed over the sub-area and all water ows (rates) in mm/day. (NB! One millimeter doesn't sound much, but it becomes a lot if the sub-area is a few square kilometers.) The constants a ; : : : ; a then are on the scale 1/day. Use a = 0:2, a = 0:7, a = 0:2 and a = 0:4. 1 1 1 1 1 1 1 1 1 2 3 4 4 (a) Put the system of dierential equations in state space form. Discretize the model with sampling time equal to 1 day. (That is, nd the matrices and in Matlab .) We assume the rainfall is constant over a day. (b) Assume now we measure the water ow out of reservoir 4. Show that the system then is observable. Why is this not the case if one of the other water ows are measured? (c) Consider now uj for j = 1; : : : ; 4 as external forces on the model. Show that the system is reachable. The initial amounts of water in each reservoir are random variables with mean vector and covariance matrix 0 1 0 1 100 10 0 0 0 B 60 C B 0 0 C C ; P = B 0 15 C =B @ 300 A @ 0 0 20 0 A : 200 0 0 0 15 The covariance matrix is diagonal, which is not the case in reality. (Why?) The water falls have known mean, so we model this mean as an external force uj (k), j = 1; : : : ; 4. We assume 2 (k + 10j ) + 20 mm/day: uj (k ) = 5 sin 365 The random component in the water fall is modeled as system noise wj (k). We further assume that fwj (k)g are independent variables (and independent of the initial state) with mean zero and covariance matrix 0 1 8 0 0 0 B 0 7 0 0 C C Q=B @ 0 0 5 0 A: 0 0 0 2 2 2 2 2 2 2 2 2 104 CHAPTER 9. PREDICTION AND SMOOTHING (d) Calculate the mean and standard deviation for all water levels over 1 year (without ltering). Why does the covariance matrix for the state vector approach a constant matrix? Why is this constant matrix independent of the initial covariance matrix? (e) The Kalman lter is based on daily measures of the water ow out from the lowest reservoir. The measurement errors are independent and Gaussian distributed with expectation zero and standard deviation equal to 2 mm/day. Calculate the standard deviations for the errors in the ltered estimates over 1 year. (f ) After all measurements are collected, it may be of interest to know as exactly as possible the water level in the year that has passed. Calculate therefore the smoothed solution, and plot the standard deviation for the error of the smoothed estimate. (g) We want to be able to predict the water level at time point k + L based on observations up to time point k. How large can L be made so that the standard deviation for the prediction error in the water level is smaller than 95 percent of the standard error based on only using average values? Chapter 10 Filter adaption Theoretical arguments rarely determine all the coeÆcients and covariance matrices f(k)g, fH (k)g, fQ(k)g and fR(k)g inuencing the lter. Those that remain unknown are usually found statistically, that is by tting observed data. Let be the vector of unknown parameters, and D the data. The idea is to adjust so as to be compatible with D. Many ways of doing this have been proposed for linear state space models. We shall here only consider so-called maximum likelihood estimation, which can be formulated very simply and in a way that applies to all state space models simultaneously. Note that we are in this case considering a dierent estimation problem compared to the one discussed in Chapter 5. In this chapter the quantity is assumed to be a xed parameter, in contrast to the situation in Chapter 5 where a random variable was to be estimated. The dierence require separate estimation methods. Maximum likelihood estimation is the most widely used tting technique in statistics in general with a number of good theoretical properties. One such property is that as the number of observations in D becomes innite, then the estimate ^ of will tend to itself. This is known as consistency. To give a mathematical denition, let i be the i'th unknown parameter of the state space model and ^i its estimate, obtained from D in some way. The estimation error is then ^i i. Note that error varies randomly with both the system and measurement noise that have inuenced the actual observations D. We might consider the expected size of the error, for example E [j^i i j] or E [(^i i) ]: Consistency means that either of these quantities converges to 0 as the length of the D-vector tends to innity. The quantity on the right can be decomposed according to E [(^i i ) ] = fb(^i)g + var[^i]; where b(^i) = E [^i i] . This is known as the brutto variance formula and shows that the expected squared error on the left is a sum of a bias term and the variance 2 2 2 105 106 CHAPTER 10. FILTER ADAPTION of ^i. The former expresses whether there is a tendency for ^i to underestimate or overestimate the true i. Usually, with state space models, the variance term is the dominating one. One might wish to carry out the estimation so that E [^i] = i exactly, and hence b(^i) = 0. The variance is then the only contributer to the expected squared error E [(^i i) ]. This situation is known as the unbiased one, and ^i is an unbiased estimate of i. With the state space models it is often hard to obtain such unbiased estimates. One way of constructing an estimate ^ of the vector of unknown parameters, is to reason as follows: The actual data D have been generated from the unknown according to mechanisms that are partially random. This applies both to the realization ~x(N ) of the system process and the realization ~z(N ) of the measurement process. Often we only have ~z(N ) and our data is then D = ~z(N ). If ~x(N ) has been observed as well, then D = (~x(N ); D = ~z(N )). We may in either case consider the likelihood 2 function LD ( ) = p(D j) (10.1) where p(Dj) is the joint probability density function of D. How the function LD () may be computed for given , is the main topic of this chapter. But for the moment the point is that dierent 's produce dierent evaluations of how likely the observed sequence D is. Suppose, for example, that is far from the true . Then LD () might well be rather low, because the actual observations have been generated under a very dierent mechanism and are therefore unlikely to occur under . Carrying this idea further we may vary systematically and compute LD () everywhere. This traces out a surface LD () over the whole -space. The surface is likely to be at its largest when is close to the true . A maximum likelihood estimate ^ ML is a maximum point, i.e. LD (^ ML) = sup LD ( ): (10.2) Note that the argument behind (10.2) is completely general and has nothing to do with state space models as such. However, to nd ^ ML in practice, we have to be able to compute (10.1) and then optimize according to (10.2). This raises several issues. The rst is that LD () = p(Dj) is not even dened unless the probability distribution of the system and measurement noise is known. In principle we need the probability distribution of all noise terms jointly, but in practice we have already made radical simplications in that noise processes were assumed to be white and uncorrelated. In addition, we have to specify the shape of the probability distribution of each v(k) and w(k), see (4.6). It is usual to proceed as if they were Gaussian. The validity of such an assumption is often hard to verify. Neither should it be taken too literally. It does not mean that the lter adaption method we are going to present can not be used for non-Gaussian noise, but it indicates that the method might work best when noise distributions are not too far form Gaussian ones. 107 10.1. OBSERVING THE SYSTEM PROCESS There is a big technical advantage in computing LD () on the basis of Gaussianity. It then turns out that LD () has a simple relationship to the Kalman lter, even when ~x(N ) is not observed, see Section 10.2. To nd ^ ML we may simply invoke the Kalman lter to compute LD () and then apply a general numerical optimization method from a program library such as Matlab to maximize LD (). The only technical problem with this approach is to make the optimizer converge to the maximum of the LD ()surface. This might at times be a serious practical obstacle, and we therefore present (Section 10.3) a special procedure, known as the EM algorithm, that is bound to produce at least a local maximum on the LD ()-surface . Note that for this method the system process x~ (N ) is estimated jointly with . Occasionally ~x(N ) has at a training stage been observed jointly with ~z(N ). The problem of adapting the lter is then simpler. We treat this situation rst. 1 10.1 Observing the System process Suppose fx(k)g; k = 0; :::; N and fz(k)g; k = 1; :::; N are observed. Their joint density can be factorized as p(~x(N ); ~z (N )j ) = p(~x(N )j )p(~z (N )j~x(N ); ): (10.3) Note that we have included x(0) in the denition of ~x(N ). This might not be true in practice, but simplies a technical detail. After mastering the contents of this chapter, the reader will be able to produce the required modications him/herself. Looking rst at p(~x(N )j), an elementary factorization gives p(~x(N )j ) = p(x(0)j)p(x(1)jx(0); )p(x(2)j~x(1); ) p(x(N )j~x(N 1); ) where all the densities on the right hand side is known directly from the model specications (4.6a). In particular, p(x(0); ) = N ((0); P (0j0)); p(x(k )j~x(k 1); ) = N ((k 1)x(k 1); Q(k 1)): Similarly for p(~zj~x). In this case we write p(~z (N )j~x(N ); ) =p(z (1)jx~ (N ); )p(z(2)j~x(N ); ~z(1); ) p(z(N )j~x(N ); ~z(N 1); ): The second factor in (10.3) is equally simple. Now p(z (k )j~x(N ); ~z (k a local is a maximum means that LD ( ) LD () for all in a neighborhood of . For maximum the inequality is valid everywhere. 1 That global 1); ) = N (H (k)x(k); R(k)): 108 CHAPTER 10. FILTER ADAPTION Combining all these factors we obtain from (10.3) the likelihood function 1 1 T 1 Lxz ( ) = e 2x P j x n= = (2) jP (0j0)j N Y 1 1 TQ k 1xk k xk e 2 xk k xk n= = (2 ) j Q(k 1)j k N Y 1 1 z k H k x k TR k 1 z k H k x k 2 : (10.4) e m= jR(k )j = (2 ) k ( (0) 2 (0)) (0 0) ( ( ) ( ( (0) (0)) 1 2 2 1) ( 1)) ( 1) ( ( ) ( 1) ( 1)) 1 2 =1 ( ( ) 2 ( ) ( )) ( ) ( ( ) ( ) ( )) 1 2 =1 which is to be maximized with respect to . It is always better to pass to logarithms because on the original scale Lxz () suers from hopeless numerical underow. A computer usually rounds it o as zero! Since the logarithm is a monotone function, the maximum point of the log-likelihood is equal to the maximum point of the likelihood function itself. Accordingly, consider lxz () = log Lxz () = n log(2) 1 log jP (0j0)j 1 (x(0) (0))T P (0j0) (x(0) (0)) 2 2 2 N X Nn 1 log jQ(k 1)j log(2 ) 2 2k N 1X T 2 k (x(k) (k 1)x(k 1)) Q(k 1) (x(k) (k 1)x(k 1)) N X Nm 1 2 log(2) 2 k log jR(k)j N 1X T (10.5) 2 k (z(k) H (k)x(k)) R(k) (z(k) H (k)x(k)); 1 =1 1 =1 =1 1 =1 which is to be maximized with respect to unknown parameters in f(k)g, fH (k)g, fQ(k)g and fR(k)g in order to obtain the maximum likelihood estimate for . As discussed in Section 4.3, there will be many cases in which nothing is known about the process before the data is observed. In that case the rst line (involving P (0j0)) will be reduced to a constant that can be neglected in the further analysis. Example 3 (Random walk, cont.) Consider the random walk model x(k ) = x(k 1) + w(k z (k ) = x(k ) + v (k ): 1) 109 10.2. MAIN METHOD Assume that Q(k 1) = Q and R(k) = R for all k. Then Q and R are the two unknown parameters and = (Q; R). Suppose P (0j0) = 1. Then the log-likelihood function (10.5) simplies to N X 1 lxz (Q; R) = 2 log(2) 2 log(Q) 2Q k (x(k) x(k 1)) N N 1 X N log(2 ) log( R) 2 2 2R k (z(k) x(k)) : N N 2 =1 2 =1 which is a rare case where the maximum can be found analytically by equating. Firstly N X @ N 1 l (Q; R) = @Q xz 2Q + 2Q2 k=1 (x(k) x(k 1)) 2 which yields Qb ML = 1 N N X k=2 (x(k) x(k 1)) : 2 (10.6) as the maximum likelihood estimate of Q. In the same manner N X @ N 1 2 l (Q; R) = @R xz 2R + 2R2 k=1 (z(k) x(k)) so that RbML N X 1 = N (z(k) x(k)) : 2 k=1 (10.7) From the rst 50 (simulated) samples in Figure 4.1, we obtained Qb =1:34 Rb =0:43 which should be compared with the true values Q = 1:0, R = 0:5. 2 10.2 Main method Suppose is to be determined from z(1); :::; z(N ) without direct knowledge of the underlying system process. The likelihood function (10.1) then becomes Lz ( ) = p(z (1); ::; z(N )j ): 110 CHAPTER 10. FILTER ADAPTION Recall that the right hand side is the joint density function of the observations. It is easy to compute this quantity by means of the Kalman lter. Indeed, we shall show that the likelihood function is given by lz () = log(Lz ( )) N X 1 Nm = 2 log(2) 2 log jS(k)j k N 1 X(z(k) H (k)^x(kjk 1))T S(k) (z(k) H (k)^x(kjk 1)); 2k (10.8) where jS(k)j is the determinant of the matrix S(k). Note that both x^ (kjk 1) and S (k ) in (10.8) are available form the Kalman lter recursion. To compute lz ( ) for given , we need to do no more than invoking the Kalman lter and adding the terms in (10.8) together. Subsequently the sum can be maximized with respect to through some numerical method in order to obtain the maximum likelihood estimate. To prove (10.8) rst recall the elementary factorization p(z (1);z (2); ::; z(N )j) =p(z(1)j)p(z(2)j~z(1); )p(z(3)j~z(2); ) p(z (N )j~z(N 1); ): (10.9) The general term p(z(k)j~z(k 1); ) is the density of z(k) given the history ~z(k 1) of the observed series up to k 1. By (6.8) ~ (kjk 1); z (k ) = H (k )^ x(k jk 1) + z where z~ (kjk 1) is the innovation of z(k) and x^ (kjk 1) = E [x(k)j~z(k 1)] is a constant when ~z(k 1) is xed. Moreover, recall (Lemma 6.1 (6.9b)) that z~ (kjk 1) is stochastically independent of ~z(k 1). It follows that z(k), in the conditional distribution given ~z(k 1), has the same covariance matrix as z~ (kjk 1), which is S (k ). The mean vector is H (k )^ x(k jk 1) and so p(z (k )j~z (k 1); ) = N (H (k )^x(k jk 1); S (k )): The logarithm of the conditional density is thus log p(z(k)j~z(k 1); ) = m2 log(2) 12 log jS(k)j 1 (z(k) H (k)^x(kjk 1))T S(k) (z(k) H (k)^x(kjk 1)); 2 and when these terms are added together, (10.8) follows. =1 1 =1 1 111 10.2. MAIN METHOD Example 4 (iid variables, cont.) Consider again the model x(k ) = x(k 1); z (k ) = x(k ) + v (k ): The unknown quantity we want to estimate in this case is the variance in the observation noise R, so that = R. In this case, we were able to obtain simple analytical expressions for x^(kjk) and P (kjk), given in (6.22) and (6.20), respectively. Using this, and assuming P (0j0) = 1, we obtain k + R= R: k 1 k 1 The log-likelihood for ~z(N ) then becomes N X N 1 k lz (R) = 2 log(2) 2 k log k 1 R N k 1 1 X (z(k) z(k 1)) 2R k k N X N 1 =C 2 log R 2R k k 1 (z(k) z(k 1)) k where C is a constant not depending on R. In order to maximize the log-likelihood with respect to R, we rst calculate the derivative of the log-likelihood: N X @ N1 1 k 1 lz (R) = + (z(k) z(k 1)) @R 2 R 2R k k and putting the derivative to zero, we obtain the maximum likelihood estimate N X 1 k 1 ^ R= (z(k) z(k 1)) : (10.10) S (k ) = R 2 =1 2 =1 2 =1 2 2 =1 N k=1 2 k Some (lengthy?) calculations can be performed to show that this expression actually is equal to N 1X (z(k) z(N )) N 2 k=1 which is the ordinary estimate for the variance. Equation (10.10) does however have the nice feature that it is possible to calculate it recursively, while the standard formulation needs z(N ) to be calculated rst. 2 are some complications due to the fact that S (1) = 1. There is however possible to carry out the calculations rigorous by introducing a nite P (0j0) which in the end is being increased to innity. The same result will however apply. 2 Actually, there 112 CHAPTER 10. FILTER ADAPTION −35 x 10 2.5 2 1.5 1 0.5 0 2 1.5 3.5 3 2.5 1 2 1.5 0.5 1 0 0.5 0 Figure 10.1: Likelihood values for dierent values of Q and R for the random walk model (values smaller than -90 are truncated). The calculations are based on the rst 50 observations of the ones shown in Figure 4.1. In situations where no simple analytical expressions are available, the estimation procedure becomes more complex. This we will see in the next example, which again consider the random walk model: Example 3 (random walk, cont.) Assume now x(k ) = x(k 1) + w(k z (k ) = x(k ) + v (k ): 1) We will again consider the case Q(k 1) = Q and R(k) = R for all k. Although in principle possible, nding the maximum likelihood estimates analytically will in practice be almost impossible, since an analytical expression of the (log-)likelihood becomes rather complex. For a given set of parameters (Q; R), the (log-)likelihood is however easy to calculate using (10.8). For the simulated data-set shown in Figure 4.1, the log-likelihood was calculated for values of Q in the range [0:4; 3:0] and in the range [0; 1:25] for R. The values are plotted as a perspective plot in Figure 10.1. Note the unimodality of the surface. The likelihood is maximized for Q^ = 0:85 and R^ = 0:35, which is not far from the true values Q = 1:0 and R = 0:5. 2 113 10.3. A SAFE APPROACH: THE EM ALGORITHM 10.3 A safe approach: The EM algorithm A disadvantage of the technique in Section 10.2 is that the numerical maximization of the likelihood function requires expertise in optimization. It may not be that easy to make numerical software work properly, especially if is a high-dimensional vector. A safe method which, however, requires more work to implement, is the so-called EM algorithm. This technique is based on the fact that maximum likelihood estimation would have been an easy task numerically if the full likelihood (10.3) was known. Perhaps it works to proceed from an estimate of the log-likelihood and determine as if the estimate was the true value. In particular, it is possible to replace the unknown log-likelihood (10.5) by some estimate ^lxz () where we have replaced all the unknown terms (those involving the system process) with their estimates. We may then maximize l^xz () with respect to . The problem is, however, as in the estimation of the system process, that the estimation of lxz () itself depends on . The situation is thus ! ^lxz () ! estimate of : We may implement this circular relationship recursively. First an initial guess of the parameters produces ^lxz () which then revises the parameters to based on maximizing the likelihood above. We may do this again and again, new values of leading to new estimates of ^lxz () and so forth. In order to apply the estimation procedure above, some type of estimate for the loglikelihood needs to be constructed. One possibility is to estimate x(k) by x^ (kjN ), the xed-interval smoother, and replace the estimates in place of x(k) into the expression (10.5) of lxz (), giving ^lxz () = lxz (). A problem with such an approach is that the variability in the estimates of the system process is not accounted for. The implication of this is that the estimation procedure will lead to wrong results. In particular, variances involved will be underestimated. The \error" that we made when constructing our estimate for lxz () is that we only estimated separate terms in lxz () which we plugged in instead of estimating the function itself. A much better approach is to estimate lxz () directly by its own expectation: (0) (1) ^ ^lxz () = E [lxz ()j~z(N )]: For lxz (), the random components involved are those containing the system process. In general, this expectation can be rather complex. In order to simplify, we will therefore only consider the case n = m = 1. Consider rst the last term of (10.5). 114 CHAPTER 10. FILTER ADAPTION We have E [(z (k ) H (k )x(k ))2 j~z (N ); = (s) ] =E [(z(k) H (k)^x(kjN ) + H (k)^x(kjN ) H (k)x(k))2j~z(N ); = (s)] =E [(z(k) H (k)^x(kjN ))2j~z(N ); = (s)] + E [(H (k)^x(kjN ) H (k)x(k))2j~z(N ); = (s)] + 2E [(z(k) H (k)^x(kjN ))(H (k)^x(kjN ) H (k)x(k))j~z(N ); = (s)] =(z(k) H (k)^x(kjN ))2 + H (k)2P (kjN ): (10.11) An interesting observation is now that all the unknown quantities in this expression can be calculated by the xed-interval smoothing algorithm in Section 9.3. Similar arguments lead to E [(x(0) (0))2 j~z(N )] = (^x(0jN ) (0))2 + P (0jN ) and E [(x(k ) (k 1)x(k 1))2 j~z (N )] =(^x(kjN ) (k 1)^x(k 1jN ))2 + P (k jN ) 2(k 1)cov[x(k ); x(k 1)j~z (N )] + (k 1) P (k 1jN ): 2 In this case we need to do some further work to obtain the full expression in that the last term is not directly available. A relative straightforward way of computing this term is however to augment the state variable by the variable of lagged values, x(k), that is, x(k ) = x(k 1) x(k ) : Using the original state space model, the augmented model becomes 0 1 0 0 (k 1) x (k 1) + w(k 1) ; z (k ) = 0 1 x(k ) + v (k ): x(k ) = Denote the estimates and covariance matrices obtained by running the xed-interval smoother on this model by x^ (kjN ) and P (kjN ). By running the smoothing lter on this augmented state model with = s , the term E [(x(k) x^(kjN ))(x(k 1) x^(k 1jN ))j~z (N ); = s ] is obtained as the o-diagonal element of the covariance ( ) ( ) 115 10.3. A SAFE APPROACH: THE EM ALGORITHM matrix P (kjN ). The estimated likelihood in the univariate case then becomes ^lxz () =C 1 log jP (0j0)j (^x(0jN ) (0)) 2 2P (0j0) N N X 1X (^x(kjN ) (k 1)^x(k 1jN )) log j Q(k 1)j 2k 2Q(k 1) k N X P (k jN ) 2(k 1)cov[x(k ); x(k 1)j~z (N )] + (k 1) P (k 1jN ) 2Q(k 1) k N N X 1X (z(k) H (k)^x(kjN )) log j R(k )j 2k 2R(k) k N X H (k ) P (k jN ) (10.12) 2R(k) : k 2 2 =1 =1 2 =1 2 =1 =1 2 =1 Maximizing this estimated log-likelihood, it can be proved, as part of a more general argument, that the process stabilizes when it has been repeated long enough and that s then has reached a local maximum on the true likelihood surface Lz ( ). We take this local optimum as our estimate ^ . The process may be repeated by starting from dierent initial vectors for , and the one ending with the highest value for Lz () is selected if dierent ^ 's are reached in the end. In the example below such multiple solutions did not occur. Finding a global optimum of surfaces with many local optima is a very diÆcult problem in optimization theory in general. ( ) Example 3 (cont.) We assume P (0j0) = 1. In this case the formula (10.12) simplies to N ^lxz () =C N log(Q) X 1 (^x(kjN ) x^(k 1jN )) 2 2Q k N 1 X 2Q k fP (kjN ) + P (k 1jN ) 2cov[x(k); x(k 1)j~z(N )]g N N X X N 1 1 2 log jRj 2R k (z(k) x^(kjN )) 2R k P (kjN ) 2 =1 =1 2 =1 =1 116 CHAPTER 10. FILTER ADAPTION 1.4 1.2 1 0.8 0.6 0.4 0.2 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Figure 10.2: Estimates of Q and R using the EM algorithm for dierent initial values. The four starting values are marked as stars, while the evolution of the parameter estimates as functions of iterations are shown as curves with dierent types. The common convergence point is marked as a circle. which is maximized by Qb = 1 N N X (^x(kjN ) x^(k 1jN )) k=1 N X +1 N Rb = 1 N 2 k=1 N X k=1 fP (kjN ) + P (k 1jN ) 2cov[x(k); x(k 1)j~z(N )]g; f(z(k) x^(kjN )) + P (kjN )g: 2 Comparing these estimates with eqns. (10.6) and (10.7), we see that some additional variance and covariance terms have appeared. This is due to the uncertainty involved in estimating the system process for a given set of parameters. Using this procedure, we may try to obtain maximum likelihood estimates for Q and R based on the simulated data-set shown in Figure 4.1. Figure 10.2 shows how the estimates changes from iteration to iteration for four dierent initial values on (Q; R). Note that the estimates in each case converge to the same value (0:832; 0:348). 2 In the example above, the likelihood function could be separated into two parts, one involving parameters related to the system process and the other involving parameters related to the observation process. Such a separation is usually obtained also in more complex models, making the maximization part of the algorithm simplify signicantly. 117 10.4. PROBLEMS 10.4 Problems Exercise 10.1 (Linear regression) Consider the regression model z (k ) = ax(k ) + b + v (k ); where the random variables v(k) are Gaussian distributed with expectation 0 and standard error . Let x(k) = k for k = 1; : : : ; N . Because the x's are known in this case, the likelihood function for the observations z (1); :::; z (N ) becomes N Y 1 2 1 L(a; b; ) = p e 22 z k a bx k : 2 k ( ( ) 2 ( )) =1 (a) Why do we not include a distribution for the x's in the likelihood function in this case? (b) Show that the maximum likelihood estimates for a; b and are PN ^b = k P(xN(k) x)(z(k) z) ) k (x(k ) x a^ = z ^bx N X 1 ^ = (z(k) a^ ^bx(k)) 2 =1 2 =1 2 where x = 2 N k=1 P N 1 k=1 x(k ) N and similarly for z. Specify now N = 10; a = 2; b = 1 and = 1:5. (c) Generate in Matlab a set of 10 pair of numbers (x(k); z(k)) by the routine randn | which give independent realizations (we hope) | and estimate a; b and by the estimates above. (d) Repeat the trial in (c) a large number of times and verify that our estimators for a and b are unbiased, that is E [^a] = a and E [^b] = b. (e) Estimate the variances for ^a and ^b. Check these estimates with the theoretical values given by var[^a] = = 2 var[^b] = 2 n X i=1 n X i=1 (xi x) ; 2 xi =(n 2 n X i=1 (xi x) ): 2 118 CHAPTER 10. FILTER ADAPTION p (f ) Make also an analysis of the estimator for . Is ^ = ^ unbiased? What about ^ itself? (g) If you wonder how critical the condition of independence is, you may experiment with situations where this is not the case. The number of possibilities is enormous. 2 2 Exercise 10.2 (Estimation in AR models) Consider the model in Exercise 7.2. The maximum likelihood estimators for c and d can in this case be shown to be the solution of a linear equation system of order 2. (a) Derive the linear equation system. (Hint: Remember the factorization (10.9) and note that all the conditional distributions are given directly from the original model.) (b) Generate some realizations of z(k) for c = 0:8, d = 0:15 and k = 0; : : : ; 100. Estimate c and d based on these realizations. Examine the results to try to give an indication on if the estimators are biased. Also make some measure one variability of the estimates. Exercise 10.3 (Maximum likelihood estimation and the EM algorithm) Consider an extension of the (time-invariant) random walk Example 3 by assuming the system equation is given by x(k ) = ax(k 1) + w(k 1) while the observation equation remains unchanged. We are interested in estimating = (a; Q; R). (a) Assume rst that both the x's and the z's are observed. Calculate analytically the maximum likelihood equations for a; Q and R. (b) Simulate N = 50 observations from the model and try out the estimators you obtained in (a). (c) Use (10.8) to calculate (using Matlab ) the log-likelihood over a grid of points (a; Q; R) and nd the set of values which maximizes the log-likelihood (d) Find an expression for ^l() for our extended model. (e) Implement and run the EM algorithm on your simulated data. (f ) Try dierent values of N . How do the estimators behave as N increases? Chapter 11 Non-linear state estimation Although the theme of this course is linear dynamical models, we will include an introduction to non-linear estimation. The theoretical solution to such problems is well known, but calculation of these optimal solutions are in general complicated to perform. Therefore, until recently only rather approximate and ad hoc techniques have been available in practice. These standard approaches are based on linearization of the non-linearities involved around some trajectory (possibly a point) in the state space. This leads to the linearized and extended Kalman lters (LKF and EKF, respectively) whose central parts are lters based on linear models that estimate deviations from the chosen trajectory. These lters will work well only if the linearization is reasonably accurate. In engineering applications, this may well be the case since the lters then often are parts of a larger feedback loop whose aim is precisely to keep the system at a chosen nominal trajectory. However, in many cases linearization may not be appropriate but is still used in the absence of any alternative. This situation may now be changing. Over the last years, so-called stochastic simulation techniques from computational statistics, originally developed for quite dierent purposes, have been used to design practical lters for general non-linear systems. An outline of this emerging eld is given here after an introduction to the linearization methods. Finally, some examples are presented where the EKF is compared to solutions obtained from stochastic simulation. We note in passing that combined state and parameter estimation may also be formulated as a non-linear estimation problem by including the unknown parameters in an augmented state vector. Using this approach, parameter and state estimates are obtained on-line. The likelihood technique in Chapter 10 was restricted to o-line processing. We will consider an example of this approach at the end of the chapter. 119 120 CHAPTER 11. NON-LINEAR STATE ESTIMATION 11.1 Problem specication Assume the following non-linear state space model: x(k ) = f k (x(k 1)) + w(k 1); (system): (11.1a) z (k ) = hk (x(k )) + v (k ); (observations): (11.1b) Note that both the system and measurement noise processes are assumed additive. As in the linear case, they will be taken to be white and have zero mean. The general estimation theory discussed in Chapter 5 is still valid in this case, making the optimal estimate for x(k) based on ~z(k), under the minimum mean square error criterion, the conditional expectation: ^ (kjk) = E [x(k)j~z(k)]: x (11.2) In addition, the covariance matrix for the estimation error is similarly still a valid measure of the uncertainty involved. In the case of linear models with Gaussian noise terms, the conditional distribution p(x(k )j~z (k )) is also Gaussian and fully specied by its means and covariance matrix. In the non-linear situation, several problems do however occur. First, calculation of the conditional expectation is far from simple. Second, for symmetric distributions, such as the Gaussian one, the minimum square error criterion, resulting in that the conditional expectation is the optimal estimate, may be a reasonable one. For more complex distributions, in particular distributions with several modes (peaks), this criterion may give estimates which give quite wrong pictures about the true state. Consider an example where p(x(k)j~z(k)) is a mixture of two Gaussian distributions, one centered at -5.0 and the other centered at 5.0. The mixture is displayed in Figure 11.1. The mixture distribution indicates that the true state either is around -5.0 or around 5.0. The mean in the distribution is however 0.0, a very unlikely value! Our focus on only calculating the mean and covariance matrix of p(x(k)j~z(k)) is therefore no longer a viable approach if we want to go beyond the linear situation, and other solutions have to be considered. One solution is to approximate our nonlinear model with a linear one and use our algorithms for linear models to compute estimates. This approach has been extensively used for many years, and we will present the main idea in Section 11.2. Linearization may however give poor results in many cases. A better approach is to focus on the problem of representing and propagating the conditional density itself. The recursion for this density is well known and simple to derive: Start with x(0) p(x(0)). Then for k = 1; 2; :::, we have p(x(k )j~z (k 1)) = Z 1 1 p(x(k )jx(k 1))p(x(k 1)j~z(k 1)) dx(k 1); p(x(k )j~z (k )) =Ck p(z (k )jx(k ))p(x(k )j~z (k 1)); (11.3) (11.4) 121 11.2. THE LINEARIZED AND EXTENDED KALMAN FILTERS 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 −6 −4 −2 0 2 4 6 Figure 11.1: Example of a distribution which is a mixture of two Gaussian ones, each with standard deviation equal to 1.0 but with expectations 5:0. where 1=Ck = p(z(k)j~z(k 1)) is a normalization constant once ~z(k) is known. Eq. (11.3) readily follows from the fact that p(x(k )jx(k 1)) = p(x(k)jx(k 1); ~z(k 1)): To see eq. (11.4), note that p(x(k)j~z(k)) = p(x(k); z(k)j~z(k 1))Ck and apply Bayes rule once more to the numerator. Attacking these equations directly has turned out to be an intractable problem at least when the dimension of the state space is not one or two. In higher dimensions, numerical integration gets very time consuming even on present day computers. The computational burden has a tendency to grow exponentially with the dimension, thus rendering the direct approach impractical. In such cases, one often speaks of the curse of dimensionality. 11.2 The linearized and extended Kalman lters The idea behind the linearized Kalman lter is that the system stays reasonably close to some desired, nominal trajectory xN (k) given by the deterministic equation xN (k ) = f k (xN (k 1)). Note that this is not the same as the expected trajectory fE [x(k)]g since in general E [f k (x(k))] 6= f k (E [x(k)]). Think of the nominal trajectory as a given rst order approximation to what the system is doing. For example, in the moving body case this would correspond to the situation where we knew approximately the position and velocity at any time. The point is then to try to estimate deviations from the nominal trajectory using a Kalman lter. Dene the state vector deviation as Æx(k) = x(k) xN (k) and the measurement deviation as Æ z (k ) = z (k ) hk (xN (k )). Assuming that f k can be expanded in a 122 CHAPTER 11. NON-LINEAR STATE ESTIMATION Taylor series, we have x(k ) =f k (x(k 1)) + w(k 1) =f k (xN (k 1)) + @ [email protected](x) jx=xN (k 1) Æ x(k 1) + w(k 1) + h:o:t: where h:o:t: are higher order terms in the Taylor expansion. This leads to the approximate linear equation in the \state" Æx Æ x(k ) (k 1)Æx(k 1) + w(k 1); (11.5) where (k 1) = @ [email protected](x) jx k =xN ( 1) : (11.6) To derive the corresponding measurement equation, we proceed in a similar way: z (k ) =hk (x(k )) + v (k ) =hk (xN (k)) + @ [email protected](x) N Æx(k) + v(k) + h:o:t: jx=x (k) leading to Æ z(k ) H (k )Æ x(k ) + v (k ); where H (k ) = @ hk (x) : @ x jx=xN (k) (11.7) (11.8) Equations (11.5) and (11.7) comprise the state space model for deviations from the nominal trajectory and a standard Kalman lter can be used to compute Æx^ (kjk). The estimate of the actual state vector is obtained as ^ (k x jk) = Æx^ (kjk) + xN (k): (11.9) The variance of the errors in this estimate is provided by the error covariance matrix for Æx^ (kjk). Of course, these variances are only meaningful as long as the linearization is valid. The higher order terms in the Taylor expansions may be regarded as a kind of modeling errors that may or may not lead to divergence of the linearized lter estimates (see Section 8.1 for an example of modeling errors causing divergence). As time increases, one would expect the actual trajectory, driven by the system noise, to deviate more from the nominal one and the linearization to become less valid. In the extended Kalman lter (EKF), one tries to alleviate this potential problem by linearizing about the currently best estimate of the state vector instead of a xed 123 11.3. MONTE CARLO FILTERING nominal trajectory. Computing the state estimate, one also avoids linearizations in the prediction and measurement update steps by using the actual non-linear expressions. The state estimates are thus computed as follows: ^ (k x jk 1) =f k (^x(k 1jk 1)); ^ (kjk) =^x(kjk 1) + K (k)(z(k) x hk (^ x(k jk 1))): (11.10) (11.11) The gain matrix is obtained through the standard covariance recursion (eqs. (6.13)) now based on the matrices (11.12) (k 1) = @ f k (x) @ x jx=x^ (k 1jk and H (k ) = @ hk (x) @ x jx=x^ (kjk 1) 1) : (11.13) The EKF may be further rened by recomputing H (k) through linearization around ^ (kjk), then derive a new gain matrix and repeat the calculation in eq. (11.11) subx stituting x(kjk) on the right hand side. Clearly, this can be repeated as many times as desired. The result is the iterated extended Kalman lter. However, we will not go into these potential improvements as they are all more or less ad hoc. Extensive computer simulations are necessary in each application to \tune" the linearization techniques. 11.3 Monte Carlo ltering Linearization, as described in the previous section, may give poor results. This has however been the only tool available from the inception of the Kalman lter in 1960 to the early 1990's when Monte Carlo ltering appeared. As noted in Section 11.1, numerical integration is not the solution because of the computational burden. The ideas behind the Monte Carlo lter are dierent in that one does not try to compute the conditional distributions themselves. Instead, the distributions are estimated by simulated sample sets which are updated recursively for each time step. From the samples, one may estimate any number of parameters for the distributions (including their mean and covariance matrices). Note that this approach does not require the system and measurement noise in eq. (11.1) to be additive. As it turns out, the system noise can enter in quite general ways as can the measurement noise as long as the likelihood p(z(k)jx(k)) can be computed during the measurement update step. The question is how to sample. Suppose samples of x(0) have been generated (by drawing from p(x(0))). By passing these through the system equation, combining them with samples of the system noise w(0) as required, samples from p(x(1)) are 124 CHAPTER 11. NON-LINEAR STATE ESTIMATION generated. This corresponds to the prediction step in Kalman ltering. There is an additional update step, where the measurement z(1) is used to transform the samples from p(x(1)) into samples from p(x(1)j~z(1)). In general, the Monte Carlo ltering problem is how to convert a collection of samples from p(x(k 1)j~z(k 1)) into one from p(x(k)j~z(k)). The crucial issue is the update step and we will only consider one method - so-called rejection sampling - for carrying this out. Another method, sampling/importance resampling, has also been applied but it seems at present less promising. The update step introduces a requirement that motivates the way the prediction step is carried out. Update is therefore discussed rst. 11.3.1 The update step Suppose it is possible to sample from the prediction density p(x(k)j~z(k 1)). Rejection sampling is a general technique from sampling theory that allows one to integrate the eect of the observation z(k) thus obtaining a sample from p(x(k)j~z(k)). Assume knowledge of a constant M such that p(z (k )jx(k )) M; for all x(k): (11.14) The conditional likelihood p(z(k)jx(k)) is typically easy to compute, at least up to a constant of proportionality (which is enough). The following algorithm returns a random vector from p(x(k)j~z(k)). Rejection sampling (RS): Repeat sample x(k) from p(x(k)j~z(k 1)), sample U from the uniform (0; 1), until U p(z(k)jx(k))=M . Return x(k). The stopping rule ensures that x(k) on return has the right distribution p(x(k)j~z(k)) although it originally came from a dierent one. When the algorithm is run B times, an independent sample set of size B is obtained. Note that the normalization constant Ck in (11.4) is not needed. If the number of tries before a sample is accepted becomes too large, the algorithm will become ineÆcient. An upper limit on the number of tries cannot be stated in principle, but the expected number of tries is proportional to M . For high eÆciency, M in (11.14) should therefore be as small as possible. The average number of tries per sample will be sued as a measure of performance. 125 11.3. MONTE CARLO FILTERING Proof: To prove the algorithm, we need to show that the output x(k) has a probability distribution equal to p(x(k)j~z(k)). Note however that since p(x(k )j~z (k )) / p(x(k )j~z (k 1))p(z (k )jx(k )) (11.15) and a probability distribution always has to integrate to one, it is enough to show that the distribution of x(k) is proportional to the right hand side of (11.15). Note that we have to condition on that x(k) is accepted. From standard probability theory, we have @ Pr(x(k) xjx(k) accepted): p(x (k )jx(k ) accepted) = @x Now, from the way x(k) is generated, we get Pr(x(k) x \Zx(k) accepted) = M1 p(x(k ) = y j~z(k 1))p(z (k )jx(k ) = y ) dy : yx Therefore Z 1 Pr(x (k) accepted) = M y2Rn p(x(k ) = y j~z(k 1))p(z (k)jx(k) = y) dy : Using Bayes rule, we have Pr(x(k) xjx(k) accepted) R p(x(k ) = y j~z(k 1))p(z (k )jx(k ) = y ) dy = R yx z(k 1))p(z (k )jx(k ) = y ) dy y2Rn p(x(k ) = y j~ =C Z yx p(x(k ) = y j~z (k 1))p(z(k)jx(k) = y) dy The scaling factor M disappears and by dierentiating the right hand side, the result shows that x(k) has a density proportional to p(x(k)j~z(k 1))p(z(k)jx(k)) as desired. The key to a successful implementation of the algorithm is that the samples x(k) from p(x(k)j~z(k 1)) do not generally produce insignicant values of the likelihood p(z (k )jx (k )) (relative to M ). Thus, for the method to work well, the likelihood should preferably cover much of the prior density. (In rejection sampling terminology, our likelihood would be called the \envelope".) To what degree this happens in a ltering problem depends on the z actually measured, the function h, and the measurement noise v. The following examples will illustrate this point. Examples of rejection sampling To be specic, assume a one dimensional linear system with both x(0) and w(0) being Gaussian, implying that x(1) is also Gaussian, say x(1) N (m; q). Let us evaluate p(x(1)jz(1)) for two dierent non-linear measurement functions h (x(1)) while 1 126 CHAPTER 11. NON-LINEAR STATE ESTIMATION assuming that v(1) is Gaussian with zero mean and variance r. The likelihood is then simply 1 1 (11.16) p(z (1)jx(1)) = p exp 2r (z(1) h (x(1))) 2r p whose maximum value is M = 1= 2r. Note rst that if r is large compared to the values taken by (z(1) h (x(1))) , the likelihood will always be close to M and most samples from the prediction density will be accepted. In this case, the measurement carries little information and the updated distribution essentially becomes equal to the predicted. Throughout all the following experiments, we will assume m = 2; q = 1, and r = 0:2. Assume that x(1) is observed indirectly through two observes, one having h (x(1)) = x(1) and the other h (x(1)) = 1=x(1) . For each measurement function, we rst consider the case where z(1) = 4:0 for the rst observer while z(1) = 0:25 for the second one. Note that if no observation error is present, both observations correspond to the case where x(1) = 2:0 (the most likely value in the predictive distribution) or x(1) = 2:0. The second case we will consider is when z(1) = 0:25 for the rst observer while z (1) = 4:0 for the second one. This would correspond to x(1) = 0:5 (both unlikely values in the predictive distribution) if no observation errors were involved. In both cases we will illustrate how the predictive distribution p(x(1)) is updated when observation z(1) becomes available. In all the gures, the distribution for x(1) is approximated by samples from it. The distributions are represented by N = 500 samples in all cases. Because we in this simple situation are able to calculate the true distribution, this is added to the plots in order to illustrate the performance of the simulation procedure. The performance (measured as the number of samples that have to be generated before one is accepted) of the rejection sampling algorithm will depend on the match between the predictive distribution p(x(1)) and the distribution for the observation p(z (1)jx(1)). The latter is therefore plotted together with the former. With z(1) = 4:0 and h (x(1)) = x(1) , the likelihood in Figure 11.2 has two sharp peaks at 2:0 and 2:0. With the comparatively small measurement noise in this example (r = 0:2), the measurement z(1) will be dominated by the h (x(1)) term but from the likelihood it is impossible to determine whether x(1) is 2:0 or 2:0. However, since practically all the mass of the predicted density lies to the right of 2:0, the updated density is quite narrow, unimodal and centered around 2:0. Note that the likelihood is always symmetric about the origin in this case. For positive z(k) it is bimodal and for negative z(k) it is unimodal. The narrowness of the likelihood caused an average of 9.4 repetitions in the rejection sampling algorithm in order to accept a sample. 1 2 2 1 2 1 2 1 1 2 1 127 11.3. MONTE CARLO FILTERING 0.9 4 Predicted pdf Likelihood Histogram predicted pdf 0.8 Updated pdf Histogram or rej samp. 3.5 0.7 3 0.6 2.5 0.5 2 0.4 1.5 0.3 1 0.2 0.5 0.1 0 −2 −1 0 1 2 3 4 5 6 0 −2 −1 0 1 2 3 4 5 6 Figure 11.2: Measurement function h1 (x(1)) = x(1)2 with z(1) = 4:0. Left: Predicted distribution with histogram based on B = 500 samples and likelihood. Right: Updated distribution also with histogram obtained through rejection sampling. 0.9 0.7 Updated pdf Histogram or rej samp. 0.8 0.6 0.7 Predicted pdf Likelihood Histogram predicted pdf 0.6 0.5 0.4 0.5 0.4 0.3 0.3 0.2 0.2 0.1 0.1 0 −3 −2 −1 0 1 2 3 4 5 6 0 −3 −2 −1 0 1 2 3 4 5 6 Figure 11.3: Measurement function h1 (x(1)) = 1=x(1)2 with z(1) = 0:25. Left: Predicted distribution with histogram based on B = 500 samples and likelihood. Right: Updated distribution also with histogram obtained through rejection sampling. Turning to h (x(1)) = x2 and z(1) = 0:25 in Figure 11.3,pthis term will not dominate z (1) since the measurement noise has standard deviation 0:2 0:44. With this z (1), the likelihood will exclude areas around 0 and attach signicant weight to values larger than 1 in absolute value. The result is an updated density considerably wider than in the previous case. Since there is a better match between the predictive distribution and the likelihood in this case, the average number of repetitions in the rejection sampling algorithm now reduced to 1:3. In Figures 11.4 and 11.5, z(1) = 0:25 and = 4:0 for the rst and second observer, respectively. Compared to the previous case, this produces a smaller measurement for h (x(1)) = (x(1)) and a larger one for h (x(1)) = x2 . This time the latter measurement function produces the sharpest estimate although the updated distribution has a small second peak at 0:5. For the rst observed, an average of 8:0 repetitions 1 (1) 1 1 2 1 1 (1) 128 CHAPTER 11. NON-LINEAR STATE ESTIMATION 0.9 1.4 Updated pdf Histogram or rej samp. Predicted pdf Likelihood Histogram predicted pdf 0.8 1.2 0.7 1 0.6 0.8 0.5 0.4 0.6 0.3 0.4 0.2 0.2 0.1 0 −2 −1 0 1 2 3 4 5 6 0 −2 −1 0 1 2 3 4 5 6 Figure 11.4: Measurement function h1 (x(1)) = x(1)2 with z(1) = 0:25. Left: Predicted distribution with histogram based on B = 500 samples and likelihood. Right: Updated distribution also with histogram obtained through rejection sampling. 0.5 14 Updated pdf Histogram or rej samp. Predicted pdf Likelihood Histogram predicted pdf 0.45 12 0.4 10 0.35 0.3 8 0.25 6 0.2 0.15 4 0.1 2 0.05 0 −3 −2 −1 0 1 2 3 4 5 6 0 −3 −2 −1 0 1 2 3 4 5 6 Figure 11.5: Measurement function h1 (x(1)) = 1=x(1)2 with z(1) = 4:0. Left: Predicted distribution with histogram based on B = 500 samples and likelihood. Right: Updated distribution also with histogram obtained through rejection sampling. were needed for generating a sample while the corresponding number for the second observer was as high as 100:6! The narrowness of the likelihood is again the reason for this. Note also that we have an increase in the number of repetitions for both observers compared to the rst case. This is because the observations in the last case correspond to more unlikely x(1) values in the predictive distribution. 11.3.2 Monte Carlo prediction The next question is how to sample from the prediction density p(x(k)j~z(k 1)) when B realizations xb (k 1), b = 1; : : : ; B from the preceding update density p(x(k 1)j~z(k 1)) are at disposal. All of x (k 1); : : : ; xB (k 1) may be inserted into the system equation (11.1a). When system noise is drawn, B samples from p(x(k)j~z(k 1 129 11.3. MONTE CARLO FILTERING 1)) are generated, but that is not enough. The rejection update algorithm above clearly requires generating samples from p(x(k 1)j~z(k 1)) many (at least B , but typically more) times to produce B samples from p(x(k)j~z(k)), since many of the samples are rejected. Starting with a very large number B for k = 0 and then reducing B with increasing k is not a solution. That would imply unnecessary much computational work in the beginning and we would eventually run out of samples in an unpredictable way. A rational approach to this problem is to use the B samples to estimate p(x(k 1)j~z(k 1)) and then draw samples from the estimated distribution as many times as we want. Density estimation is a highly developed area of statistics with a host of possible methods available. As a rst approach, one might believe that sampling could be performed from the histogram representing the updated distribution. This would mean to attach probability 1=B to each of the B samples and sample with replacement. However, when there is little system noise, the Monte Carlo lter would propagate almost the same sample values from one time step to the next, and the number of dierent samples would tend to collapse to a few or even a single value. Consider instead a smoothed version of the histogram. The procedure is simple to carry out. First derive a set of corrected samples each assigned a probability 1=B : B (k 1) + f(1 )B=(B 1)g = (xb(k 1) x B (k 1)) xb (k 1) = x 2 1 2 for b = 1; 2; : : : ; B . Here x B (k 1) is the mean vector of fxb(k 1)g, P B (k 1) the corresponding sample covariance matrix, and 2 [ 0; 1 ] is a user selected parameter. It is easy to verify that the distribution has mean x B (k 1) and covariance matrix (1 )P B (k 1). To generate a sample from the distribution we want, draw a corrected sample xb(k 1) and add an independent Gaussian variable with mean zero and covariance matrix P B (k 1). Clearly, the resulting distribution has a mean and covariance matrix equal to x b and P B (k 1), respectively. The distribution is a continuous one and thus represents a smoothed version of the histogram. Note that there is no restriction for to be Gaussian. Any continuous distribution might do (and will typically not inuence much on the results), but we will for simplicity concentrate on the Gaussian one. The choice of is more crucial, and has been given much consideration in the literature about density estimation. In particular it must depend on both the uncertainty involved in the samples and on the sample size. One possible choice is to use 2 2 = CB 1 n+4 q jP B (k 1)j where C is some constant that can be specied by inspection of the results. In detail, the algorithm runs as follows: (11.17) 130 CHAPTER 11. NON-LINEAR STATE ESTIMATION Sampling from the (estimated) prediction density. Draw b with equal probabilities from f1; : : : ; B g. Sample from N (0; P B (k 1)). 2 Compute x(k 1) = x B (k 1) + (1B 1)B 2 1=2 (xb(k 1) x B (k 1)) + Sample w(k 1) and return x(k) = f (x(k 1)) + w(k 1)); The complete Monte Carlo lter is obtained by combining the prediction samples generated in this way with the rejection algorithm in the previous section. 11.4 Examples of non-linear ltering As noted above, the EKF has been the standard approach for generating approximate solutions to non-linear ltering problems. Except in simple cases, the degree of approximation has been hard or impossible to assess. The Monte Carlo techniques oer the hope of radically better solutions in that the underlying distributions themselves are approximated. Much work remains to be done regarding the properties of these lters. However, as a rule of thumb we may say that suÆcient accuracy has been obtained once the results do not change appreciably by increasing B . Example 5 Example from Gordon, Salmond & Smith (1993) originally proposed by Kitagawa (1987). Consider the system x(k 1) 1) + 1 25 + x (k 1) + 8 cos(1:2(k 1)) + w(k 1); x (k ) z (k ) = 20 + v(k): x(k ) = 0:5x(k 2 2 Disregarding the cosine term and the system noise, we have a stable non-linear dierence equation. The cosine term causes the system to undergo periodic oscillations and the system noise makes the oscillations random. As noted above, the squared term in the measurement equation causes the likelihood to be bimodal for positive z and unimodal for negative z. The example is designed so that z is positive most of the time. This will be one reason why the EKF is not expected to work very well. The EKF may be thought of as approximating the predicted and updated distributions by Gaussians and we know that with the quadratic measurement equation this may be inappropriate. In the example, q = 10; r = 1 and x(0) N (0; 5). The number of updated samples is set at B = 1000 (which is too much really). 131 11.4. EXAMPLES OF NON-LINEAR FILTERING 30 30 EKF estimate True state MC estimate True state 20 20 10 10 0 0 −10 −10 −20 −20 −30 0 5 10 15 20 25 30 35 40 45 50 −30 0 5 10 15 20 25 30 35 40 45 50 120 30 MC stand.dev. EKF stdand.dev. 100 25 80 20 60 15 10 40 5 20 0 0 5 10 15 20 25 30 35 40 45 50 0 −21.5 −21 −20.5 −20 −19.5 −19 −18.5 −18 −17.5 Figure 11.6: Example from Gordon et al. (1993). Upper left panel: State estimate from EKF and true state. Upper right panel: State estimate from MC lter with true state and measurements. Lower left panel: Standard deviation of the EKF estimate error as computed by the EKF and the corresponding standard deviation from the MC lter. Lower right panel: Histogram of samples from p(x(25)j~z(25)). Figure 11.6 compares the results of the EKF and the Monte Carlo lter. Clearly, the latter is superior. The EKF generally exhibits fairly large deviations from the true state with some extreme estimates at k = 17; 30 and 41. Note also that the estimate of the EKF standard deviation does not reect the real error; the EKF is over-optimistic regarding its own performance. The Monte Carlo lter, on the other hand, follows the true state quite well and is realistic regarding its performance. Note that the estimates in most cases have the right signs. In the Monte Carlo lter, we have used the mean of samples from p(x(k)j~z(k)) as an estimate of x(k). As noted above, the mean correspond to the optimal estimator under the minimum square error criterion. Such a criterion is however only sensible if the distribution is unimodal and fairly symmetric. The lower right panel of Figure 11.6 shows the histogram of samples from p(x(25)j~z(25)). The distribution does look wellbehaved in this case, implying that the mean might be a reasonable estimate. 2 Example 6 (Direction measurements) The next example (from Bucy & Senne (1971)) is two-dimensional with a linear system equation and a non-linear measurement equation. The example models an observer, rotating on the unit circle with constant angular velocity, measuring the direction to an object moving randomly in the plane. The aim is to estimate the position (x ; x )T 1 2 132 CHAPTER 11. NON-LINEAR STATE ESTIMATION of the object based only on the directional information. The model is given by x1(k ) = 0:5x1 (k 1) + w1 (k 1) x2(k ) = x2(k 1) + w2(k 1) and x (k ) sin(k ) z (k ) = atan 2 + v(k) x1 (k ) cos(k ) Here the covariance matrices for the noise terms are given by Q= 0:1 0:05 0:05 0:1 and R = 0:1: Note that the system equation is linear in this case, making (k 1) = 0:5 0 ; 0 1 that is constant for all k in the EKF. Further, @ h (x) @ x k jx=x^ (kjk 1) =C (^x(kjk 1)) x^2(kjk H (k ) = 1) + sin(k); x^ (kjk 1) cos(k) 1 where 1)) = (^x (kjk 1) cos(k)) +1 (^x (kjk 1) sin(k)) The upper panel of Figure 11.7 shows the results obtained by EKF for data simulated from the model. In particular for x (k) (but also for x (k)) the result is poor. The estimate diverge although the true system process is stable! The Monte Carlo lter was run with B = 1000, and for each k, the mean of the simulated samples from p(x(k)jz(k)) was used as an estimate for x(k). These estimates are shown in the middle panel of Figure 11.7. As for the previous example, the Monte Carlo lter obtains much better results, although even the Monte Carlo lter has problems estimating x (k) reasonable correct. The lower panel shows the estimated variances involved. Note that the variance of x (k) obtained by the MC lter seems to diverge in this case. If this actually can happen is still an open question. Also in this case we want to inspect the actual distribution of x(k) in order to evaluate if the mean is a realistic estimate. Figure 11.8 shows the histograms of samples from p(x(25)j~z (25)). Since both histograms are unimodal and fairly symmetric, it is reasonable to use the mean as an estimate of the state. 2 C (^x(k jk 2 1 2 1 2 2 2 1 133 11.4. EXAMPLES OF NON-LINEAR FILTERING 1 10 0.8 EFK estimate 8 0.6 True state 0.4 6 0.2 0 4 −0.2 2 −0.4 −0.6 0 EFK estimate −0.8 −1 0 True state 5 10 15 20 25 30 35 40 45 50 −2 0 1 5 10 15 20 25 30 35 40 45 50 45 50 10 0.8 MC estimate 8 0.6 True state 0.4 6 0.2 0 4 −0.2 2 −0.4 −0.6 0 MC estimate −0.8 −1 0 True state 5 10 15 20 25 30 35 40 45 50 −2 0 0.8 5 10 15 20 25 30 35 40 1.6 EFK standard errors 0.7 1.4 MC standard errors EFK standard errors 1.2 MC standard errors 0.6 1 0.5 0.8 0.4 0.6 0.3 0.4 0.2 0.1 0 0.2 5 10 15 20 25 30 35 40 45 0 0 50 5 10 15 20 25 30 35 40 45 50 Figure 11.7: Direction measurements. Upper panel: State estimate from EKF and true state. Middle panel: State estimate from MC lter with true state and measurements. Lower panel: Standard deviation of the EKF estimate error as computed by the EKF and the corresponding standard deviation from the MC lter. 120 120 100 100 80 80 60 60 40 40 20 20 0 −1 −0.5 0 0.5 1 1.5 0 −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0 Direction measurements. Histogram of samples from p(x(25)j~z(25)). Left panel shows x1(25) while right panel shows x2 (25). Figure 11.8: Example 7 (Parameter estimation) The third example is a parameter estimation problem. The original system is linear x1(k ) = ax1(k 1) + w1 (k z (k ) = x1(k ) + v (k ); 1); but the constant a is unknown. To estimate it, dene a second state variable as 134 CHAPTER 11. NON-LINEAR STATE ESTIMATION 2.5 EKF estimate True state EKF estimate True state 2.4 1.1 2.3 1 2.2 2.1 0.9 2 0.8 1.9 1.8 0.7 1.7 0.6 1.6 1.5 0 5 10 15 20 25 30 35 40 45 50 0.5 0 5 10 15 20 25 30 35 40 45 50 2.5 True state MC estimate True state MC estimate 2.4 1.1 2.3 1 2.2 2.1 0.9 2 0.8 1.9 1.8 0.7 1.7 0.6 1.6 1.5 0 5 10 15 20 25 30 35 40 45 50 1.8 0.5 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 30 35 40 45 50 1.4 1.6 1.2 1.4 1 1.2 0.8 1 0.8 0.6 0.6 0.4 0.4 0.2 0.2 0 0 5 10 15 20 25 30 35 40 45 50 0 Figure 11.9: Parameter estimation. Left: State estimates and true states for x(k). Right: State estimates and true states for a. The upper panels show the EKF estimates, the middle panels show the estimates from the MC lter, while the lower panel shows the variance estimates. x2(k ) = a and derive the augmented non-linear system x1(k ) = x2(k x2(k ) = x2(k 1)x (k 1) + w (k 1); 1); 1 1 and z (k ) = x2 (k )x1(k ) + v (k ): Figure 11.9 shows the results obtained by applying the EKF and the MC lter on simulated data from this model. In this case, EKF is doing a reasonable job. Even so, the MC lter (using B = 1000 and taking the means as estimates) again perform better. Note in particular how the estimate of a converge towards its true value a = 1:0. The histograms in Figure 11.10 indicate that using the mean is sensible in this case also. 2 135 11.5. PROBLEMS 90 140 80 120 70 100 60 50 80 40 60 30 40 20 20 10 0 2.1 2.2 2.3 2.4 2.5 2.6 2.7 2.8 0 0.97 0.98 0.99 1 1.01 1.02 1.03 1.04 Figure 11.10: Parameter estimation. Histogram of samples from p(x(25)j~z(25)). Left panel shows x1(25) while right panel shows x2(25). 11.5 Problems Exercise 11.1 (EKF and the linearized lter) Explain in detail why the EKF may be expected to work better than the linearized lter. Exercise 11.2 (Random walk) Consider again the random walk example, that is the model x(k ) =x(k 1) + w(k z (k ) =x(k ) + v (k ) 1) where w(k 1) N (0; Q), and v(k) N (0; R). (a) Extend the state vector to x(k) = (x(k); (k))T and show that the system equations now can be written as x(k ) = x (k 1) + "(k 1) z (k ) = H x(k ) + v (k ) where now "(k 1) N (0; Qm), and Qm is a 2 2 matrix with only non-zero element which is equal to one. Specify ; Qm and H . (b) Run a Kalman lter on the extended model and show that the point estimates and the variances obtained for x(k) are equal to those obtained by the original model. Use Q = 1:0; R = 0:5 and m0 = 0:0; P 0 = 1:0. Now our aim of extending the state vector is not to obtain point estimates in another way, but because the extended model is more suitable for estimation of parameters using the ideas discussed in this chapter. We will only consider estimation of Q, assuming R is known. p (c) Extend the model further by including Q in the state vector. Write down the system and observation equations in this case. 136 CHAPTER 11. NON-LINEAR STATE ESTIMATION (d) Discuss why we have chosen to include Q in the model from (a) and not directly from the original model. (e) Implement the extended Kalman lter on the model from (c) and use this to estimate Q. (f ) Now implement the Monte Carlo lter. Does this lter obtain better results than the extended Kalman lter? Appendix A Matlab commands Properly used, Matlab is a superb experimental tool that allows us to explore the theory as well as getting suggestions for possible underlying theoretical relationships. In this chapter, we list some useful Matlab commands. For most of the examples, also included are the Matlab codes and functions which have been used for obtaining the dierent results. To get the details, type help followed by the function name in the Matlab window. In Matlab , indices of vectors and matrices run from 1 and upwards. This cause some problems concerning the covariance matrices fQ(k)g of the state vector, since these run from 0 and upwards. In all the routines we therefore have used Q(k) as the covariance matrix for w(k 1) instead. A.1 Properties of deterministic systems In this section, we list some useful commands for the concepts discussed in Chapter 2. Solving dierential equations like (2.1) in Matlab can be performed by using the commands ode23 or ode45, employing Runge-Kutta numerical integration methods of order 2/3 and 4/5, respectively. (One may also use the graphically based SIMULINK package within Matlab to solve dierential (and dierence) equations.) For time-varying systems, the routines lsim and dlsim can be applied. Matlab provides the routines c2d and c2dm (continuous - to - discrete) for transforming a continuous system to a discrete one. These routines compute the matrices and for time invariant systems with a xed sampling time (cf. (2.7)). In order to compute eigenvalues of a matrix, the command eig can be used. For calculating a matrix exponential, the routine expm is appropriate. In order to check reachability, Matlab provides the special purpose routines ctrb and 137 138 ctrbf. A.2 APPENDIX A. MATLAB COMMANDS For observability, Matlab has special purpose routines obsv and obsvf. Matlab commands for the Gaussian distribution Some useful commands related to Gaussian distributions are randn Normally distributed random numbers (see also normrnd). mvnrnd Random matrices from the multivariate normal distribution. normcdf Normal cumulative distribution function (cdf). normpdf Normal probability density function (pdf). normspec Plots normal density between specication limits. A.3 Code concerning the random walk example The following Matlab commands were used for obtaining the variables in Figure 4.1: randn('state',0); n = 100; Q = [ones(n/2,1); ones(n/2,1)/10]; R = ones(n,1)/2; w = normrnd(zeros(n,1),sqrt(Q)); x = cumsum(w); z = x+normrnd(zeros(n,1),sqrt(R)); a = 1:n; plot(a,x,a,z,'*'); The following routine was used for using the Kalman lter on the data from Example 3: function [xhat,P] = klm_rw(z,Q,R) %Kalman filter for random walk model %Assumes P(0) is infinity [n,m] = size(z); P = zeros(n,1); xhat = zeros(n,1); P(1) = R(1); xhat(1) = z(1); for k=2:n P1 = P(k-1)+Q(k); S = P1+R(k); A.3. CODE CONCERNING THE RANDOM WALK EXAMPLE 139 K = P1/S; P(k) = (1-K)*P1; x1 = xhat(k-1); xhat(k) = x1+K*(z(k)-x1); end The following routine was used for calculating the xed-point smoothers for the random walk process: function [xhatt,Pt] = smo_point_rw(z,Q,R,t) %Routine for estimating x(t) based on z(1),...,z(k) [n,m] = size(z); Pt = zeros(n,1);xhatt = zeros(n,1); P = R(1); %k=1, using P(0) equal to infinity xhat = z(1); Pt(1) = P + sum(Q(2:t));xhatt(1) = xhat; for k=2:t %Kalman filter up to t P1 = P+Q(k);S = P1+R(k);K = P1/S;P = (1-K)*P1; x1 = xhat;xhat = x1+K*(z(k)-x1); xhatt(k) = xhat; %Prediction of x(t) if(k<t) %Prediction error of x(t) Pt(k) = P + sum(Q((k+1):t)); else Pt(k) = P; end end P = [P,P;P,P];H = [0,1]; %Augmented model for k>t xhat = [xhat;xhat]; for k=(t+1):n %Kalman on augmented model P1 = P+[0,0;0,Q(k)];S = H*P1*H'+R(k);K = P1*H'./S; P = (diag([1,1])-K*H)*P1; x1 = xhat;xhat = x1+K*(z(k)-H*x1); xhatt(k) = xhat(1); Pt(k) = P(1,1); end For the xed-interval smoother, only a small routine (utilizing the Kalman lter routine dened above) is needed: function [xhatn,Pn] = smo_int_rw(z,Q,R) %Function for fixed-inverval smoothing for the random walk model [n,m] = size(z); 140 APPENDIX A. MATLAB COMMANDS [xhat,P] = klm_rw(z,Q,R); %Kalman filter Pn= zeros(n,1);xhatn = zeros(n,1); Pn(n) = P(n);xhatn(n) = xhat(n); %k=n directly from Kalman for k = (n-1):-1:1 %Downwards recursion P1 = P(k)+Q(k+1);A = P(k)/P1; Pn(k) = P(k)+A*(Pn(k+1)-P1)*A; xhatn(k) = xhat(k)+A*(xhatn(k+1)-xhat(k)); end Fixed-lag smoothing: function [xhatL,PL] = smo_lag_rw(z,Q,R,L) %Routine for estimating x(k) based on z(1),...,z(k+L) [n,m] = size(z); H = [1,zeros(1,L)]; %Parameters in augmented model Phi=[1,zeros(1,L);diag(ones(1,L)),zeros(L,1)]; Qm = zeros(L+1,L+1); %Qm(1,1) is set in each iteration P = diag(10000*ones(L+1,1)); %Initial values xhat = diag(zeros(L+1,1)); Qm(1,1) = Q(1); PL = zeros(n,1); xhatL = zeros(n,1); for k=1:n %Kalman filter Qm(1,1) = Q(k); P1 = Phi*P*Phi'+Qm;S = H*P1*H'+R(k);K = P1*H'./S; P = (diag([ones(1,L+1)])-K*H)*P1; x1 = Phi*xhat;xhat = x1+K*(z(k)-H*x1); PL(k) = P(L+1,L+1);xhatL(k) = xhat(L+1); end xhatL = xhatL(L+1:n);PL = PL(L+1:n); %Shift Calculation of (log-)likelihood. function [loglik,Qhat,Rhat] = loglik_rw(z,q,r) %Function for calculating log-likelihood for all values of Q and R %given in the vectors q and r %Also find the ML-estimates [n,m] = size(z); [Qm,Rm] = meshgrid(q,r); konst = -0.5*n*log(2*pi); loglik = zeros(size(Qm))+konst; %Kalman filter for all Q and R values simultaneously A.4. CODE CONCERNING THE MOVING BODY EXAMPLE 141 P = Rm; xhat = ones(size(Qm)).*z(1); for k=2:n P1 = P+Qm; S = P1+Rm; K = P1./S; P = (1-K).*P1; x1 = xhat; xhat = x1+K.*(z(k)-x1); loglik = loglik - 0.5.*log(S)-0.5.*(z(k)-x1).^2./S; end Qhat = Qm(loglik==max(max(loglik))); Rhat = Rm(loglik==max(max(loglik))); Note the use of matrix calculations, making it possible to calculate the (log-)likelihood for all sets of values for (Q; R) simultaneously. Estimation by the EM-algorithm: function [Qhat,Rhat] = em_rw(z,Q0,R0) [n,m] = size(z); s = 1; Qhat = Q0; Rhat = R0; conv = 1; while (conv>0.0001 & s < 1000) s = s+1; [xhatn2,Pn2] = smo_rw2(z,Qhat(s-1),Rhat(s-1)); Qhat = [Qhat,mean(Pn2(2,2,2:n))+ mean((xhatn2(2,2:n)-xhatn2(2,1:n-1)).^2)+ mean(Pn2(2,2,1:n-1)-2*Pn2(1,2,1:n-1))]; Rhat = [Rhat,mean((z'-xhatn2(2,:)).^2)+mean(Pn2(2,2,:))]; conv = abs(Qhat(s)-Qhat(s-1))+abs(Rhat(s)-Rhat(s-1)); end A.4 Code concerning the moving body example A routine for simulating from the (discrete) moving body model (6.24): function [x,z]= body(Phi,H,R,Q,P0,m0,N) % Simulates the moving body example 142 APPENDIX A. MATLAB COMMANDS % % Phi : System matrix % H: Measurement matrix % R: Measurement error variance % Q: System noise covariance matrix (assumed to be diagonal) % P0: Initial error covariance matrix (assumed to be diagonal) % m0: Imitial expected state estimate % N: Number of time steps to compute the solution % x: Realization of the state vector - a (2,iter) matrix % z: Realizations of the measurements - a (1,iter) matrix % x=zeros(2,N); z=zeros(1,N); x(:,1)= m0+[sqrt(P0(1,1))*randn(1,1);sqrt(P0(2,2))*randn(1,1)]; for k=2:N x(:,k)=Phi*x(:,k-1)+[sqrt(Q(1,1))*randn(1,1);sqrt(Q(2,2))*randn(1,1)]; z(k)=H*x(:,k) + sqrt(R)*randn(1,1); end Kalman ltering of model (6.24): function [xhat, xhat1,P,P1,K]= kfbody(z,Phi,H,R,Q,P0,m0,N) % % Generates Kalman filter estimates of the state vector of the % moving body. % The covariance matrix and Kalman gain also calculated. % % xhat: xhat(k|k) % xhat1: xhat(k|k-1) % P: P(k|k) % P1: P(k|k-1) % K: Kalman gain % % Initializing xhat=zeros(2,N); xhat1=zeros(2,N); P=zeros(2,2,N); P1=zeros(2,2,N); K=zeros(2,N); % % k=1 P1(:,:,1)= Phi*P0*Phi' + Q; A.4. CODE CONCERNING THE MOVING BODY EXAMPLE S= H*P1(:,:,1)*H' + R; K(:,1)= P1(:,:,1)*H'/S; P(:,:,1)= (eye(2) - K(:,1)*H)*P1(:,:,1); xhat1(:,1)= Phi*m0; xhat(:,1)= xhat1(:,1) + K(:,1)*(z(1) - H*xhat1(:,1)); % Remaining k's for k=2:N P1(:,:,k)= Phi*P(:,:,k-1)*Phi' + Q; S= H*P1(:,:,k)*H' + R; K(:,k)= P1(:,:,k)*H'/S; P(:,:,k)= (eye(2) - K(:,k)*H)*P1(:,:,k); xhat1(:,k)= Phi*xhat(:,k-1); xhat(:,k)= xhat1(:,k) + K(:,k)*(z(k) - H*xhat1(:,k)); end Prediction: function [xpred, Ppred]= kfbody_pred(z,Phi,H,R,Q,P0,m0,k,j) % % Make predictions of position at k+1,...,k+j % for the moving body model % % xpred: xhat(k+l|k) % Ppred: P(k+l|k) % % First run Kalman filter [xhat, xhat1,P,P1,K]= kfbody(z,Phi,H,R,Q,P0,m0,k); % Then run Kalman filter on modified model Hm = [0,0]; xpred=zeros(2,k+j); Ppred=zeros(2,2,k+j); xpred(:,1:k)=xhat; Ppred(:,:,1:k)=P; % % j=1 P1= Phi*P(:,:,k)*Phi' + Q; S= Hm*P1*Hm' + R; K= P1*Hm'/S; 143 144 APPENDIX A. MATLAB COMMANDS Ppred(:,:,k+1)= (eye(2) - K*Hm)*P1; xhat1= Phi*xhat(:,k); xpred(:,k+1)= xhat1 + K*(0 - Hm*xhat1); % Remaining l's for l=2:j P1= Phi*Ppred(:,:,k+l-1)*Phi' + Q; S= Hm*P1*Hm' + R; K= P1*Hm'/S; Ppred(:,:,k+l)= (eye(2) - K*Hm)*P1; xhat1= Phi*xpred(:,k+l-1); xpred(:,k+l)= xhat1 + K*(0 - Hm*xhat1); end Kalman ltering with K (k) = 0: function [xhat, xhat1,P,P1,K]= kfbody_k0(z,Phi,H,R,Q,P0,m0,N) % % Generates Kalman filter estimates using K=0 % Initializing xhat=zeros(2,N); xhat1=zeros(2,N); P=zeros(2,2,N); P1=zeros(2,2,N); K=zeros(2,N); % % k=1 P1(:,:,1)= Phi*P0*Phi' + Q; S= H*P1(:,:,1)*H' + R; K(:,1)= 0; P(:,:,1)= (eye(2) - K(:,1)*H)*P1(:,:,1); xhat1(:,1)= Phi*m0; xhat(:,1)= xhat1(:,1) + K(:,1)*(z(1) - H*xhat1(:,1)); % Remaining k's for k=2:N P1(:,:,k)= Phi*P(:,:,k-1)*Phi' + Q; S= H*P1(:,:,k)*H' + R; K(:,k)= 0; P(:,:,k)= (eye(2) - K(:,k)*H)*P1(:,:,k); A.5. NONLINEAR FILTERING 145 xhat1(:,k)= Phi*xhat(:,k-1); xhat(:,k)= xhat1(:,k) + K(:,k)*(z(k) - H*xhat1(:,k)); end A.5 Nonlinear ltering A.5.1 Rejection sampling A direct implementation of rejection sampling for the example in Section 11.3.1: function [x,rej] = rej_samp(z,m,q,r,B) x = zeros(B,1); rej = zeros(B,1); M = 1/(sqrt(2*pi*r)); for i=1:B x(i) = normrnd(m,sqrt(q)); u = rand(1); p = normpdf(z,x(i)^2,sqrt(r))/M; rej(i) = 0; while(u>p) rej(i) = rej(i)+1; x(i) = normrnd(m,sqrt(q)); u = rand(1); p = normpdf(z,x(i)^2,sqrt(r))/M; end end For loops through all the samples may be ineÆcient in Matlab . An alternative is to generate all samples simultaneously by utilizing the vector ability of Matlab . In particular, at the rst step B samples are generated. At the next only those samples that not were accepted are sampled again, and this continue untill all samples are generated. The code is as follows: function x = rej_samp_fast(z,m,q,r,B) M = 1/(sqrt(2*pi*r)); %Sample for all B x = normrnd(m,sqrt(q),B,1); 146 APPENDIX A. MATLAB COMMANDS u = rand(B,1); p = normpdf(z,x.*x,sqrt(r))/M; acc = u <= p; samp = B; ind = 1:B; while(sum(acc)<B) ind2 = ind(acc==0); n_a = length(ind2); samp = samp+n_a; x(ind2) = normrnd(m,sqrt(q),n_a,1); u(ind2) = rand(n_a,1); p = normpdf(z,x.*x,sqrt(r))/M; acc = u <= p; end Simulating data for Example 5: function [x,z] = sim_gss(Q,R,m0,P0,N); %Generating sample from Gordon, Salmond & Smith example x0 = normrnd(m0,sqrt(P0)); x = zeros(N,1); z = zeros(N,1); x(1) = 0.5*x0+25*x0/(1+x0^2)+8*cos(1.2*0)+normrnd(0,sqrt(Q)); z(1) = x(1)^2/20+normrnd(0,sqrt(R)); for k=2:N x(k) = 0.5*x(k-1)+25*x(k-1)/(1+x(k-1)^2)+ 8*cos(1.2*(k-1))+normrnd(0,sqrt(Q)); z(k) = x(k)^2/20+normrnd(0,sqrt(R)); end Extended Kalman ltering on Example 5: function [xhat,P] = ekf_gss(z,Q,R,m0,P0) %Extended Kalman filter for Gordon, Salmond & Smith example [n,m] = size(z); P = zeros(n,1); xhat = zeros(n,1); % Phi = 0.5+(25-25*m0^2)/(1+m0^2)^2; x1 = 0.5*m0+25*m0/(1+m0^2)+8*cos(1.2*0); H = x1/10; P1 = Phi*P0*Phi+Q; A.5. NONLINEAR FILTERING S = H*P1*H+R; K = P1*H/S; P(1) = (1-K*H)*P1; xhat(1) = x1+K*(z(1)-x1^2/20); for k=2:n Phi = 0.5+(25-25*xhat(k-1)^2)/(1+xhat(k-1)^2)^2; x1 = 0.5*xhat(k-1)+25*xhat(k-1)/(1+xhat(k-1)^2)+8*cos(1.2*(k-1)); H = x1/10; P1 = Phi*P(k-1)*Phi+Q; S = H*P1*H+R; K = P1*H/S; P(k) = (1-K*H)*P1; xhat(k) = x1+K*(z(k)-x1^2/20); end Monte Carlo lter on Example 5: function [xhat,P,samp] = mcfilt_gss(z,Q,R,m0,P0,B) %MC filter filter for Gordon, Salmond & Smith example %Initialization [N,m] = size(z); P = zeros(N,1); xhat = zeros(N,1); samp = zeros(N,1); %k=0 xstar = normrnd(m0,sqrt(P0),B,1); ind = 1:B; for k=1:N %Making prediction distribution x0 = xstar; xmean = mean(x0); xvar = var(x0); lambda=0.2; kon = sqrt((1-lambda^2)*B/(B-1)); x0= xmean+kon*(xstar-xmean); %Sampling x(k) from pred. distr b = unidrnd(B,B,1); x0star = x0(b)+normrnd(0,lambda*sqrt(xvar),B,1); xstar = 0.5.*x0star+25.*x0star./(1+x0star.*x0star)+ 8.*cos(1.2.*(k-1))+normrnd(0,sqrt(Q),B,1); p = normpdf(z(k),xstar.*xstar/20,sqrt(R)); u = rand(B,1); 147 148 APPENDIX A. MATLAB COMMANDS acc = u <= p; samp(k) = B; %Rejection sampling (fast implementation) while(sum(acc)<B) ind2 = ind(acc==0); n_a = length(ind2); samp(k) = samp(k)+n_a; b = unidrnd(B,n_a,1); x0star = xmean+kon*(x0(b)-xmean)+normrnd(0,lambda,n_a,1); xstar(ind2) = 0.5.*x0star+25.*x0star./(1+x0star.*x0star)+ 8.*cos(1.2.*(k-1))+normrnd(0,sqrt(Q),n_a,1); u(ind2) = rand(n_a,1); p = normpdf(z(k),xstar.*xstar/20,sqrt(R)); acc = u <= p; if samp(k)>30000 k sum(acc) end end k samp(k) xhat(k)=mean(xstar); P(k)=var(xstar); end Bibliography Anderson, B. D. O. & Moore, J. B. (1979), Optimal ltering, Electrical Engineering Series, Prentice Hall. Boyce, W. E. & DiPrima, R. C. (1977), Elementary Dierential Equations and Boundary Value Problems, John Wiley & Sons. Brown, R. G. & Hwang, P. Y. C. (1992), Introduction to random signals and applied Kalman ltering, John Wiley. Bucy, R. S. & Senne, K. D. (1971), `Digital synthesis of non-linear lters', Automatica 7, 287{298. Gordon, N., Salmond, D. & Smith, A. F. M. (1993), `Novel approach to nonlinear/nonGaussian Bayesian state estimation', IEE Proceedings-F 140(2), 107{113. Grewal, M. & Andrews, A. (1993), Kalman ltering - Theory and practice, Prentice Hall. Harrison, P. J. & Stevens, C. F. (1976), `Bayesian forecasting', Journal of Royal Statistical Society, Series B 38, 205{247. Harvey, A. C. (1989), Forecasting, structural time series models and the Kalman lter, Cambridge University Press. Kalman, R. E. (1960), `A new approach to linear ltering and prediction problem', ASME Journal of Basic Engineering, series D 82, 34{45. Kitagawa, G. (1987), `Non-Gaussian state-space modeling of non-stationary time series', JASA 82(400), 1032{1041. Mardia, K. V., Kent, J. T. & Bibby, J. M. (1979), Multivariate Analysis, Academic Press. Minkler, G. & Minkler, J. (1992), Theory and application of Kalman ltering, Magellan Book Company. 149 150 BIBLIOGRAPHY Taylor, H. H. & Karlin, S. (1993), An introduction to Stochastic modelling, second edn, Academic Press. West, M. & Harrison, J. (1989), Bayesian forecasting and dynamic models, Springer Series in Statistics, second edn, Springer-Verlag, New York. Index AR process, 30, 33, 47, 48, 75, 76, 78, 118 ARMA model, 77 asymptotically stable, see stability autocorrelation function, 30, 32, 37 autocovariance function, 30 autoregressive moving average process, see ARMA process autoregressive process, see AR process brutto variance, 105 colored process, see noise process, colored conditional density, see density, conditional conditional mean, see mean, conditional conditional variance, see variance, conditional consistency, 105 controllability, see reachability correlation, 18, 19 covariance, 18, 19 cross covariance, 24, 27 matrix, 20, 23, 24 covariation, see dependence cross covariance, see covariance density conditional, 17 joint, 17, 23 marginal, 20, 23 random process, 29 univariate, 15 dependence, 17 deterministic model, see state space model, deterministic 151 dierence equation, 7, 137 dierential equation, 1, 2, 5{7, 137 diuse prior, see noninformative prior direction measurements, 131 discretization, 5, 11, 12, 137 distribution function, see density divergence, see Kalman lter, divergence dynamic linear model, see state space model EM algorithm, 113, 118 equilibrium point, 7, 11 error process, see noise process estimation error, 46, 51, 53 covariance matrix, 46, 51, 53, 54, 66 estimator bias, 105 error, see estimation error linear, 46, 51, 52 non-linear, 119 optimal, 44, 46, 51 optimal, Gaussian, 45 unbiasedness, 32, 43, 44, 106, 117 expectation, see mean extended Kalman lter, see Kalman lter, extended lter adaption, 105 ltering, see Kalman lter xed-interval smoothing, see smoothing xed-lag smoothing, see smoothing xed-point smoothing, see smoothing Gauss-Markov, see AR process 152 Gaussian conditional distribution, 22, 25 conditional mean, 22, 26 conditional variance, 22, 26 joint distribution, 20, 25 properties, 25 random process, see random process, Gaussian univariate distribution, 16 globally asymptotically stable, see stability homogeneous part, 7 independence, 18, 22, 23, 31 information lter, see Kalman lter, information lter information matrix, 82 initial conditions, 39, 41, 81 innovation process, 53, 68 covariance matrix, 53 properties, 53 input function, 2, 6, 8, 66 Kalman lter, 2, 51, 52, 54, 96 arbitrary gain, 85 asymptotic stability, 80 covariance lter, 55 divergence, 79, 90 extended, 119, 122 Gaussian conditions, 54 information lter, 82, 89 iterated extended, 123 likelihood calculation, 110 linear estimator, 52 linearized, 119, 121 non-Gaussian noise, 64 non-zero input, 66 optimal estimation, 51 prediction, see prediction square root lter, 83 steady state, 81 Kalman gain, 54, 81, 83, 85 INDEX likelihood function, 106, 108{110 estimated, 115 linear dynamic model, see state space model linear growth, 62 linearized Kalman lter, see Kalman lter, linearized MA process, 33, 77 matrix inversion lemma, 82, 88 maximum likelihood, 106, 108, 110, 113, 118 mean, 15, 17, 19 conditional, 18, 44, 52 vector, 20, 23, 24 measurement deviation, 121 missing observations, 40, 94 modeling errors, 84 Monte Carlo lter, 123 moving averange process, see MA process moving body, 1, 2, 5, 7, 10, 40, 42, 60, 80, 87, 94, 102, 141 multivariate density, see density, joint navigation, 61, 63 noise process, 37, 38 colored, 30 white, 30, 37, 38, 60 white Gaussian, 30 noninformative prior, 40, 83 normal, see Gaussian observability, 8, 10, 12, 78, 80, 81, 103, 138 observation vector, 38 parameter estimaton, see lter adaption pendulum movement, 11 prediction, 53, 94, 101 prediction error, 94 covariance matrix, 94, 101 probability density, see density INDEX random process, 28 density, see density, random process Gaussian, 30 stationary, 22, 29, 30 weakly stationary, 29 random variable, 15 random walk model, 35, 35, 59, 98, 108, 112, 138 reachability, 8, 10, 12, 103, 137 stochastic, 78, 80 regression Bayesian, 72 linear, 59, 71, 117 multiple linear, 73 regressor, see mean, conditional rejection sampling, 124, 124 sampling time, 6 sampling/importance resampling, 124 serial correlation, see noise process, colored signal to noise ratio, 67 smoothing xed-interval, 93, 96, 102, 104, 114 xed-lag, 94, 97, 102 xed-point, 93, 95, 102 stability, 7, 7, 11, 13, 81 standard deviation, 16 state estimation, 2, 51 state space form, see state space model state space model, 13 continuous, 5, 6 deterministic, 5, 6, 7 discrete, 7, 35, 36, 38 misspecication, 86 non-linear, 120 stochastic, 35, 36, 38 time-invariant, 7, 38, 80, 81 state vector, 2, 5, 38 deviation, 121 stationary, see random process 153 steady state Kalman lter, see Kalman lter,steady state time-invariant models, see state space model, time-invariant transition matrix, 6 unbiasedness, see estimator, unbiasedness uncorrelatedness, 24 variance, 15, 17, 19 conditional, 18 white noise, see noise process,white

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement