An Introduction to the Kalman Filter
by
Greg Welch1
and
Gary Bishop2
Department of Computer Science
University of North Carolina at Chapel Hill
Chapel Hill, NC 27599-3175
Abstract
In 1960, R.E. Kalman published his famous paper describing a recursive solution to the discrete-data linear filtering problem. Since that time, due in large part to ad-vances in digital computing, the Kalman filter has been the subject of extensive re-arch and application, particularly in the area of autonomou
s or assisted
navigation.
腰子的做法
The Kalman filter is a t of mathematical equations that provides an efficient com-putational (recursive) solution of the least-squares method. The filter is very pow-erful in veral aspects: it supports estimations of past, prent, and even future states, and it can do so even when the preci nature of the modeled system is un-known.
羽泉彩虹
The purpo of this paper is to provide a practical introduction to the discrete Kal-man filter. This introduction includes a description and some discussion of the basic discrete Kalman filter, a derivation, description and some discussion of the extend-ed Kalman filter, and a relatively simple (tangible) example with real numbers & results.
2. gb@cs.unc.edu, www.cs.unc.edu/~gb
An Introduction to the Kalman Filter 2 1The Discrete Kalman Filter
In 1960, R.E. Kalman published his famous paper describing a recursive solution to the discrete-data linear filtering problem [Kalman60]. Since that time, due in large part to advances in digital computing, the Kalman filter has been the subject of extensive rearch and application,
particularly in the area of autonomous or assisted navigation. A very “friendly” introduction to the general idea of the Kalman filter can be found in Chapter 1 of [Maybeck79], while a more complete introductory discussion can be found in [Sorenson70], which also contains some interesting historical narrative. More extensive references include [Gelb74], [Maybeck79], [Lewis86],
[Brown92], and [Jacobs93].
The Process to be Estimated
The Kalman filter address the general problem of trying to estimate the state of a discrete-time controlled process that is governed by the linear stochastic difference equation
,
(1.1)
with a measurement that is
.(1.2)The random variables and reprent the process and measurement noi (respectively). They are assumed to be independent (of each other), white, and with normal probability distributions
,
(1.3).(1.4)
The matrix
A in the difference equation (1.1) relates the state at time step k to the state at step k +1, in the abnce of either a driving function or process noi. The matrix
B relates the control input to the state x . The matrix H in the measurement equation (1.2) relates the state to the measurement z k .
唐太宗年号The Computational Origins of the Filter
We define (note the “super minus”) to be our a priori state estimate at step k given knowledge of the process prior to step k , and to be our a posteriori state estimate at step k given measurement . We can then define a priori and a posteriori estimate errors as
The a priori estimate error covariance is then
,(1.5)
x ℜn ∈x k 1+A k x k Bu k w k ++=z ℜm ∈z k H k x k v k +=w k v k p w ()N 0Q ,()∼p v ()N 0R ,()∼n n ×n l ×u ℜl ∈m n ×x ˆk -ℜn ∈x
ˆk ℜn ∈z k e k -x k x
ˆk -, and –≡e k x k x
ˆk .–≡P k -E e k -e k -T []=
and the a posteriori estimate error covariance is
.(1.6)
In deriving the equations for the Kalman filter, we begin with the goal of finding an equation that computes an a posteriori state estimate as a linear combination of an a priori estimate and a weighted difference between an actual measurement and a measurement prediction as shown belo
w in (1.7). Some justification for (1.7) is given in “The Probabilistic Origins of the Filter” found below.
(1.7)The difference in (1.7) is called the measurement innovation , or the residual . The residual reflects the discrepancy between the predicted measurement and the actual measurement . A residual of zero means that the two are in complete agreement.
The matrix
K in (1.7) is chon 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 , tting that result equal to zero, and then solving for K . For more details e [Maybeck79], [Brown92], or [Jacobs93]. One form of the resulting K that minimizes (1.6) is given by 1
.(1.8)
Looking at (1.8) we e that as the measurement error covariance approaches zero, the gain K weights the residual more heavily. Specifically,万方论文检索
.
On the other hand, as the a priori estimate error covariance approaches zero, the gain K weights
the residual less heavily. Specifically,
.Another way of thinking about the weighting by K is that as the measurement error covariance approaches zero, the actual measurement is “trusted” more and more, while the predicted measurement is trusted less and less. On the other hand, as the a priori estimate error covariance approaches zero the actual measurement is trusted less and less, while the predicted measurement
is trusted more prents the Kalman gain in one popular form.
P k E e k e k T []=x ˆk x ˆk -z k H k x ˆk -x
ˆk x ˆk -K z k H k x ˆk -–()+=z k H k x ˆk -–()H k x
ˆk -z k n m ×e k K k P k -H k T H k P k -H k T R k
+()1–=P k -H k T H k P k -H k T R k +---------------------------------=R k K k R k 0→lim H k
1–=P k -K k P k -
健康检查制度0→lim 0=R k z k H k x ˆk -P k -z k H k x
ˆk -
The Probabilistic Origins of the Filter
The justification for (1.7) is rooted in the probability of the a priori estimate conditioned on all prior measurements (Bayes’ rule). For now let it suffice to point out that the Kalman filter maintains the first two moments of the state distribution,
The a posteriori state estimate (1.7) reflects the mean (the first moment) of the state distribution— it is normally distributed if the conditions of (1.3) and (1.4) are met. The a posteriori estimate error covariance (1.6) reflects the variance of the state distribution (the cond non-central moment). In other words,
.For more details on the probabilistic origins of the Kalman filter, e [Maybeck79], [Brown92], or
[Jacobs93].
The Discrete Kalman Filter Algorithm
We will begin this ction with a broad overview, covering the “high-level” operation of one form of the discrete Kalman filter (e the previous footnote). After prenting this high-level view, we will narrow the focus to the specific equations and their u in this version of the filter.
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 rembles that of a predictor-corrector algorithm for solving numerical problems as shown below in Figure 1-1.
绿色的壁纸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.
x
ˆk -
z k E x k []x
ˆk =E x k x
ˆk –()x k x ˆk –()T []P k .=p x k z k ()N E x k []E x k x
ˆk –()x k x ˆk –()T [],()∼N x ˆk P k ,().=
The specific equations for the time and measurement updates are prented below in Table 1-1 and Table 1-2.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 discusd 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 ud 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.
Filter Parameters and Tuning
In the actual implementation of the filter, each of the measurement error covariance matrix and the process noi (given by (1.4) and (1.3) respectively) might be measured prior to operation of the filter. In the ca of the measurement error covariance in particular this makes n—becau we need to be able to measure the process (while operating the filter) we should generally be able to take some off-line sample measurements in order to determine the variance of the measurement error.
Table 1-1: Discrete Kalman filter time update equations.
(1.9)(1.10)
Table 1-2: Discrete Kalman filter measurement update equations.
(1.11)
(1.12)
(1.13)x
ˆk 1+-A k x ˆk Bu k +=P k 1+-A k P k A k T Q k +=A k Q k K k P k -H k T H k P k -H k T R k +()1–=x ˆk x ˆk -K z k H k x ˆk -
–()+=P k I K k H k –()P k -=K k z k R k Q k R k
In the ca of , often times the choice is less deterministic. For example, this noi source is often ud to reprent the uncertainty in the process model (1.1). Sometimes a very poor model can be ud simply by “injecting” enough uncertainty via the lection of . Certainly in this ca one would hope that the measurements of the process would be reliable.
In either ca, whether or not we have a rational basis for choosing the parameters, often times superior filter performance (statistically speaking) can be obtained by “tuning” the filter parameters and . The tuning is usually performed off-line, frequently with the help of another (distinct) Kalman filter.
Figure 1-2. A complete picture of the operation of the Kalman filter, com-bining the high-level diagram of Figure 1-1 with the equations from Table 1-1 and Table 1-2.
In closing we note that under conditions where and .are constant, both the estimation error covariance and the Kalman gain will stabilize quickly and then remain constant (e the filter update equations in Figure 1-2). If this is the ca, the 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 .
It is frequently the ca however that the measurement error (in particular) does not remain constant. For example, when sighting beacons in our optoelectronic tracker ceiling panels, the noi in measurements of nearby beacons will be smaller than that in far-away beacons. Also, the process noi is sometimes changed dynamically during filter operation in order to adjust to different dynamics. For example, in the ca of tracking the head of a ur of a 3D virtual environment we might reduce the magnitude of if the ur ems to be moving slowly, and increa the magnitude if the dynamics start changing rapidly. In such a ca can be ud to model not only the uncertainty in the model, but also the uncertainty of the ur’s intentions.
排卵试纸可以测早孕吗Q k Q k Q k R k
k -and P k -Q k R k P k K k P k P k -P k ≡P k Q k Q k Q k