This article was downloaded by: [University of South Florida] On: 21 September 2011, At: 14:21 Publisher: Taylor

& Francis Informa Ltd Registered in England and Wales Registered Number: 1072954 Registered office: Mortimer House, 37-41 Mortimer Street, London W1T 3JH, UK

International Journal of Control

Publication details, including instructions for authors and subscription information: http://www.tandfonline.com/loi/tcon20

Kalman filters for non-linear systems: a comparison of performance

Tine Lefebvre , Herman Bruyninckx & Joris De Schutter
a a a a

Katholieke Universiteit Leuven, Dept. of Mechanical Eng., Division P.M.A., Celestijnenlaan 300B, B-3001 Heverlee, Belgium
b

Katholieke Universiteit Leuven, Dept. of Mechanical Eng., Division P.M.A., Celestijnenlaan 300B, B-3001 Heverlee, Belgium E-mail: Tine.Lefebvre@mech.kuleuven.ac.be Available online: 19 Feb 2007

To cite this article: Tine Lefebvre , Herman Bruyninckx & Joris De Schutter (2004): Kalman filters for non-linear systems: a comparison of performance, International Journal of Control, 77:7, 639-653 To link to this article: http://dx.doi.org/10.1080/00207170410001704998

PLEASE SCROLL DOWN FOR ARTICLE Full terms and conditions of use: http://www.tandfonline.com/page/terms-and-conditions This article may be used for research, teaching and private study purposes. Any substantial or systematic reproduction, re-distribution, re-selling, loan, sub-licensing, systematic supply or distribution in any form to anyone is expressly forbidden. The publisher does not give any warranty express or implied or make any representation that the contents will be complete or accurate or up to date. The accuracy of any instructions, formulae and drug doses should be independently verified with primary sources. The publisher shall not be liable for any loss, actions, claims, proceedings, demand or costs or damages whatsoever or howsoever caused arising directly or indirectly in connection with or arising out of the use of this material.

INT. J. CONTROL,

10 MAY 2004, VOL. 77, NO. 7, 639653

Kalman lters for non-linear systems: a comparison of performance

TINE LEFEBVREy*, HERMAN BRUYNINCKXy and JORIS DE SCHUTTERy
The Kalman lter is a well-known recursive state estimator for linear systems. In practice, the algorithm is often used for non-linear systems by linearizing the systems process and measurement models. Dierent ways of linearizing the models lead to dierent lters. In some applications, these Kalman lter variants seem to perform well, while for other applications they are useless. When choosing a lter for a new application, the literature gives us little to rely on. This paper tries to bridge the gap between the theoretical derivation of a Kalman lter variant and its performance in practice when applied to a non-linear system, by providing an application-independent analysis of the performances of the common Kalman lter variants. This paper separates performance evaluation of Kalman lters into (i) consistency, and (ii) information content of the estimates; and it separates the lter structure into (i) the process update step, and (ii) the measurement update step. This decomposition provides the insights supporting an objective and systematic evaluation of the appropriateness of a particular Kalman lter variant in a particular application.

Downloaded by [University of South Florida] at 14:21 21 September 2011

1. Introduction During recent decades, many research areas looked into the matter of on-line state estimation. The uncertainty on the state value varies over time due to the changes in the system state (the process updates) and due to the information in the measurements (the measurement updates). The uncertainty can be represented in dierent ways, e.g. by intervals or fuzzy sets. In Bayesian estimation (Bayes 1763, Laplace 1812), a state estimate is represented by a probability density function (pdf ). Fast analytical update algorithms require the pdf to be an analytical function of a limited number of time-varying parameters, which is only true for some systems. A well-known example is systems with linear process and measurement models and with additive Gaussian uncertainties. The pdf is then a Gaussian distribution, which is fully determined by its mean vector and covariance matrix. This mean and covariance are updated analytically with the Kalman lter (KF) algorithm (Kalman 1960, Sorenson 1985). For most non-linear systems, the pdf cannot be written as an analytical function with time-varying parameters. In order to have a computationally interesting update algorithm, the KF is used as an approximation. This is achieved by linearization of the process and measurement models of the system. It also means that the true pdf is approximated

by a Gaussian distribution. Dierent ways of linearization (dierent KF variants) lead to dierent results. This paper describes (i) how the common KF variants dier in their linearization of the process and measurement models; (ii) how they take the linearization errors into account; and (iii) how the quality of their state estimates depends on the previous two choices. The studied algorithms are: 1. The extended Kalman lter (EKF) (Gelb et al. 1974, Maybeck 1982, Bar-Shalom and Li 1993, Tanizaki 1996); 2. The iterated extended Kalman lter (IEKF) (Gelb et al. 1974, Maybeck 1982, Bar-Shalom and Li 1993, Tanizaki 1996); 3. The linear regression Kalman lter (LRKF) (Lefebvre et al. 2002). This lter comprises the central dierence lter (CDF) (Schei 1997), the rst-order divided dierence lter (DD1) (Nrgaard et al. 2000 a, b) and the unscented Kalman lter (UKF) (Uhlmann 1995, Julier and Uhlmann 1996, 2001, Julier et al. 2000). The paper gives the following new insights: 1. The quality of the estimates of the KF variants can be expressed by two criteria, i.e. the consistency and the information content of the estimates (dened in } 3). This paper relates the consistency and information content of the estimates to (i) how the linearization is performed and (ii) how the linearization errors are taken into account. 2. Although the lters use similar linearization techniques for the linearization of the process and measurement models, there can be

