Understanding the Basis of the Kalman Filter

Ramsey Faragher [lecture NOTES]Understanding the Basis of the Kalman FilterVia a Simple and Intuitive DerivationT his article provides a simpleand intuitive derivation ofth e Kalman filter, with th eaim of teach ing th is usefultool to students from disci-plines th at do not require a strong math ematical background. Th e most complicated level of math ematics required to understand this derivation is th e ability to multiply two Gaussian functions together and reduce the result to a compact form.The Kalman filter is over 50 years old but is still one of th e most important and common data fusion algorith ms in use today. Named after Rudolf E. Kálmán, th e great success of th e Kalman filter is due to its small compu-tational requirement, elegant recursive properties, and its status as the optimal estimator for one-dimensional linear systems with Gaussian error statistics [1] . Typical uses of th e Kalman filter include smooth ing noisy data and pro-viding estimates of parameters of inter-est. Applications include global positioning system receivers, ph ase-locked loops in radio equipment, smooth ing th e output from laptop trackpads, and many more.From a th eoretical standpoint, th e Kalman filter is an algorithm permitting exact inference in a linear dynamical system, which is a Bayesian model simi-lar to a hidden Markov model but where the state space of the latent variables is continuous and wh ere all latent and observed variables h ave a Gaussian dis-tribution (often a multivariate Gaussian distribution). Th e aim of th is lecture note is to permit people wh o find th is description confusing or terrifying to understand the basis of the Kalman fil-ter via a simple and intuitive derivation.RELEVANCEThe Kalman filter [2] (and its variantssuch as the extended Kalman filter [3]and unscented Kalman filter [4]) isone of th e most celebrated and popu-lar data fusion algorith ms in th e fieldof information processing. Th e mostfamous early use of th e Kalman filterwas in the Apollo navigation computerthat took Neil Armstrong to the moon,and (most importantly) brough t h imback. Today, Kalman filters are at workin every satellite navigation device,every smart ph one, and many com-puter games.The Kalman filter is typically derivedusing vector algebra as a minimummean squared estimator [5], anapproach suitable for students confidentin mathematics but not one that is easyto grasp for students in disciplines thatdo not require strong mathematics. TheKalman filter is derived h ere from firstprinciples considering a simple physicalexample exploiting a key property of theGaussian distribution—specifically th eproperty th at th e product of twoGaussian distributions is anoth erGaussian distribution.PREREQUISITESThis article is not designed to be a thor-ough tutorial for a brand-new student toth e Kalman filter, in th e interests ofbeing concise, but instead aims to pro-vide tutors with a simple meth od ofteaching the concepts of the Kalman fil-ter to students wh o are not strongmathematicians. The reader is expectedto be familiar with vector notation andterminology associated with Kalman fil-tering such as the state vector and cova-riance matrix. Th is article is aimed atthose who need to teach the Kalman fil-ter to oth ers in a simple and intuitivemanner, or for th ose wh o already h avesome experience with the Kalman filterbut may not fully understand its founda-tions. This article is not intended to be athorough and standalone education toolfor th e complete novice, as th at wouldrequire a ch apter, rath er th an a fewpages, to convey.PROBLEM STATEMENTTh e Kalman filter model assumes th atthe state of a system at a time t evolvedfrom the prior state at time t-1 accord-ing to the equationx F x B u wt t t t t t1=++-, (1 )where■x t is th e state vector containingth e terms of interest for th e system(e.g., position, velocity, h eading) attime t■u t is th e vector containing anycontrol inputs (steering angle, throt-tle setting, braking force)■F t is th e state transition matrixwh ich applies th e effect of each sys-tem state parameter at time t-1 onth e system state at time t (e.g., th eposition and velocity at time t-1both affect the position at time t)■B t is th e control input matrixwh ich applies th e effect of eachD igital Object Identifier 10.1109/MSP.2012.2203621 D ate of publication: 20 August 2012THE KALMAN FILTER IS OVER 50 YEARS OLD BUT IS STILL ONE OF THE MOST IMPORTANT AND COMMON DATA FUSION ALGORITHMS IN USE TODAY.c ontrol input parameter in th ev ector u t on th e state vector (e.g., applies the effect of the throttle set-ting on th e system velocity and position)■w t is th e vector containing th e process noise terms for each parame-ter in th e state vector. Th e process noise is assumed to be drawn from a zero mean multivariate normald istribution with covariance given bythe covariance matrix Q t.Measurements of the system can also be performed, according to the modelH x vz t t t t=+, (2)where■z t is the vector of measurements■H t is th e transformation matrix th at maps th e state vector parame-ters into the measurement domain■v t is th e vector containing th e measurement noise terms for eachobservation in the measurement vec-tor. Like the process noise, the mea-surement noise is assumed to be zero mean Gaussian wh ite noise withcovariance R t.In the derivation that follows, we will consider a simple one-dimensional track-ing problem, particularly that of a train moving along a railway line (see Figure 1). We can th erefore consider some example vectors and matrices in this problem. The state vector x t contains the position and velocity of the trainxxx ttt =o ;E.The train driver may apply a braking or accelerating input to the system, which we will consider here as a function of anapplied force f t and the mass of the trainm. Such control information is storedwithin the control vector u tumftt=.Th e relationsh ip between th e forceapplied via the brake or throttle duringth e time period ∆t (th e time elapsedbetween time epochs t-1 and t) and theposition and velocity of the train is givenby the following equations:()()x x x tmf t2t t tt112#TT=++--ox xmf tt tt1T=+-o o .These linear equations can be written inmatrix form as()xxt xxttmf1012ttttt112TTT=+--o o;;;>E E E H.And so by comparison with (1), we cansee for this example thatt TF B()t t101andt t2T T==2;>E H .The true state of the system x t cannot bedirectly observed, and the Kalman filterprovides an algorith m to determine anestimate x t t by combining models of thesystem and noisy measurements of cer-tain parameters or linear functions ofparameters. The estimates of the param-eters of interest in th e state vector areth erefore now provided by probabilitydensity functions (pdfs), rather than dis-crete values. The Kalman filter is basedon Gaussian pdfs, as will become clearfollowing th e derivation outlined belowin th e “Solutions” section. To fullydescribe th e Gaussian functions, weneed to know their variances and covari-ances, and these are stored in the covari-ance matrix P t. Th e terms along th emain diagonal of P t are th e variancesassociated with the corresponding termsin th e state vector. Th e off-diagonalterms of P t provide th e covariancesbetween terms in the state vector. In thecase of a well-modeled, one-dimensionallinear system with measurement errorsdrawn from a zero-mean Gaussian distri-bution, th e Kalman filter h as beensh own to be th e optimal estimator [1].In the remainder of this article, we willderive th e Kalman filter equations th atallow us to recursively calculate x t t bycombining prior knowledge, predictionsfrom systems models, and noisy mea-surements.The Kalman filter algorithm involvestwo stages: prediction and measure-ment update. Th e standard Kalman fil-ter equations for the prediction stage arex F x B ut t t t t t t111=+;;---t t (3)P F P F Qt t t t t t t111T=+;;---, (4)where Q t is the process noise covariancematrix associated with noisy controlinputs. Equation (3) was derived explic-itly in th e discussion above. We canderive (4) as follows. The variance asso-ciated with th e prediction x t t1;-t of anunknown true value x t is given by[()()]P x x x xE,t t t t t t t tT111=--;;;---t tand taking th e difference between (3)and (1) gives[FIG1] This figure shows the one-dim e nsional system under consideration.()x x F x x w t t t t t t t 111-=-+;;---t t[(())(())]P F x xF x xE w w t t t t t t t t t t T 1111111&#=-+-+;;;-------t t [()()][()][][].F x xx xF F x xw w x x F w w E E E E t t t t t t t t t t t t t t t t 111111111111TT T T T T ##=--+-+-+;;;;------------t t t t Noting th at th e state estimation errorsand process noise are uncorrelated[()]()E x x w E w x x0t t t t T t t t t T 111111-=-=;;------t t 6@[()()][].P F x xx xF w w P FP F Q E E t t t t t t t t t t t t t t t 1111111111T T T T &&=--+=+;;;;;----------t t The measurement update equations are given by xH x )(x K z t t t t t t t t t 11=+-;;;--t t t (5)P P K H P t t t t t t 1t t 1=-;;-;-, (6)whereK P H H P H R ().T T t t t t t t t t t 111=+;;--- (7)In the remainder of this article, we will derive th e measurement update equa-tions [(5)–(7)] from first principles. SOLUTIONSTh e Kalman filter will be derived h ere by considering a simple one-dimension-al tracking problem, specifically that of a train is moving along a railway line. At every measurement epoch we wish toknow th e best possible estimate of th e location of the train (or more precisely, the location of the radio antenna mount-ed on the train roof). Information is avail-able from two sources: 1) predictions based on th e last known position and velocity of the train and 2) measurementsfrom a radio ranging system deployed at the track side. The information from the predictions and measurements are com-bined to provide the best possible estimate of the location of the train. The system is shown graphically in Figure 1.Th e initial state of th e system (at time t = 0 s) is known to a reasonable accuracy, as sh own in Figure 2. Th e location of th e train is given by a Gaussian pdf. At th e next time epoch ()t 1s =, we can estimate the new posi-tion of the train, based on known limita-tions such as its position and velocity at t = 0, its maximum possible acceleration and deceleration, etc. In practice, we may h ave some knowledge of th e control inputs on the brake or accelerator by the driver. In any case, we have a prediction of the new position of the train, represented in Figure 3 by a new Gaussian pdf with a new mean and variance. Mathematically this step is represented by (1). The vari-ance has increased [see (2)], representing our reduced certainty in the accuracy of our position estimate compared to t = 0, due to the uncertainty associated with any process noise from accelerations or decel-erations undertaken from time t = 0 to time t = 1.[FIG2] The initial knowledge of the system at time t = 0. The red Gaussian distribution represents the pdf providing the initial confidence in the estimate of the position of the train. The arrow pointing to the right represents the known initial velocity of the train.???Prediction (Estimate)[FIG3] Here, the predict i on of the location of the train at time t = 1 and the level of uncertainty in that prediction is shown. The confidence in the knowledge of the position of the train has decreased, as we are not certain if the train has undergone any accelerations or decelerations in the intervening period from t = 0 to t = 1.THE BEST ESTIMATEWE CAN MAKE OF THE LOCATION OF THE TRAIN IS PROVIDED BY COMBINING OUR KNOWLEDGE FROM THE PREDICTION AND THEMEASUREMENT.At t = 1, we also make a measure-ment of th e location of th e train using the radio positioning system, and this is represented by the blue Gaussian pdf in Figure 4. The best estimate we can make of the location of the train is provided by combining our knowledge from the pre-diction and th e measurement. Th is is ach ieved by multiplying th e two corre-sponding pdfs togeth er. Th is is repre-sented by the green pdf in Figure 5.A key property of the Gaussian function is exploited at this point: the product of two Gaussian functions is anoth er Gaussian function. This is critical as it permits an endless number of Gaussian pdfs to be multiplied over time, but th e resulting function does not increase in complexity or number of terms; after each time epoch the new pdf is fully r epresented by a Gaussian function. Th is is th e key to th e elegant recursive properties of the Kalman filter.The stages described above in the fig-ures are now considered again math e-matically to derive th e Kalman filter measurement update equations.Th e prediction pdf represented by the red Gaussian function in Figure 3 is given by the equation ()y r;,()r 11121212_n v v n --. (8)Th e measurement pdf represented bythe blue Gaussian function in Figure 4 is given by ()y r;,()r 22222222v n v n --. (9)The information provided by these two pdfs is fused by multiplying the two t ogether, i.e., considering th e prediction and th e measurement together (see Figure 5). The new pdf representing th e fusion of th einformation from the prediction and mea-surement, and our best current estimate of the system, is therefore given by the prod-uct of these two Gaussian functions (,).y r e ,;,()()((r r r r 11222222fused ))1212222212122222n v n v =v n v n v n v n ------+-=eo(10)Th e quadratic terms in th is newfunction can expanded and th en th e whole expression rewritten in Gaussian form()r n -()y r ;,2fused fused fused 22fusedfused n v v -=, (11)where()1222122212112221221fused n v v n v n v n v v v n n =++=++- (12)and21222122212122214fusedv v v v v v v v v =+=-+. (13)Th ese last two equations represent th emeasurement update steps of th e Kalman filter algorith m, as will be sh own explicitly below. However, to present a more general case, we need to consider an extension to this example.In the example above, it was assumed th at th e predictions and measurements were made in the same coordinate frame and in the same units. This has resulted in a particularly concise pair of[FIG4] Shows the measur e ment of the location of the train at time t = 1 and the level of uncertainty in that noisy measurement, represented by the blue Gaussian pdf. The combined knowledge of this system is provided by multiplying these two pdfs together.???Measurement (Noisy)Prediction (Estimate)[FIG5] Shows the new pdf (green) generated by multiplying the pdfs associated with the prediction and measurement of the train’s location at time t = 1. This new pdf provides the best estimate of the location of the train, by fusing the data from the prediction and the measurement.A KEY PROPERTY OF THE GAUSSIAN FUNCTION IS EXPLOITED AT THIS POINT: THE PRODUCT OF TWO GAUSSIAN FUNCTIONS IS ANOTHER GAUSSIANFUNCTION.[lecture NOTES ]c ontinuedequations representing th e prediction and measurement update stages. It isi mportant to note however that in reality a function is usually required to map predictions and measurements into th e same domain. In a more realistic exten-sion to our example, the position of the train will be p redicted directly as a new distance along the railway line in units of meters, but the time of flight measure-ments are recorded in units of seconds. To allow th e prediction and measure-ment pdfs to be multiplied together, one must be converted into th e domain of the other, and it is standard practice to map th e predictions into th e measure-ment domain via th e transformation matrix H t .We now revisit (8) and (9) and, instead of allowing y 1 and y 2 to both represent val-ues in meters along the railway track, we consider the distribution y 2 to represent the time of flight in seconds for a radio signal propagating from a transmitter positioned at x = 0 to the antenna on the train. The spatial prediction pdf y 1 is con-verted into the measurement domain by scaling th e function by c , th e speed of light. Equations (8) and (9) therefore must be rewritten as()y s c;,,c s c 11121212_n v v n --`c jm and()y s ;,s 22222222_n v v n --^h ,where both distributions are now defined in th e measurement domain, radio sig-nals propagate along th e time “s ” axis, and the measurement unit is the second.Following th e derivation as before we now findccccc112221221fusedvn n v v n n =++-``c j j m.c c c 112221221fused &$n n v v v n n =++-J LKKK K `c N P O O OO j m (16)Substituting H c 1= and ()K H 12v =H 22221v v +^h results inK H 121fused $n n n n =+-^h . (17)Similarly th e fused variance estimate becomescccc2212122214fusedv v v v v =-+```j j jcc c12122212122fused &v v v v v v =-+J L KK K K `N P O O O Oj . KH 1212v v =- (18)We can now compare certain terms resulting from th is scalar derivation with th e standard vectors and matrices used in the Kalman filter algorithm:■xt t fused "n ;t : th e state vector fol-lowing data fusion■xt t 11"n -;t : the state vector before data fusion, i.e., the prediction■P t t 2fused "v ;: the covariance matrix (confidence) following data fusion■P t t 112"v -;: th e covariance matrix (confidence) before data fusion■z t 2"n : the measurement vector■R t 22"v : th e uncertainty matrix associated with a noisy set of mea-surements■H H t ": the transformation matrix used to map state vector parameters into the measurement domain■ K K H H t2122212"v v v =+P H H P H R ()T T t t t t t t t t 111=+;;---: the Kalman gain.It is now easy to see h ow th e stan-dard Kalman filter equations relate to (17) and (18) derived above: H H H 1212221221fused $n n v v v n n =++-e^o h xx K z H x t t t t t t t t t 11"=+=;;;--t t t ^h H H H 212212221212fused v v v v v v =-+eo P P K H P t t t t t t t t 11"=-;;;--.CONCLUSIONSThe Kalman filter can be taught using a simple derivation involving scalar math-ematics, basic algebraic manipulations, and an easy-to-follow th ough t experi-ment. This approach should permit stu-dents lacking a strong math ematical interest to understand th e core math e-matics underlying th e Kalman filter in an intuitive manner and to understand th at th e recursive property of th e filter are provided by th e unique multiplica-tive property of the Gaussian function. AUTHORRamsey Faragher (ramsey@) is a principal scientist at th e BAE Systems Advanced Tech nology Centre, United Kingdom.REFERENCES[1] B. D. O. Anderson and J. B. Moore, Optimal Filtering . New York: Dover, 2005.[2] R. E. Kalman, “A new approach to linear filtering and prediction problems,” J. Basic Eng., vol. 82, no. 1, pp. 35–45, Mar. 1960.[3] P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integra ted Na viga tion Systems . Norwood, MA: Artech House, 2008.[4] S. J. Julier and J. K. Uhlmann, “Unscented filter-ing and nonlinear estimation,” Proc. IEEE , vol. 92, no. 3, pp. 401–422, 2004.[5] J. Bibby and H. Toutenburg, Prediction a nd Improved Estimation in Linear Models . New York: Wiley, 1977. [SP ]THE KALMAN FILTER CAN BE TAUGHT USING A SIMPLE DERIVATION INVOLVING SCALAR MATHEMATICS, BASIC ALGEBRAIC MANIPULATIONS, AND AN EASY-TO-FOLLOW THOUGHT EXPERIMENT.。