The matrix K in (1.7) is chosen to be the gain or blending factor that minimizes the a posteriori error covariance (1.6). This minimization can be accomplished by first substituting (1.7) into the above definition for , substituting that into (1.6), performing the indicated expectations, taking the derivative of the trace of the result with respect to K, setting that result equal to zero, and then solving for K. For more details see [Maybeck79], [Brown92], or [Jacobs93]. One form of the resulting K that minimizes (1.6) is given by

The Kalman filter estimates a process by using a form of feedback control: the filter estimates the process state at some time and then obtains feedback in the form of (noisy) measurements. As such, the equations for the Kalman filter fall into two groups: time update equations and measurement update equations. The time update equations are responsible for projecting forward (in time) the current state and error covariance estimates to obtain the a priori estimates for the next time step. The measurement update equations are responsible for the feedback¯i.e. for incorporating a new measurement into the a priori estimate to obtain an improved a posteriori estimate.

The time update equations can also be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. Indeed the final estimation algorithm resembles that of a predictor-corrector algorithm for solving numerical problems as shown below in Figure 1-1.

The specific equations for the time and measurement updates are presented below in Table 1-1 and Table 1-2.Figure 1-1.The ongoing discrete Kalman filter cycle. The time update projects the current state estimate ahead in time. The measurement update adjusts the projected estimate by an actual measurement at that time. Notice the resemblance to a predictor- corrector algorithm.

Again notice how the time update equations in Table 1-1 project the state and covariance estimates from time step k to step k+1. and B are from (1.1), while is from (1.3). Initial conditions for the filter are discussed in the earlier references.

The first task during the measurement update is to compute the Kalman gain, . Notice that the equation given here as (1.11) is the same as (1.8). The next step is to actually measure the process to obtain , and then to generate an a posteriori state estimate by incorporating the measurement as in (1.12). Again (1.12) is simply (1.7) repeated here for completeness. The final step is to obtain an a posteriori error covariance estimate via (1.13).

After each time and measurement update pair, the process is repeated with the previous a posteriori estimates used to project or predict the new a priori estimates. This recursive nature is one of the very appealing features of the Kalman filter¯it makes practical implementations much more feasible than (for example) an implementation of a Weiner filter [Brown92] which is designed to operate on all of the data directly for each estimate. The Kalman filter instead recursively conditions the current estimate on all of the past measurements. Figure 1-2 below offers a complete picture of the operation of the filter, combining the high-level diagram of Figure 1-1 with the equations from Table 1-1 and Table 1-2.

In the case of , often times the choice is less deterministic. For example, this noise source is often used to represent the uncertainty in the process model (1.1). Sometimes a very poor model can be used simply by "injecting" enough uncertainty via the selection of . Certainly in this case one would hope that the measurements of the process would be reliable.

In closing I would point out that under conditions where and .are constant, both the estimation error covariance and the Kalman gain will stabilize quickly and then remain constant (see the filter update equations in Figure 1-2). If this is the case, these parameters can be pre-computed by either running the filter off-line, or for example by solving (1.10) for the steady-state value of by defining and solving for .Figure 1-2.A complete picture of the operation of the Kalman filter, combining the high-level diagram of Figure 1-1 with the equations from Table 1-1 and Table 1-2.

In something akin to a Taylor series, we can linearize the estimation around the current estimate using the partial derivatives of the process and measurements functions to compute estimates even in the face of non-linear relationships. To do so, we must begin by modifying some of the material presented in section 1. Let us assume that our process again has a state vector , but that the process is now governed by the non-linear difference equation

In this case the non-linear function f(·) in the difference equation (2.1) relates the state at time step k to the state at step k+1. It includes as parameters any driving function uk and the process noise wk. The non-linear function h(·) in the measurement equation (2.2) now relates the state to the measurement zk.

- and are the projected state and measurement vectors from (2.1) and (2.2),
- is an a posteriori estimate of the state at step k,
- the random variables and represent the process and measurement noise as in (1.3) and (1.4).
- A is the Jacobian matrix of partial derivatives of f(·) with respect to x, that is
### (2.5)

### ,

- W is the Jacobian matrix of partial derivatives of f(·) with respect to w,
### (2.6)

### ,

- H is the Jacobian matrix of partial derivatives of h(·) with respect to x,
### (2.7)

### ,

- V is the Jacobian matrix of partial derivatives of h(·) with respect to v,
### (2.8)

### .

Notice that the equations (2.11) and (2.12) are linear, and that they closely resemble the difference and measurement equations (1.1) and (1.1) from the discrete Kalman filter. This motivates us to use the measured residual in (2.10) and second (hypothetical) Kalman filter to estimate the prediction error given by (2.11). This estimate, call it , could then be used along with (2.9) to obtain the a posteriori state estimates for the non-linear process as

As with the basic discrete Kalman filter, the time update equations in Table 2-1 project the state and covariance estimates from time step k to step k+1. Again f(·) in (2.16) comes from (2.1), and W are the Jacobians (2.5) and (2.6) at step k, and is the process noise covariance (1.3) at step k.