Received 1 September 2003. Revised and accepted 1 April 2004. * Author for correspondence. e-mail: Tine.Lefebvre@ mech.kuleuven.ac.be y Katholieke Universiteit Leuven, Dept. of Mechanical Eng., Division P.M.A., Celestijnenlaan 300B, B-3001 Heverlee, Belgium.

International Journal of Control ISSN 00207179 print/ISSN 13665820 online # 2004 Taylor & Francis Ltd http://www.tandf.co.uk/journals DOI: 10.1080/00207170410001704998

640

T. Lefebvre et al. a substantial dierence in their performance for both updates: (a) for the linearization of the process update (} 4), which describes the evolution of the state, the state estimate and its uncertainty are the only available information; (b) the measurement update (} 5), on the other hand, describes the fusion of the information in the state estimate with the information in the new measurement. Hence, in this update also the new measurement is available and can be used to linearize the measurement model. They lead to a systematic choice of a lter, where previously the choice was mainly made based on success in similar applications or based on trial and error. Examples of 2D systems are provided. The models are chosen such that they provide a clear graphical demonstration of the discussed eects. For the measurement update, the lters performances are system dependent, hence, in that case several models are used for illustration.

2.

The Kalman lter algorithm

Downloaded by [University of South Florida] at 14:21 21 September 2011

Therefore, it can be interesting to use dierent lters for both updates. 3. Two new insights on the performance of specic KF variants are: (i) the IEKF measurement update outperforms the EKF and LRKF updates if the stateor at least the part of it that causes the non-linearity in the measurement modelis instantaneously fully observable (} 5.2); and (ii) for large uncertainties on the state estimate, the LRKF measurement update yields consistent but non-informative state estimates (} 5.3). These insights are obtained because: 1. This paper describes all lter algorithms as the application of the basic KF algorithm to linearized process and measurement models. The dierence between the KF variants is situated in the choice of linearization and the compensation for the linearization errors. In previous work this linearization was not always recognized, e.g. the UKF is originally derived as a lter which does not linearize the models. 2. The analysis claries how some lters automatically adapt their compensation for linearization errors, while other lters have constant (developer-determined) error compensation. 3. Additionally, the paper compares the lter performances separately for process updates and measurement updates instead of their overall performance when they are both combined. In the existing literature, the performances of the KF variants are often compared by interpreting the estimation results for a specic application after executing a large number of process and measurement update steps. The analysis starts from a general formulation of non-linear process and measurement models, making the results application independent. The obtained insights are important for all researchers and developers who want to apply a KF variant to a specic application.

2.1. The (linear) Kalman lter The Kalman lter (KF) (Kalman 1960, Sorenson 1985) is a special case of Bayesian ltering theory. It applies to the estimation of a state x if the state space description of the estimation problem has linear process and measurement equations subject to additive Gaussian uncertainty xk F k1 xk1 bk1 C k1 qp, k1 zk H k xk d k E k qm, k : 1 2

z is the measurement vector. The subscripts k and k 1 indicate the time step. F, b, C, H, d and E are (possibly non-linear) functions of the system input. qp denotes the process uncertainty, being a random vector sequence with zero mean and known covariance matrix Q. qm is the measurement uncertainty and is a random vector sequence with zero mean and known covariance matrix R; qp and qm are mutually uncorrelated and uncorrelated between sampling timesy. Furthermore, assume a ^ Gaussian prior pdf px0 with mean x0j0 and covariance matrix P0j0 . ^ ^ For this system, the pdfsz pxk jZk1 and pxk jZk are also Gaussian distributions. The ltering formulas can be expressed as analytical functions calculating the ^ mean vector x and covariance matrix P of these pdfs ^ xkjk1 Epxk jZk1 xk ^ ^ F k1 xk1jk1 bk1 h T i ^ ^ Pkjk1 Epxk jZk1 xk xkjk1 xk xkjk1 ^ F k1 Pk1jk1 F T C k1 Qk1 CT k1 k1 4 3

y Correlated uncertainties can be dealt with by augmenting the state vector, this is the original formulation of the KF (Kalman 1960). Expressed in this new state vector, the process and measurement models are of the form (1) and (2) with uncorrelated uncertainties. ^ z pxk jZj denotes the pdf of the state x at time k, given the ^ ^ ^ measurements Zj fz1 , . . . , zj g up to time j.

Kalman lters for non-linear systems ^ xkjk Epxk jZk xk ^ ^ xkjk1 K k gk h T i ^ ^ Pkjk Epxk jZk xk xkjk xk xkjk ^ I nn K k H k Pkjk1 where ^ ^ gk zk H k xkjk1 d k Sk E k Rk E T H k Pkjk1 H T k k K k Pkjk1 H T S 1 : k k 7 8 9 6 3. Consistency and information content of the estimates 5

641

leads to non-optimal estimates and covariance matrices. Dierent ways of linearizing the process and measurement models, i.e. dierent choices for F, b, Q , H, d and R , yield other results. This paper aims at making an objective comparison of the performances of the commonly used linearizations (KF variants).

Downloaded by [University of South Florida] at 14:21 21 September 2011

g is called the innovation, its covariance is S. K is the Kalman gain. Equations (3)(4) are referred to as the process update, equations (5)(9) as the measurement ^ update. xkjk1 is called the predicted state estimate and ^ ^ xkjk the updated state estimate. If no measurement zk is available at a certain time step k, then equations (5)(9) ^ ^ reduce to xkjk xkjk1 and Pkjk Pkjk1 . 2.2. Kalman lters for non-linear systems The KF algorithm is often applied to systems with non-linear process and measurement modelsy xk f k1 xk1 Ck1 qp, k1 zk hk xk E k qm, k by linearization xk F k1 xk1 bk1 q k1 C k1 qp, k1 p, zk H k xk d k q k m, E k qm, k : 12 13 10 11

