Practical issues and extensions

The basic ideas of Kalman filtering are straightforward, but the filter equations rely heavily on matrix algebra. Listing 2 shows the Kalman filter update equations in C. The matrix algebra listings referenced in Listing 2 can be found at ftp://ftp.embedded.com/pub/2001/simon06.

Listing 2: Kalman filter equations

// The following code snippet assumes that the linear system has n states, m
// inputs, and r outputs. Therefore, the following variables are assumed to
// already be defined.
// A is an n by n matrix
// B is an n by m matrix
// C is an r by n matrix
// xhat is an n by 1 vector
// y is an r by 1 vector
// u is an m by 1 vector
// Sz is an r by r matrix
// Sw is an n by n matrix
// P is an n by n matrix

float AP[n][n];	// This is the matrix A*P
float CT[n][r];	// This is the matrix CT
float APCT[n][r];	// This is the matrix A*P*CT
float CP[r][n];	// This is the matrix C*P
float CPCT[r][r];	// This is the matrix C*P*CT
float CPCTSz[r][r];	// This is the matrix C*P*CT+Sz
float CPCTSzInv[r][r];	// This is the matrix (C*P*CT+Sz)-1
float K[n][r];	// This is the Kalman gain.
float Cxhat[r][1];	// This is the vector C*xhat
float yCxhat[r][1];	// This is the vector y-C*xhat
float KyCxhat[n][1];	// This is the vector K*(y-C*xhat)
float Axhat[n][1];	// This is the vector A*xhat
float Bu[n][1];	// This is the vector B*u
float AxhatBu[n][1];	// This is the vector A*xhat+B*u
float AT[n][n];	// This is the matrix AT
float APAT[n][n];	// This is the matrix A*P*AT
float APATSw[n][n];	// This is the matrix A*P*AT+Sw
float CPAT[r][n];	// This is the matrix C*P*AT
float SzInv[r][r];	// This is the matrix Sz-1
float APCTSzInv[n][r];	// This is the matrix A*P*CT*Sz-1
float APCTSzInvCPAT[n][n];	// This is the matrix A*P*CT*Sz-1*C*P*AT

// The following sequence of function calls computes the K matrix.
MatrixMultiply((float*)A, (float*)P, n, n, n, (float*)AP);
MatrixTranspose((float*)C, r, n, (float*)CT);
MatrixMultiply((float*)AP, (float*)CT, n, n, r, (float*)APCT);
MatrixMultiply((float*)C, (float*)P, r, n, n, (float*)CP);
MatrixMultiply((float*)CP, (float*)CT, r, n, r, (float*)CPCT);
MatrixAddition((float*)CPCT, (float*)Sz, r, r, (float*)CPCTSz);
MatrixInversion((float*)CPCTSz, r, (float*)CPCTSzInv);
MatrixMultiply((float*)APCT, (float*)CPCTSzInv, n, r, r, (float*)K);

// The following sequence of function calls updates the xhat vector.
MatrixMultiply((float*)C, (float*)xhat, r, n, 1, (float*)Cxhat);
MatrixSubtraction((float*)y, (float*)Cxhat, r, 1, (float*)yCxhat);
MatrixMultiply((float*)K, (float*)yCxhat, n, r, 1, (float*)KyCxhat);
MatrixMultiply((float*)A, (float*)xhat, n, n, 1, (float*)Axhat);
MatrixMultiply((float*)B, (float*)u, n, r, 1, (float*)Bu);
MatrixAddition((float*)Axhat, (float*)Bu, n, 1, (float*)AxhatBu);
MatrixAddition((float*)AxhatBu, (float*)KyCxhat, n, 1, (float*)xhat);

// The following sequence of function calls updates the P matrix.
MatrixTranspose((float*)A, n, n, (float*)AT);
MatrixMultiply((float*)AP, (float*)AT, n, n, n, (float*)APAT);
MatrixAddition((float*)APAT, (float*)Sw, n, n, (float*)APATSw);
MatrixTranspose((float*)APCT, n, r, (float*)CPAT);
MatrixInversion((float*)Sz, r, (float*)SzInv);
MatrixMultiply((float*)APCT, (float*)SzInv, n, r, r, (float*)APCTSzInv);
MatrixMultiply((float*)APCTSzInv, (float*)CPAT, n, r, n, (float*)APCTSzInvCPAT);
MatrixSubtraction((float*)APATSw, (float*)APCTSzInvCPAT, n, n, (float*)P);

These listings are very general and, if the problem is small enough, could probably be simplified considerably. For example, the inverse of the 2-by-2 matrix:

is equal to:

So if you need to invert a 2-by-2 matrix you can use the above equation. Some additional C code for matrix manipulation and Kalman filtering can be found at http://wad.www.media.mit.edu/people/wad/mas864/proj_src.html.

