A Study of Covariances within Basic and Extended Kalman Filters

Transcription

1 A Study of Covariances within Basic and Extended Kalman Filters David Wheeler Kyle Ingersoll December 2, 2013 Abstract This paper explores the role of covariance in the context of Kalman filters. The underlying principles of both the basic and extended Kalman filter are discussed and the equations used to implement these filters are given. Both a linear and non-linear estimation scenario is presented. Details for modelling these scenarios and implementing a basic and extended Kalman filter in MATLAB are outlined. A basic Monte-Carlo simulation is performed for each scenario to model how the robot would move. It is shown that in the absence of measurements both the analytical covariance (Kalman equations only) and experimental covariance (Monte-Carlo simulation results) increases linearly with time. It is further shown that these two covariances match each other closely for both the linear and non-linear cases. It is also shown that the analytical covariance of the Kalman filter can converge in the linear scenario under certain conditions. Results of our simulations are plotted and interpreted. I. INTRODUCTION The Kalman filter is a popular algorithm used to estimate the current state of a given system by optimally weighting the information from the model of the system with any available measurements. Theoretically there are infinite ways to estimate the state of the system using these two pieces of information; however, the Kalman filter is unique because it minimizes the mean square error of the estimate for linear problems. Methods have been developed to extend the Kalman filter to non-linear problems by locally linearizing the problem; this modification is known as the extended Kalman filter (EKF). The Kalman filter can be broken down into two basic steps. The first step is referred to as the predict step, in which the algorithm makes a prediction of the systems current state using only information provided by the model during the previous step. This is followed by the update step, in which the algorithm uses any available measurements to modify the a priori prediction. The algorithm calculates the Kalman gain (K k ) which determines how much weight is given to the model versus how much weight is given to the measurement. All of the notation and equations used throughout this paper comes from [1]. Central to determining K k are P k and R k. P k is an estimate the covariance of the error of the state of the system at time step k and is calculated during each iteration of the algorithm; R k is the covariance of the measurement noise and is often treated as a constant R. K k then becomes a ratio of certainties: how certain we are in the estimate of the current state of our system versus how certain we are of our measurements. This paper aims to: 1) Explore how P k varies over time. 2) Explore how P k compares with an empirically derived value of the system s covariance. Throughout this paper, we will refer to P k as the analytical covariance. In order to understand how well this theoretical estimate models the underlying process, we conducted a Monte Carlo simulation of the process, performing many runs through the same Kalman filter scenario; each run uses random noise values taken from the appropriate normal distributions. The mean of these simulated runs corresponds to the estimated state of the system, ˆx k, and the covariance of the simulated states corresponds to the analytical covariance, P k. Throughout this paper, we will refer to the covariance of the simulated data as the empirical or experimental covariance. A. Notation II. BACKGROUND The Kalman filter is designed to work for the problems that can be formulated as the following linear finite difference equation x k = Ax k 1 + Bu k 1 + w k 1 where w k represents zero mean, normally distributed process noise with covariance Q. Measurements providing information about the state are represented by z k = Hx k + ν k where ν k represents zero mean, normally distributed measurement noise with covariance R. The Kalman filter consists of two, distinct steps: a predict step and an update step. Continuing from the previously defined notation and as defined in [1] the predict step is given by ˆx k = Aˆx k 1 + Bu k 1 (1) P k = AP k 1 A T + Q (2) where the superscript denotes the a priori prediction. The update step is given by B. Predict Step Derivation K k = P k H T (HP k H T + R) 1 (3) x k = x k + K k (z k Hx k ) (4) P k = (I K k H)P k (5) A full derivation of the Kalman filter will not be given here. However, the derivation of the predict step, Eqn. 1 and Eqn. 2, are given below. In the absence of additional information the expected value is the optimal estimator (in the MSE sense). Hence, ˆx k = E[x k ] = E[Ax k 1 + Bu k 1 + w k 1 ] = AE[x k 1 ] + BE[u k 1 ] + E[w k 1 ] = Aˆx k 1 + Bu k 1 If we define e to be error of our a posteriori estimate and e to be the error of our a priori estimate then it follows that and e = x k ˆx k e = x k ˆx k = A(x k 1 ˆx k 1 ) + B(u k 1 u k 1 ) + w k 1 = Ae k 1 + w k 1 1