The KF variants for non-linear systems calculate ^ an estimate xkji and covariance matrix Pkji for a pdf which is non-Gaussian. The performance of these KFs depends on how representative the Gaussian pdf with ^ mean xkji and covariance Pkji is for the (unknown) pdf ^ ^ pxk jZi . Figure 1 shows a non-Gaussian pdf pxk jZ i ^ i , and three possible Gaussian approximations p1 xk jZ ^ ^ ^ p2 xk jZ i and p3 xk jZi . Intuitively we feel that p1 xk jZ i is a good approximation because the same values of x ^ are probable. Similarly p3 xk jZi is not a good approximation because a lot of probable values for x of the original distribution have a probability density of ^ ^ approximately zero in p3 xk jZ i . Finally pdf p2 xk jZ i ^ is more uncertain than pxk jZ i because a larger domain of x values is uncertain. These intuitive reexions are formulated in two criteria: the consistency and the information content of the state estimate. The consistency of the state estimate is a necessary condition for a lter to be acceptable. The information content of the state estimates denes an ordering between all consistent lters. 3.1. The consistency of the state estimate ^ A state estimate xkji with covariance matrix Pkji is called consistent if h T i ^ ^ Epxk jZi xk xkji xk xkji 14 Pkji : ^ For consistent results, the matrix Pkji is equal to or larger than the expected squared deviation with respect ^ to the estimate xkji under the (unknown) distribution ^ ^ pxk jZi . The mean and covariance of pdfs p1 xk jZ i ^ i in gure 1 obey equation (14). Pdf and p2 xk jZ ^ p3 xk jZ i , on the other hand, is inconsistent. ^ Inconsistency of the calculated state estimate xkji and covariance matrix Pkji (divergence of the lter) is the most encountered problem with the KF variants. In this case, Pkji is too small and no longer represents a ^ reliable measure for the uncertainty on xkji . Even more, once an inconsistent state estimate is met, the subsequent state estimates are also inconsistent. This is because the lter believes the inconsistent state estimate to be more accurate than it is in reality and hence it

The dierence between these models and models (1) and (2) is the presence of the terms q and q representing p m the linearization errors. The additional uncertainty on the linearized models due to these linearization errors is modelled by the covariance matrices Q and R . Unfortunately, applying the KFz (3)(9) to systems with non-linear process and/or measurement models

y Models which are non-linear functions of the uncertainties qp and qm , can be dealt with by augmenting the state vector with the uncertainities. Expressed in this new state vector, the process and measurement models are of the form (10) and (11). z Ck1 qp, k1 of equation (1) corresponds to qp, k1 Ck1 qp, k1 of equation (12); hence instead of T T Ck1 Qk1 Ck1 in equation (4), Q Ck1 Qk1 Ck1 is used. k1 Ek qm, k of equation (2) corresponds to q k Ek qm, k of m,
T equation (13); hence instead of Ek Rk Ek in equation (9), T Rk Ek Rk Ek is used.

642

T. Lefebvre et al.

Downloaded by [University of South Florida] at 14:21 21 September 2011

Figure 1.

^ ^ ^ ^ ^ Non-Gaussian pdf pxk jZi with three Gaussian approximations p1 xk jZi , p2 xk jZi and p3 xk jZ i . p1 xk jZ i and ^ i are consistent, p3 xk jZi is inconsistent. p1 xk jZ i is more informative than p2 xk jZi . ^ ^ ^ p2 xk jZ

attaches too much weight to this state estimate when processing new measurements. Testing for inconsistency is done by consistency tests such as tests on the sum of a number of normalized innovation squared values (SNIS) (Willsky 1976, BarShalom and Li 1993). 3.2. The information content of the state estimate The calculated covariance matrix Pkji indicates how ^ uncertain the state estimate xkji is: a large covariance matrix indicates an inaccurate (and little useful) state estimate; the smaller the covariance matrix, the larger the information content of the state estimate. For ^ ^ example both pdfs p1 xk jZi and p2 xk jZ i of gure 1 ^ i , however, p1 xk jZi has a ^ are consistent with pxk jZ smaller variance, hence it is more informative than ^ p2 xk jZi (a smaller domain of x values is probable). The most informative, consistent approximation is the Gaussian with the same mean and covariance as the ^ original distribution, i.e. p1 xk jZ i for the example. There is a trade-o between consistent and informative state estimates: inconsistency can be avoided by making Pkji articially larger (see equation (14)). However, making Pkji too large, i.e. larger than necessary for consistency, corresponds to losing information about the actual accuracy of the state estimate. The dierent KF variants linearize the process and the measurement models in the uncertainty region

around the state estimate. Consistent estimates are obtained by adding process and measurement uncertainty on the linearized models to compensate for the linearization errors. In order for the estimates to be informative, (i) the linearization errors need to be as small as possible; and (ii) the extra uncertainty on the linearized models should not be larger than necessary to compensate for these errors. The following sections describe how the extended Kalman lter, the iterated extended Kalman lter and the linear regression Kalman lter dier in their linearization of the process and measurement models; how they take the linearization errors into account; and how the qualityy of the state estimates, expressed in terms of consistency and information content, depends on these two choices.

4.

Non-linear process models

This section contains a comparison between the process updates of the dierent KF variants when dealing with a non-linear process model (10) with linearization (12). The KF variants dier by their choice of F k1 ,