Systems with more than three states could exceed your budget for program size and computational effort. The computational effort associated with matrix inversion is proportional to n³ (where n is the size of the matrix). This means that if the number of states in the Kalman filter doubles, the computational effort increases by a factor of eight. It's not too hard to see how you could run out of throughput pretty quickly for a moderately sized Kalman filter. But never fear! The so-called "steady state Kalman filter" can greatly reduce the computational expense while still giving good estimation performance. In the steady state Kalman filter the matrices K_k and P_k are constant, so they can be hard-coded as constants, and the only Kalman filter equation that needs to be implemented in real time is the equation, which consists of simple multiplies and addition steps (or multiply and accumulates if you're using a DSP).

We have discussed state estimation for linear systems. But what if we want to estimate the states of a nonlinear system? As a matter of fact, almost all real engineering processes are nonlinear. Some can be approximated by linear systems but some cannot. This was recognized early in the history of Kalman filters and led to the development of the "extended Kalman filter," which is simply an extension of linear Kalman filter theory to nonlinear systems.

Up to this point we have talked about estimating the state one step at a time as we obtain measurements. But what if we want to estimate the state as a function of time after we already have the entire time-history of measurements? For example, what if we want to reconstruct the trajectory of our vehicle after the fact? Then it seems that we could do better than the Kalman filter because to estimate the state at time k we could use measurements not only up to and including time k, but also after time k. The Kalman filter can be modified for this problem to obtain the so-called Kalman smoother.

The Kalman filter not only works well but is theoretically attractive. It can be shown that the Kalman filter minimizes the variance of the estimation error. But what if we have a problem where we are more concerned with the worst case estimation error? What if we want to minimize the "worst" estimation error rather than the "average" estimation error? This problem is solved by the H_∞ filter. The H_∞ filter (pronounced "H infinity" and sometimes written as H∞ ) is an alternative to Kalman filtering that was developed in the 1980s. It is less widely known and less commonly applied than the Kalman filter, but it has advantages that make it more effective in certain situations.

Kalman filter theory assumes that the process noise w and the measurement noise z are independent from each other. What if we have a system where these two noise processes are not independent? This is the correlated noise problem, and the Kalman filter can be modified to handle this case. In addition, the Kalman filter requires that the noise covariances S_w and S_z be known. What if they are not known? Then how can we best estimate the state? Again, this is the problem solved by the H_∞ filter.

Kalman filtering is a huge field whose depths we cannot hope to begin to plumb in these few pages. Thousands of papers and dozens of textbooks have been written on this subject since its inception in 1960.

Historical perspective

The Kalman filter was developed by Rudolph Kalman, although Peter Swerling developed a very similar algorithm in 1958. The filter is named after Kalman because he published his results in a more prestigious journal and his work was more general and complete. Sometimes the filter is referred to as the Kalman-Bucy filter because of Richard Bucy's early work on the topic, conducted jointly with Kalman.

The roots of the algorithm can be traced all the way back to the 18-year-old Karl Gauss's method of least squares in 1795. Like many new technologies, the Kalman filter was developed to solve a specific problem, in this case, spacecraft navigation for the Apollo space program. Since then, the Kalman filter has found applications in hundreds of diverse areas, including all forms of navigation (aerospace, land, and marine), nuclear power plant instrumentation, demographic modeling, manufacturing, the detection of underground radioactivity, and fuzzy logic and neural network training.

Dan Simon is a professor in the electrical and computer engineering department at Cleveland State University and a consultant to industry. His teaching and research interests include filtering, control theory, embedded systems, fuzzy logic, and neural networks. He is presently trying to implement a DSP-based motor controller using a Kalman filter. You can contact him at d.j.simon@csuohio.edu.

For further reading

Gelb, A. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1974. This is what you call an "oldie but goodie." And don't worry that it's published by MIT Press; it's a simple and straightforward book that starts with the basics and is heavy on practical issues.

Anderson, B. and J. Moore. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall, 1979. This is very mathematical and difficult to read, but I have relied heavily on it for obtaining a fundamental theoretical understanding of Kalman filtering and related issues.

Grewal, M. and A. Andrews. Kalman Filtering Theory and Practice. Englewood Cliffs, NJ: Prentice-Hall, 1993. This is a happy medium between the first two references, a nice balance between theory and practice. One good feature of this book is that it includes Kalman filtering source code on a floppy disk. One not-so-nice feature is that the source code is written in Fortran.

Sorenson, H. Kalman Filtering: Theory and Application. Los Alamitos, CA: IEEE Press, 1985. This is a collection of some of the classic papers on Kalman filtering, starting with Kalman's original paper in 1960. The papers are academically oriented, but someone who likes theory will obtain an interesting historical perspective from this book.

http://ourworld.compuserve.com/homepages/PDJoseph/This is Peter Joseph's Web site, and a useful resource on the topic of Kalman filtering. Dr. Joseph has worked with Kalman filters since their inception in 1960, and coauthored perhaps the earliest text on the subject (in 1968). His Web page includes lessons for the beginning, intermediate, and advanced student.

Return to June 2001 Table of Contents