2 2 With the assumption that the process noise w k is independent of the state x k, it follows that the a priori covariance estimate P k should be P k = E[e e T ] C. Experimental Setup = E[(Ae k 1 + w k 1 )(Ae k 1 + w k 1 ) T ] = E[Ae k 1 e T k 1A T + w k 1 w T k 1] = AP k 1 A T + Q To explore these aspects of the Kalman filter, we designed two basic estimation scenarios, one using a basic (i.e. linear) Kalman filter and one using the EKF. The basic Kalman filter scenario represents a lost robot situation; there is some uncertainty in the initial position of the robot but as more measurements are taken we become increasingly more certain of its global position. The EKF scenario is fundamentally different, in that we know the exact initial position of the robot, but become more uncertain of its position over time in the absence of measurements. D. Basic Kalman Filter Scenario The following scenario was used in implementing a basic Kalman filter. A robot is said to move in the X Y plane with the following possible input controls xstep u k = y step where x step and y step both have a value of 1 for each k. Thus, we are telling the robot to travel in a linear path, moving one positive unit in the x and y directions during each time step. The state of the robot can be expressed as x x k = y or in other words by its global x and y coordinates. The robot motion can be modeled by Eqn. 1 where A, B, and w k are 1 0 A = 1 0 B = p(w k ) = N(0, Q) where Q is the variance of the process noise of the system. In each simulation run, the true path of the robot was constructed by moving it forward one unit in both the x and y directions and then by adding white noise w k to each step. The following scheme was used to make simulated measurements. At each step along the true path, a measurement is created by adding white noise v k : p(v k ) = N(0, R) to the true position. This measurement scheme is similar to that of GPS measurements. Because the measurements are global, the H matrix is a 2x2 identity matrix. Finally, the P matrix is seeded as P 0 = Q indicating that there is some uncertainty in the initial position of the robot. Two scenarios were explored through the simulation. The first simulation scenario was run with the predict step only using Eqns. 1 and 2. After each time step, the analytical covariance given by the P matrix and the simulated covariance of the data points were plotted. The second simulation scenario was run with both the predict and update steps. The update step is calculated with Eqns. 3, 4, and 5. Due to the Gaussian noise assumption made in the derivation of the Kalman filter, the distribution of the filter s output is given by N(x k, P k ). E. Extended Kalman Filter Scenario The similar but more sophisticated scenario is used in implementing an Extended Kalman filter. A robot can move in the X Y plane with the following possible input controls Df u = θ where D f represents the distance that the robot moves forward and θ represents the change in heading angle (with positive clockwise rotations). The state of the robot can be expressed as x = x loc y loc θ where x loc and y loc are the coordinates of the vehicle in the X Y plane and θ is the robot heading relative to East (+x). The robot motion can be modelled with the following difference equation x[k] = f(x[k 1], u[k 1], w[k 1]) where w[k] represents the process noise and is also zero mean with covariance Q. The non-linear function f is approximated by f x = x + D f cos(θ + θ/2) y + D f sin(θ + θ/2) θ + δθ The Jacobian of f x w.r.t the state x (used to determine the Kalman gain in an EKF) is 1 0 D f sin(θ + θ/2) A = fx x = D f cos(θ + θ/2) 0 The Jacobian of f x w.r.t the input u (also used to determine the Kalman gain in an EFK) is cos(θ + θ/2) D f /2 sin(θ + θ/2) B = fx u = sin(θ + θ/2) D f /2 cos(θ + θ/2) A beacon location is defined in the X Y plane as xb B xy = allowing for a distance measurement at timestep k defined by z k = [x loc,k x b ] 2 + [y loc,k y b ] 2 + ν k where ν k represents the measurement noise. A series of N inputs are provided to the system, creating a theoretical path (in the absence of process noise Q). With the specified model noise added to the system, the true robot motion can be modelled. At the final timestep, N, a distance measurement is received and the EKF is updated. As with the basic Kalman Filter scenario we explore how P k varies with time and how it compares with an experimentally determined covariance. This scenario uses ideas presented in [2]. y b III. NUMERICAL RESULTS A. Basic Kalman Filter: Predict Step Only The basic Kalman filter running the predict step only was run through 10 time steps. A large process noise Q = 0.01I was used in this simulation in order to better show how the errors from the process uncertainty evolve over time. Figure 1 shows the results of this simulation. We see that the covariance of the filter output grows in a linear manner when only the predict step is run. The propagation of the error, step after step, increases our uncertainty as to the exact