y The more non-linear the behaviour of the process or measurement model in the uncertainty region around the state estimate, the more pronounced the dierence in quality performance (consistency and information content of the state estimates) between the KF variants.

Kalman lters for non-linear systems

F k1  @f k1  @x x ^  k1jk1 @f k1   @x
^ xk1jk1

643
Q k1 0nn 0nn
1 r

bk1 ^ f k1 ^k1jk1 F k1 xk1jk1 x ^ f k1 ^k1jk1 F k1 xk1jk1 x arg minF, b Pr

T j1 ej ej

EKF IEKF LRKF

arg minF, b

Pr

T j1 ej ej

Pr

T j1 ej ej

Table 1. Summary of the linearization of the process model by the extended Kalman lter (EKF), the iterated extended Kalman lter (IEKF) and the linear regression Kalman lter (LRKF).

Downloaded by [University of South Florida] at 14:21 21 September 2011

bk1 and Q . After linearization, they all use process k1 update equations (3) and (4) to update the state estimate and its uncertainty. Section 4.1 describes the linearization of the process model by the EKF and IEKF, } 4.2 by the LRKF. The formulas are summarized in table 1. Section 4.4 presents some examples. 4.1. The (iterated) extended Kalman lter The EKF and the IEKF linearizey the process model by a rst-order Taylor series around the updated state ^ estimate xk1jk1  @f k1   F k1 15 @x xk1jk1 ^ ^ ^ bk1 f k1 xk1jk1 F k1 xk1jk1 : 16

The LRKF algorithm uses a linearized process function (12) where F k1 , bk1 and Q are obtained k1 by statistical linear regression through the X jk1jk1 , X jkjk1 points, j 1, . . . , r; i.e. the deviations ej between the function values in X jk1jk1 for the non-linear and the linearized function are minimized in least-squares sense   ej X jkjk1 FX jk1jk1 b 19 F k1 , bk1 arg min
F, b r X j1

eT ej : j

20

The sample covariance of the deviations ej Q k1

r 1X T ee r j1 j j

21

The basic (I)EKF algorithms do not take the linearization errors into account (n is the dimension of the state vector x) Q  0nn : k1 17

This leads to inconsistent state estimates when these errors cannot be neglected. 4.2. The linear regression Kalman lter The linear regression Kalman lter (LRKF) uses the function values of r regression points X jk1jk1 in state space to model the behaviour of the process function in the uncertainty region around the updated ^ state estimate xk1jk1 . The regression points are chosen such that their mean and covariance matrix equal the ^ state estimate xk1jk1 and its covariance matrix Pk1jk1 . The CDF, DD1 and UKF lters correspond to specic choices. The function values of the regression points are X
j kjk1

gives an idea of the magnitude of the linearization errors ^ in the uncertainty region around xk1jk1 . Intuitively we feel that when enoughz regression points are taken, the state estimates of the LRKF process update are consistent and informative. They are consistent because Q gives a well-founded approxik1 mation of the linearization errors (equation (21)). They are informative because the linearized model is a good approximation of the process model in the uncertainty ^ region around xk1jk1 (equations (19) and (20)). 4.3. Extra process uncertainty In all of the presented lters, the user can decide to add extra process uncertainty Q (or to multiply the k1 calculated covariance matrix Pkjk1 by a fading factor larger than 1 (Bar-Shalom and Li 1993)). This is useful if the basic lter algorithm is not consistent. For example, this is the case for the (I)EKF or for an LRKF with a number of regression points too limited to capture the
z This depends on the non-linearity of the model in the uncertainty region around the state estimate. A possible approach is to increase the number of regression points until the resulting linearization (with error covariance) does not change any more. Of course, because the true pdf is unknown, it is not possible to guarantee that the iteration has converged to a set of regression points representative for the model behaviour in the uncertainty region.

f k1 X

j k1jk1 :

18

y The EKF and IEKF only dier in their measurement update (} } 5.1 and 5.2).

644

T. Lefebvre et al.

Downloaded by [University of South Florida] at 14:21 21 September 2011

Figure 2. Non-linear process model. Uncertainty ellipses for the updated state estimate at k 1 (dashed line), for the (I)EKF predicted state estimate (full line) and the Monte Carlo uncertainty ellipse (dotted line). The predicted state estimate is inconsistent due to the neglected linearization errors: the uncertainty ellipse of the IEKF predicted estimate is shifted with respect to the Monte Carlo uncertainty ellipse and is somewhat smaller.

non-linear behaviour of the process model in the uncer^ tainty region around xk1jk1 . For a particular problem, values for Q that result k1 in consistent and informative state estimates are obtained by o-line tuning or on-line parameter learning (adaptive ltering, Mehra 1972). In many practical cases consistency is assured by taking the added uncertainty too large, e.g. by taking a constant Q over time which compensates for decreasing linearization errors. This, however, results in less informative estimates. 4.4. Examples The dierent process updates are illustrated by a simple 2D non-linear process model (xi denotes the ith element of x) ) xk 1 xk1 12 22 xk 2 xk1 1 3xk1 2 with no process uncertainty: qp, k1  02 1 . xk 1 depends non-linearly on xk1 . The process update of xk 2 is linear. The updated state estimate and its uncertainty at time step k 1 are " # " # 10 36 0 ^ ; Pk1jk1 : 23 xk1jk1 15 0 3600