As with the basic discrete Kalman filter, the measurement update equations in Table 2-2 correct the state and covariance estimates with the measurement . Again h(·) in (2.19) comes from (2.2), and V are the Jacobians (2.7) and (2.8) at step k, and is the measurement noise covariance (1.4) at step k.

The basic operation of the EKF is the same as the linear discrete Kalman filter as shown in Figure 1-1. Figure 2-1 below offers a complete picture of the operation of the EKF, combining the high-level diagram of Figure 1-1 with the equations from Table 2-1 and Table 2-2.

An important feature of the EKF, and indeed the key to the one-step-at-a-time approach, is that the Jacobian in the equation for the Kalman gain serves to correctly propagate or "magnify" only the relevant component of the measurement information. For example, if there is not a one-to-one mapping between the measurement and the state via h(·), the Jacobian affects the Kalman gain so that it only magnifies the portion of the residual that does affect the state. Of course if for all measurements there is not a one-to-one mapping between the measurement and the state via h(·), then as you might expect the filter will quickly diverge. The control theory term to describe this situation is unobservable.Figure 2-1.A complete picture of the operation of the extended Kalman filter, combining the high-level diagram of Figure 1-1 with the equations from Table 2-1 and Table 2-2.

In this example, our process is governed by the linear difference equation

Similarly we need to choose an initial value for , i.e. . If we were absolutely certain that our initial state estimate was correct, we would let . However given the uncertainty in our initial estimate , choosing would cause the filter to initially and always believe . As it turns out, the alternative choice is not critical. We could choose almost any and the filter would eventually converge. We'll start our filter with .

In the first simulation I fixed the measurement variance at . Because this is the "true" measurement error variance, we would expect the "best" performance in terms of balancing responsiveness and estimate variance. This will become more evident in the second and third simulation. Figure 3-1 depicts the results of this first simulation. The true value of the random constant is given by the solid line, the noisy measurements by the cross marks, and the filter estimate by the remaining curve.

When considering the choice for above, I mentioned that the choice was not critical as long as because the filter would eventually converge. Below in Figure 3-2 I have plotted the value of versus the iteration. By the 50th iteration, it has settled from the initial (rough) choice of 1 to approximately 0.0002 (Volts2).Figure 3-1.The first simulation:. The true value of the random constant is given by the solid line, the noisy measurements by the cross marks, and the filter estimate by the remaining curve.

In section 1 under the topic "Filter Parameters and Tuning" I briefly discussed changing or "tuning" the parameters Q and R to obtain different filter performance. In Figure 3-3 and Figure 3-4 below we can see what happens when R is increased or decreased by a factor of 100 respectively. In Figure 3-3 the filter was told that the measurement variance was 100 times greater (i.e. ) so it was "slower" to believe the measurements.Figure 3-2.After 50 iterations, our initial (rough) error covariance choice of 1 has settled to about 0.0002 (Volts2).

In Figure 3-4 the filter was told that the measurement variance was 100 times smaller (i.e. ) so it was very "quick" to believe the noisy measurements.Figure 3-3.Second simulation: . The filter is slower to respond to the measurements, resulting in reduced estimate variance.

While the estimation of a constant is relatively straight-forward, it clearly demonstrates the workings of the Kalman filter. In Figure 3-3 in particular the Kalman "filtering" is evident as the estimate appears considerably smoother than the noisy measurements.Figure 3-4.Third simulation: . The filter responds to measurements quickly, increasing the estimate variance.

Brown92 Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random Signals and Applied Kalman Filtering, Second Edition, John Wiley & Sons, Inc.

Gelb74 Gelb, A. 1974. Applied Optimal Estimation, MIT Press, Cambridge, MA.

Jacobs93 Jacobs, O. L. R. 1993. Introduction to Control Theory, 2nd Edition. Oxford University Press.

Julier Julier, Simon and Jeffrey Uhlman. "A General Method of Approximating Nonlinear Transformations of Probability Distributions," Robotics Research Group, Department of Engineering Science, University of Oxford [cited 14 November 1995]. Available from http://phoebe.robots.ox.ac.uk/ reports/New_Fltr.zip (88K).

Kalman60 Kalman, R. E. 1960. "A New Approach to Linear Filtering and Prediction Problems," Transaction of the ASME¯Journal of Basic Engineering, pp. 35-45 (March 1960).

Lewis86 Lewis, Richard. 1986. Optimal Estimation with an Introduction to Stochastic Control Theory, John Wiley & Sons, Inc.

Maybeck79 Maybeck, Peter S. 1979. Stochastic Models, Estimation, and Control, Volume 1, Academic Press, Inc.

Sorenson70 Sorenson, H. W. 1970. "Least-Squares estimation: from Gauss to Kalman," IEEE Spectrum, vol. 7, pp. 63-68, July 1970.