3 3 position of the robot. Figure 2 plots the empirical covariance in red and the analytical covariance in blue for each time step. We see that the empirical and analytical covariances match closely and that they grow linearly over time. Thus, a Kalman filter without the update step does not force the covariance to converge, rather, the covariance will continue to grow as time progresses. Fig. 3. [Zoomed-in view of Figure 1] The filter s outputs are plotted here along with visual representations of the analytical (red, solid line) and empirical covariances (cyan dots). Although the analytical and empirical covariances have slightly different values and although their means are shifted, this figure clearly shows that covariance calculated by the Kalman filter closely matches the covariance obtained through our simulation. Fig. 1. The outputs of the predict step of a basic Kalman filter. The range of the distribution grows with each time step indicating increasing uncertainty in the robot s global position. B. Basic Kalman Filter: Predict and Update Steps After exploring the predict step, measurements are fed into the basic Kalman filter and the update step was performed. A more realistic process noise Q = 10 6 I was used. If the process noise Q is too large, the Kalman filter looses too much information each time step and will ultimately diverge. R was set to be [0.01]. Figure 4 shows the output of the basic Kalman filter over 500 runs and ten time steps. This figure is notable because it shows that the covariance of the filter output reaches a steady-state value after a small number of time steps. Indeed, the filter output appears to become more tightly distributed about the mean value after each time step. The large pink ellipse surrounding the first time step reflects the fact that we seeded the P k matrix with a large value (P 0 = I) compared to the Q and R values of the simulation. In terms of our simulation, this means that we become increasingly more certain of the robot s true global position with each additional time step, even though we were uncertain of its initial position. Fig. 2. The experimental (red) and analytical (blue) estimates of the covariance of the Kalman filter s outputs. This plot shows that the covariance grows linearly over time. Figure 3 shows a closer view of a single time step in this simulation. This closer view better shows individual filter outputs (dots) along with the visual representations of the analytical and empirical covariances. The covariances are represented by ellipses whose major and minor radii are the square root of the eigenvalues, i.e. the ellipses are 1 σ confidence intervals on the state of the system. This figure shows that the empirical and analytical covariances match very closely, though not exactly. The empirical covariance ellipses uses the empirical means as their centers whereas the analytical covariances use the perfect path points (i.e. (1, 1), (2, 2), etc.) as the centers of its covariance ellipses. Thus we see that the empirical and analytical covariances have slightly different radii and are slightly shifted with respect to each other. Despite the small differences, we can see that the covariance calculated by the Kalman filter algorithm is very similar to the empirical covariance produced by our simulations. Fig. 4. The outputs of a fully-implemented basic Kalman filter. Notice that the covariance of the filter s outputs appears to decrease over time, i.e. we become more and more certain of the robot s true position.

4 4 Figure 5 plots the analytical and empirical covariances of this second simulation. This figure shows that the covariances quickly converge to a steady-state value. In this simulation, both the analytical and empirical covariances had almost reached their steady-state value after only ten time steps. While Figure 5 shows convergence of the covariance to a steady-state value, a second kind of convergence is present; Figure 6 shows that the analytical and empirical covariances quickly converge to the same value, even when the algorithm is seeded with a large P 0 as was the case in our simulation. In this lost robot scenario, we see that the Kalman filter quickly maximizes our certainty of the robot s position, even if we were uncertain of the robot s initial position. Figure 7 provides a closer view of one of the Fig. 7. [Zoomed-in view of figure 4]The filter s outputs are plotted here along with visual representations of the analytical (pink, solid line) and experimental covariances (green dots). The covariances are represented by ellipse whose major and minor radii are the square root of the eigenvalues, i.e. the ellipses are 1 σ confidence intervals on the state of the system. This figure shows that the analytical and empirical covariances are in agreement. Fig. 5. The experimental (red) and analytical (blue) estimates of the covariance of the Kalman filter s outputs. This plot shows that the covariance of the error of the Kalman filter s outputs quickly reaches a steady-state value. noise γ k : p(γ k ) = Γ k = N(0, 0.05 [diag(u k )]), implying each input has a 5% uncertainty. Figure 8 shows the results of the EKF simulation. At each time step 500 points are plotted, each indicting a possible state given the input noise. As before, the covariance of this experimental data is plotted along side the analytical covariance for comparison purposes. Similar to the basic Kalman Filter scenario, in the absence of any measurements (and therefore update steps) the covariance grows with each time step. Figure 9 helps illustrate Fig. 6. The absolute value of the difference between the analytical and empirical covariances of the output of the basic Kalman filter. This simulation was run over 20 time steps. This plot shows that the analytical and empirical covariances quickly converge to the same value independent of the seeding of P 0. Kalman filter s time steps. The analytical and empirical covariances are also plotted. Although the covariances do not agree exactly and although their centers are shifted, this figure still shows that the analytical covariance calculated by the Kalman filter algorithm matches very closely the covariance that would be seen in a Monte Carlo simulation. C. Extended Kalman Filter For the EKF scenario we propagated the filter through 30 timesteps with 500 iterations in our Monte Carlo simulation. We defined the input to be u k = [0.02, ] T such that the robot travels 3/8 of the way around a circle. We initialized our filter such that x 0 = [0, 0, 0] T and P 0 = [0], implying 100% confidence that our initial position is at the origin. Rather than implicitly defining Q, we applied an input Fig. 8. Results of the EKF scenario. The possible states at each of the 30 time steps are represented by the alternatively colored clusters. The cyan ellipses represent the covariance at each state. The red circle at the top indicates the location of the beacon. the type of information that a beacon distance measurement provides at the final time step. Seven potential measurements are visualized by the green lines (distance between beacon and simulated robot location at time step N). Each measurement has additive Gaussian noise associated with it, represented by the red x s. Figure 10 shows the comparison between the analytical and experimental covariance prior to the update (P k ) as well as the analytical and experimental covariance after the update step (P k ). It can clearly be seen that both the a priori and a posteriori analytical covariance determined by the