4.4.1. Monte Carlo simulation. The mean value and ^ the covariance of the true pdf pxk jZk1 are calculated with a (computationally expensive) Monte Carlo ^ simulation based on a Gaussian pdf pxk1 jZ k1 with ^ mean xk1jk1 and covariance matrix Pk1jk1 . The results of this computation are used to illustrate the (in) consistency and information content of the state estimates of the dierent KF variants. The mean and ^ covariance matrix of the pxk jZ k1 calculated by the Monte Carlo algorithm are " # " # 136 16 994 721 ^ xkjk1 ; Pkjk1 : 24 55 721 32 436 4.4.2. (I)EKF. Figure 2 shows the updated and (I)EKF predicted state estimates and their uncertainty ellipsesy. The dotted line is the uncertainty ellipse of the distribution obtained by Monte Carlo simulation. The IEKF
y The uncertainty ellipsoid ^ ^ xk xkji T P1 xk xkji 1 kji

25

is a graphical representation of the uncertainity on the state ^ ^ estimate xkji . Starting from the point xkji , the distance to the ^ ellipse in a direction is a measure for the uncertainty on xkji in that direction.

Kalman lters for non-linear systems

645

Downloaded by [University of South Florida] at 14:21 21 September 2011

Figure 3. Non-linear process model. Uncertainty ellipses for the updated state estimate at k 1 (dashed line, top gure), for the LRKF predicted state estimate (full line, bottom gure), and Monte Carlo uncertainty ellipse (dotted line which coincides with the full line, bottom gure). The LRKF predicted state estimate is consistent and informative: its uncertainty ellipse coincides with the Monte Carlo uncertainty ellipse.

state prediction and its covariance matrix are ! ! 100 14 400 720 ^ xkjk1 ; Pkjk1 : 26 55 720 32 436 Due to the neglected linearization errors, the state estimate is inconsistent: the covariance Pkjk1 is smaller than the covariance calculated by the Monte Carlo simulation for the rst state component x1 which

had a non-linear process update. For consistent results this covariance should even be larger because the IEKF ^ estimate xkjk1 diers from the mean of the pdf ^ k1 , calculated by the Monte Carlo simulation. pxk jZ 4.4.3. LRKF. Figure 3 shows the X jk1jk1 points (top gure) and the X jkjk1 points (bottom gure), the updated state estimate (top) and the predicted state

646
Hk  @hk  

T. Lefebvre et al.
dk ^ hk ^kjk1 H k xkjk1 x ^ hk ^kjk H k xkjk x minH, d Pr
T j1 ej ej 1 r

R k 0mm 0mm Pr
T j1 ej ej

EKF IEKF LRKF

@x xkjk1 ^ @hk  @x xkjk ^

minH, d

Pr

T j1 ej ej

Table 2. Summary of the linearization of the measurement model by the extended Kalman lter (EKF), the iterated extended Kalman lter (IEKF) and the linear regression Kalman lter (LRKF).

Downloaded by [University of South Florida] at 14:21 21 September 2011

estimate (bottom) and their uncertainty ellipses for the LRKF. The X jk1jk1 points are chosen with the UKF algorithm of Julier and Uhlmann (1996) where  3 n 1. This corresponds to choosing six ^ regression points, including two times the point xk1jk1 . The uncertainty ellipse obtained by Monte Carlo simulation coincides with the nal uncertainty ellipse of the LRKF predicted state estimate (bottom gure). This indicates consistent and informative results. The LRKF predicted state estimate and its covariance matrix are ! ! 136 16 992 720 ^ xkjk1 ; Pkjk1 : 27 55 720 32 436 4.5. Conclusion: the process update The LRKF performs better than the (I)EKF when dealing with non-linear process functions: 1. The LRKF linearizes the function based on its behaviour in the uncertainty region around the updated state estimate. The (I)EKF on the other hand only uses the function evaluation and its Jacobian in this state estimate. 2. The LRKF deals with linearization errors in a theoretically founded way (provided that enough regression points are chosen). The (I)EKF on the other hand needs trial and error for each particular example to obtain good values for the covariance matrix representing the linearization errors. 3. Unlike the (I)EKF, the LRKF does not need the function Jacobian. This is an advantage where this Jacobian is dicult to obtain or non-existing (e.g. for discontinuous process functions).

The linearization of the measurement model by the IEKF (} 5.2) takes the measurement into account; the EKF (} 5.1) and LRKF (} 5.3) linearize the measurement model based only on the predicted state estimate and its uncertainty. For the latter lters, the linearization errors (R ) are larger, especially when the measurek ment function is quite non-linear in the uncertainty region around the predicted state estimate. A large uncertainty on the linearized measurement model R k E k Rk E T (due to a large uncertainty on the state estik mate) results in throwing away the greater part of the information of the possibly very accurate measurement. The dierent linearization formulas are summarized in table 2. Section 5.5 presents some examples. 5.1. The extended Kalman lter The EKF linearizes the measurement model around ^ the predicted state estimate xkjk1  @h  Hk k 28 @x xkjk1 ^ ^ ^ d k hk xkjk1 H k xkjk1 : 29

The basic EKF algorithm does not take the linearization errors into account R  0mm k 30

5. Non-linear measurement models The previous section contains a comparison between the (I)EKF and LRKF process updates; this section focuses on their measurement updates for a non-linear measurement model (11) with linearization (13). The EKF, IEKF and LRKF choose H k , d k and R in a k dierent way. After linearization they use the KF update equations (5)(9).

where m is the dimension of the measurement vector zk . If the measurement model is non-linear in the uncertainty region around the predicted state estimate, the linearization errors are not negligible. This means that the linearized measurement model does not reect the relation between the true state value and the measurement. For instance, the true state value is far fromy the linearized measurement model. After processing the measured value, given the linear measurement model and the measurement uncertainty, the state is believed
y Far from (and close to) must be understood as: the deviation of the true state with respect to the linearized measurement model is not justied (is justied) by the ~k ~ ~ k measurement uncertainty R Ek Rk ET .

Kalman lters for non-linear systems to be in a region which does not include the true state estimate, i.e. the updated state estimate is inconsistent. 5.2. The iterated extended Kalman lter The EKF of the previous section linearizes the measurement model around the predicted state estimate. The IEKF tries to do better by linearizing the measurement model around the updated state estimate  @hk   Hk 31 @x xkjk ^ ^ ^ d k hk xkjk H k xkjk : 32

647

are chosen such that their mean and covariance matrix ^ are equal to the predicted state estimate xkjk1 and its covariance Pkjk1 . The CDF, DD1 and UKF lters correspond to specic choices. The function values of the regression points through the non-linear function are Z jk hk X jkjk1 : 34

Downloaded by [University of South Florida] at 14:21 21 September 2011

This is achieved by iteration: the lter rst linearizes ^ kjk the model around a value x0 (often taken equal to ^ the predicted state estimate xkjk1 ) and calculates the updated state estimate. Then, the lter linearizes the ^ kjk model around the newly obtained estimate x1 and cal^ culates a new updated state estimate (based on xkjk1 , Pkjk1 and the new linearized model). This process ^ is iterated until a state estimate xikjk is found for which ^ kjk ^ ^ xikjk is close to xi1 . The state estimate xkjk and uncertainty Pkjk are calculated starting from the state estimate ^ xkjk1 with its uncertainty Pkjk1 and the measurement ^ model linearized around xikjk . Like the EKF algorithm, the basic IEKF algorithm does not take the linearization errors into account R  0mm : k 33

The LRKF algorithm uses a linearized measurement function (13) where H k , d k and R are obtained by k statistical linear regression through the points X jkjk1 , Z jk , j 1, . . . , r. The statistical linear regression is such that the deviations ej between the non-linear and the linearized function in the regression points are minimized in least-squares sense   35 ej Z jk HX jkjk1 d H k , d k arg min
r X j1 H, d

eT e j : j

36

The sample covariance matrix of the deviations ej gives an idea of the magnitude of the linearization errors r 1X T ee : 37 R k r j1 j j Intuitively we feel that when enough regression points (X jkjk1 , Z jk ) are taken the state estimates of the LRKF measurement update are consistent because R gives a well-founded approximation of the linearizak tion errors (equation (37)). However, if the measurement model is highly non-linear in the uncertainty region ^ around xkjk1 , the (X jkjk1 , Z jk ) points deviate substantially from a hyperplane. This results in a large R and k non-informative updated state estimates (see the example in } 5.5). 5.4. Extra measurement uncertainty In order to make the state estimates consistent, the user can tune an inconsistent lter by adding extra measurement uncertainty R . k Only o-line tuning or on-line parameter learning can lead to a good value for R for a particular probk lem. In many practical cases consistency is assured by choosing the added uncertainty too large, e.g. by taking a constant R over time which compensates for decreasing linearization errors. This reduces the information content of the results. 5.5. Examples

If the measurement model is non-linear in the uncer^ tainty region around the updated state estimate xkjk , state estimates will be inconsistent. In case of a measurement model that instantaneously fully observes the state (or at least the part of the state that causes the non-linearities in the measurement model), the linearization errors will be smally in the uncertainty region ^ around xkjk . The true state estimate is then close to the linearized measurement function and the updated state estimate is consistent. The result is also informative because no uncertainty due to linearization errors needs to be added. 5.3. The linear regression Kalman lter The LRKF evaluates the measurement function in r regression points X jkjk1 in the uncertainty region ^ around the predicted state estimate xkjk1 . The X jkjk1

^ y This assumes that the iterations lead to an accurate xikjk . The linearizations are started around a freely choosen In order to assure quick and correct iteration, (part of) this value can be chosen based on the measurement information if this information is more accurate than the predicted state estimate. ^ kjk x0 .

5.5.1. First example. The comparison between the different measurement updates is illustrated with the measurement function zk h1 xk m, k h1 xk xk 12 xk 22 38

648 where xk is the true value and ^ xkjk1 " #

T. Lefebvre et al. 5.5.3. EKF. Figure 4 shows the state estimates, uncertainty ellipses and measurement functions for the EKF applied to the rst example (equation (38)). The true measurement function is non-linear. xk is the true value of the state, and is close to this function. The linearization around the uncertain predicted state estimate is not a good approximation of the function around the true state value: the true state value is far from the linearized measurement function. The resulting updated state estimate " # " # 10 36 24 ^ xkjk ; Pkjk 42 25 24 16 is inconsistent. 5.5.4. IEKF. Figure 5 shows the measurement function, the linearized measurement function around the point ^ xikjk , the true state value xk and the state estimates for the IEKF applied to the rst example (equation (38)). The measurement model does not fully observe the state. ^ This results in an uncertain updated state estimate xikjk around which the lter linearizes the measurement function. As was the case for the EKF, the linearization errors are not negligible and the true value is far from the linearized measurement function. The updated state estimate " # " # 10 36 16 ^ xkjk ; Pkjk 43 23 16 7:0 is inconsistent. If however the measurement model fully observes the state, the IEKF updated state estimate is accurately known; hence, the linearization errors are small and the true state value is close to the linearized measurement function. In this case, the updated state estimate is consistent. Figure 6 shows the measurement function, the linearized measurement function, the true state value xk , the state estimates and the uncertainty ellipses for the IEKF applied to the second example (equations (39) and (40)). The updated state estimate and covariance matrix " # " # 14 2:6 1:7 ^ xkjk ; Pkjk 44 21 1:7 1:3 are consistent and informative due to the small, ignored, linearization errors. 5.5.5. LRKF. An LRKF is run on the rst example (equation (38)). The X jkjk1 points are chosen with the UKF algorithm with  3 n 1 (Julier and Uhlmann 1996). This corresponds to choosing six regression ^ points, including two times the point xkjk1 . Figures 7 and 8 show the non-linear measurement function, the X jkjk1 -points and the linearization. The predicted