5 5 Fig. 9. Measurement Illustration. As before, the experimental state of the filter (blue dots), analytical and experimental covariances (cyan, thin and thick), and the beacon location (red circle) are plotted. Seven potential measurements are visualized by the green lines. Each measurement has additive Gaussian noise associated with it, represented by the red x s. EKF do an excellent job at estimating the underlying covariances. As expected, the update step reduces the uncertainty in the direction of the measurement while not adjusting the uncertainty in the direction perpendicular to the measurement. predicts what the covariance will be over many simulated runs. Fourth, the analytical and empirical covariances will quickly converge to the same value if P k is seeded as a large value. In terms of our lost robot and wandering robot scenarios, we can draw the following conclusions. First, by taking into account measurement information, a Kalman filter can quickly hone in on the system s true state (within a degree of uncertainty) even if the system s initial state is unknown. Second, as an unmeasured system evolves over time, the uncertainty surrounding its current state will continue to increase. However, if a measurement is taken at some arbitrary time in the future, the Kalman filter can use that single measurement to significantly reduce the covariance of the state estimate. This project could be extended in several different ways. One such direction would be to study how changing the R value (the covariance of the measurement noise) affects the accuracy of the filter outputs. If modifying the R value increases the accuracy of the filter, we could research different methods of dynamically assigning R values depending on the environment of our system. For example, if an unmanned air vehicle (UAV) carried an array of sensors, it might have limited information on how well each sensor performs in different environments. If the UAV enters an environment for which it does not have information about how one of its sensors should perform, it could compare that sensor s output to the output of the Kalman filter (which would utilize all of the other sensors measurements) and dynamically adjust that sensor s R value depending on how closely its measurement matched the output of the Kalman filter. A thorough understanding of the underlying stochastic properties of the Kalman filter, as outlined in this paper, is critical for exploration in this exciting field. REFERENCES [1] G. Welch and G. Bishop. (2006, July 24). An Introduction to the Kalman Filter [Online] Available: welch/media/pdf/kalman_intro.pdf [2] E. Kiriy and M. Buehler. (2002, April 12). Three-state Extended Kalman Filter for Mobile Robot Localization [Online] Available: filtering/ekf-3state.pdf Fig. 10. The a posteriori covariances (analytical and experimental) of the final time step are plotted in magenta (thin and thick). As before the a priori covariances (analytical and experimental) of the final time step are plotted in cyan (thin and thick). IV. CONCLUSIONS We were able to make the following observations and conclusions as a result of our research. First, during the predict step, the propagation of process noise results in linearly growing covariances and increasingly larger uncertainties about the current state of the system. Second, under certain conditions a Kalman filter will cause the covariance to converge to a steady-state value 1. Third, the analytical covariance calculated by the Kalman filter algorithm accurately 1 The error distribution outlined in our scenarios provide an example of a convergent case. If the process noise is too great, or the measurement do not provide enough information, the solution will diverge. Defining the limits for convergence is outside the scope of this paper