15 20 "

10 15

is the predicted state estimate with covariance matrix " # 36 0 Pkjk1 : 0 3600 ^ The processed measurement is zk 630 and the measurement covariance is Rk 400.

Downloaded by [University of South Florida] at 14:21 21 September 2011

5.5.2. Second example. To illustrate the consistency of the state estimate of an IEKF when the measurement observes the state completely, a second example is used. The measurement function is " # h1 xk qm, k 1 zk hxk qm, k 39 h2 xk qm, k 2 with h1 xk xk 12 xk 22 h2 xk 3xk 22 =xk 1 where xk is the true value and ^ xkjk1 " # ) 40

15 20 "

10 15

is the predicted state estimate with covariance matrix " # 36 0 : Pkjk1 0 3600 The processed measurement and the measurement covariance matrix are " # " # 630 400 0 ^ zk ; Rk : 41 85 0 400 In all gures, the true state value xk is plotted; if this value is far outside the uncertainty ellipse of a state estimate, the corresponding estimate is inconsistent. Because the measurement is accurate and the initial estimate is not, the uncertainty on the state estimate should drop considerably when the measurement is processed. The updated state estimate is not informative if this is not the case.

Kalman lters for non-linear systems

649

Downloaded by [University of South Florida] at 14:21 21 September 2011

Figure 4.

^ ^ Non-linear measurement model z h1 xk and EKF linearization around xkjk1 (dotted lines). The true state xk is far ^ from this linearization and the obtained state estimate xkjk (uncertainty ellipse in full line) is inconsistent.

^ ^ Figure 5. Non-linear measurement model z h1 xk that does not observe the full state, and its IEKF linearization around xkjk ^ (dotted lines). The true state xk is far from this linearization, leading to an inconsistent state estimate xkjk (uncertainty ellipse in full line).

650

T. Lefebvre et al.

Downloaded by [University of South Florida] at 14:21 21 September 2011

^ Figure 6. Non-linear measurement model z hxk that fully observes the state, and its IEKF linearization around xkjk (dotted ^ lines). The true state xk is close to this linearization, leading to a consistent state estimate (uncertainty ellipse in full line).

Figure 7.

^ Non-linear measurement model z h1 x and LRKF linearization. The linearization errors are large.

Kalman lters for non-linear systems

651

Downloaded by [University of South Florida] at 14:21 21 September 2011

Figure 8.

Figure 7 seen from another angle.

^ Figure 9. Non-linear measurement model z h1 x and LRKF linearization (dotted lines). The large linearization errors result in a large measurement uncertainty R E k Rk E T . The updated state estimate (uncertainty ellipse in full line) is consistent k k but non-informative.

state estimate is uncertain, hence the X jkjk1 -points are widespread. R is large (R 2:6 107 ) due to the large k k deviations between the (X jkjk1 , Z jk ) points and the linearized measurement function (see gure 8). The updated state estimate and its covariance matrix are " ^ xkjk 10 2:6 # ; Pkjk " 36 0 0 3600 # : 45

Figure 9 shows the X jkjk1 points, the measurement function, the LRKF linearized measurement function, the true state value xk , the state estimates and the uncertainty ellipses. The updated state estimate is consistent. However, it can hardly be called an improvement over the previous state estimate (Pkjk % Pkjk1 ). The information in the measurement is neglected due to the high measurement uncertainty R E k Rk E T k k on the linearized function.

652

T. Lefebvre et al. 2. The analysis claries how some lters automatically adapt their compensation for linearization errors, while other lters have constant (developer-determined) error compensation. 3. The quality of the state estimates is expressed by two criteria: the consistency and the information content of the estimates. This paper relates the consistency and information content of the estimates to (i) how the linearization is performed and (ii) how the linearization errors are taken into account. The understanding of the linearization processes allows us to make a well-founded choice of lter for a specic application. 4. The performance of the dierent lters is compared for the process and measurement updates separately, because a good performance for one of these updates does not necessarily mean a good performance for the other update. This makes it interesting in some cases to use dierent lters for both updates. For process updates the LRKF performs better than the other mentioned KF variants because (i) the LRKF linearizes the process model based on its behaviour in the uncertainty region around the updated state estimate. The (I)EKF on the other hand only uses the function evaluation and its Jacobian in this state estimate; and (ii) the LRKF deals with linearization errors in a theoretically founded way, provided that enough regression points are chosen. The (I)EKF on the other hand needs trial and error for each particular application to obtain a good covariance matrix representing the linearization errors. The IEKF is the best way to handle non-linear measurement models that fully observe the part of the state that makes the measurement model non-linear. In the other cases, none of the presented lters outperforms the others: the LRKF makes an estimation of the linearization errors; the EKF and IEKF on the other hand require extensive o-line tuning or on-line parameter learning in order to yield consistent state estimates. However, unlike the EKF and LRKF, the IEKF additionally uses the measurement value in order to linearize the measurement model. Hence, its linearization errors are smaller and once a well-tuned IEKF is available, the state estimates it returns can be far more informative than those of the LRKF or a well-tuned EKF. The insights described in this paper are important for all researchers and developers who want to apply a KF variant to a specic application. They lead to a systematic choice of a lter, where previously the choice

Note that some kind of iterated LRKF (similar to the iterated EKF) would not solve this problem: the ^ updated state estimate xkjk and its covariance matrix Pkjk are more or less the same as the predicted state ^ estimate xkjk1 and its covariance matrix Pkjk1 . Hence, the regression points and the linearization would be approximately the same after iteration. 5.6. Conclusion: the measurement update Measurements which fully observe the part of the state that makes the model non-linear, are best processed by the IEKF. In this case (and assuming that the algorithm iterates to a good linearization pointy), the IEKF linearization errors are negligible. In the other cases, none of the presented lters outperforms the others. A lter should be chosen for each specic application: the LRKF makes an estimate of its linearization errors (R ), the EKF and IEKF on the k other hand require o-line tuning or on-line parameter learning of R to yield consistent state estimates. k Because the IEKF additionally takes the measurement into account when linearizing the measurement model, its linearization errors are smaller than those of the EKF and LRKF. This means that once a well-tuned IEKF is available, the state estimates it returns can be far more informative than those of the LRKF or a well-tuned EKF. Finally, note that the LRKF does not use the Jacobian of the measurement function, which makes it possible to process discontinuous measurement functions.

Downloaded by [University of South Florida] at 14:21 21 September 2011

6. Conclusions This paper gives insight into the advantages and drawbacks of the extended Kalman lter (EKF), the iterated extended Kalman lter (IEKF) and the linear regression Kalman lter (LRKF). These insights are a result of the distinct analysis approach taken in this paper: 1. The paper describes all lter algorithms as the application of the basic KF algorithm to linearized process and measurement models. The dierence between the KF variants is situated in the choice of linearization and the compensation for the linearization errors. In previous work this linearization was not always recognized, e.g. the UKF is originally derived as a lter which does not linearize the models.
^ y This assumes that the iterations lead to an accurate xikjk . ^ kjk The linearizations are started around a freely choosen x0 . In order to assure quick and correct iteration, (part of) this value can be chosen based on the measurement information if this information is more accurate than the predicted state estimate.

Kalman lters for non-linear systems was mainly made based on success in similar applications or based on trial and error. Further work should report on practical applications using these insights and an eort should be made to analyse and include future KF algorithms in this framework. Acknowledgements T. Lefebvre is a Postdoctoral Fellow of the Fund for Scientic ResearchFlanders (FWO) in Belgium. Financial support by the Belgian Programme on Inter-University Attraction Poles initiated by the Belgian StatePrime Ministers OceScience Policy Programme (IUAP), and by K. U. Leuvens Concerted Research Action GOA/99/04 are gratefully acknowledged. References
Bar-Shalom, Y., and Li, X., 1993, Estimation and Tracking: Principles, Techniques and Software (Boston, London: Artech House). Bayes, T., 1763, An essay towards solving a problem in the doctrine of chances. Philosophical Transactions of the Royal Society of London, 53, 370418. Reprinted in Biometrika, 45, 293315, 1958. Gelb, A., Kasper, J., Nash, R., Price, C., and Sutherland, A., 1974, Applied Optimal Estimation (Cambridge, MA: MIT Press). Julier, S., and Uhlmann, J., 1996, A general method for approximating nonlinear transformations of probability distributions. Technical report, Robotics Research Group, Department of Engineering Science, University of Oxford. Julier, S., and Uhlmann, J., 2001, Data fusion in non-linear systems. In D. Hall and J. Llinas (Eds) Handbook of Multisensor Data Fusion (New York: CRC Press), Chapter 13.

653

Downloaded by [University of South Florida] at 14:21 21 September 2011

Julier, S., Uhlmann, J., and Durrant-Whyte, H., 2000, A new method for the transformation of means and covariances in lters and estimators. IEEE Transactions on Automatic Control, 45(3), 477482. Kalman, R. E., 1960, A new approach to linear ltering and prediction problems. Transactions of the ASME, Journal of Basic Engineering, 82, 3445. Laplace, P.-S., 1812, Theorie Analytique des Probabilites (Paris: Courcier Imprimeur). Lefebvre, T., Bruyninckx, H., and De Schutter, J., 2002, Comment on A new method for the nonlinear transformation of means and covariances in lters and estimators. IEEE Transactions on Automatic Control, 47(8), 14061408. Maybeck, P., 1982, Stochastic Models, Estimation, and Control, Volume 2 (New York: Academic Press). Mehra, R., 1972, Approaches to adaptive ltering. IEEE Transactions on Automatic Control, 17(5), 693698. Nrgaard, M., Poulsen, N., and Ravn, O., 2000 a, Advances in derivative-free state estimation for nonlinear systems. Technical Report IMM-REP-1998-15 (revised edition), Technical University of Denmark, Denmark. Nrgaard, M., Poulsen, N., and Ravn, O., 2000 b, New developments in state estimations for nonlinear systems. Automatica, 36(11), 16271638. Schei, T., 1997, A nite-dierence method for linearisation in nonlinear estimation algorithms. Automatica, 33(11), 20532058. Sorenson, H. W., 1985, Kalman Filtering: Theory and Application (New York, NY: IEEE Press). Tanizaki, H., 1996, Nonlinear Filters: Estimation and ApplicationsSecond, Revised and Enlarged Edition (Berlin-Heidelberg: Springer-Verlag). Uhlmann, J., 1995, Dynamic map building and localization: new theoretical foundations. PhD thesis, University of Oxford, UK. Willsky, A., 1976, A survey of design methods for failure detection in dynamic systems. Automatica, 12, 601611.