{'fraction_non_alphanumeric': 0.03989078032339263, 'fraction_numerical': 0.011092623405435386, 'mean_word_length': 4.298372513562387, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Performance evaluation of iterated extended Kalman filter with variable step-length

The paper deals with state estimation of nonlinear stochastic dynamic systems. In particular, the iterated extended Kalman filter is studied. Three recently proposed iterated extended Kalman filter algorithms are analyzed in terms of their performance and specification of a user design parameter, more specifically the step-length size. The performance is compared using the root mean square error evaluating the state estimate and the noncredibility index assessing covariance matrix of the estimate error. The performance and influence of the design parameter, are analyzed in a numerical simulation.


Introduction
Nonlinear state estimation of discrete-time stochastic dynamic systems is an important field of study, which has undergone a rapid development especially in the last two decades. The importance of this field stems from its crucial role in many areas such as signal processing, target tracking, satellite navigation, fault detection, and adaptive and optimal control problems. It constitutes an essential part of any decisionmaking process. The state estimation methods are mostly based on the Bayesian approach or on the optimization approach.
Application of the Bayesian approach to the recursive state estimation problem leads to the Bayesian recursive relations (BRRs). The BRRs provide probability density functions (PDFs) of the state conditioned by the measurements. The conditional PDFs represent a full description of the state. A closed-form solution to the BRRs is available only for a few special cases such as a linear Gaussian system [1]. In other cases, an approximate method must be used. The approximate methods include the particle filtering [2,3], the point-mass method [4], and the Gaussian sum method [5].
The optimization approach is largely based on minimizing the mean square error criterion and for linear Gaussian systems leads to the celebrated Kalman filter. As opposed to the Bayesian approach providing the conditional PDF of the state, the optimization approach provides a point estimate of the state and corresponding covariance matrix (CM) of the estimate error. An analytical solution to the state estimation problem based on the optimization approach can be found in a few special cases only, as is the case of the Bayesian approach. For other cases, the estimation methods usually follow the Kalman filter framework and utilize some approximation techniques such as linearization of the nonlinear functions. For example, the extended (EKF) and the second-order extended Kalman filters [6], [1] approximate the nonlinear functions by the Taylor series expansion around the current estimate up to the first or second order, respectively.
When the effects of the linearization errors tend to disrupt performance of the filter or its convergence, the relinearizing of the measurement equation around the updated state may alleviate the difficulties. This method is known as the iterated extended Kalman filter (IEKF) [6]. The IEKF computes the state estimates not as an approximate conditional mean (as the EKF does), but as a maximum a posteriori (MAP) estimate [7]. In [8] it was shown that the IEKF measurement update is an application of the Gauss-Newton (GN) method and the EKF is a special case of the IEKF with only one GN method iteration. In [9] a new technique improving the EKF and the IEKF has been proposed based on a control of the step-length.
The paper presented a derivation of three alternative algorithms of the IEKF with the step-control, but a comprehensive performance analysis was not given. However, a complex performance analysis evaluating not only the state estimates, but also the CMs of the estimate error provided by filters is known to be important [10]. The reason is that the CM represents a self-assessment of the filter and as such it is used in the next filtering step to compute the estimate. Therefore, the goal of this paper is to deliver a comprehensive performance analysis of the filters proposed in [9] in terms of performance metrics utilizing the estimate error and the CM. More specifically, the root mean square error (RMSE), its distance from the Cramer-Rao lower bound (CRLB) [11,3] and the noncredibility index (NCI) [12] will be analyzed. The comparison of the new filters with the EKF and the IEKF using the above mentioned performance metrics should provide better understanding of their capabilities.
The paper is organized as follows: Section 2 is devoted to a brief introduction to the nonlinear state estimation and its solution by the EKF and the IEKF. The recent development of the IEKFs is briefly presented in Section 3. Section 4 presents various performance metrics used in the analysis. Next, in Section 5 the filters are compared in two numerical examples; and the paper is concluded by Section 6.
2. Nonlinear state estimation by EKF and IEKF 2.1. System description Let a discrete-time nonlinear stochastic system be considered in the following state-space form where the vectors x k ∈ R n x , and z k ∈ R n z represent the state of the system, and the measurement at time instant k, respectively, f k : R n x → R n x and h k : R n x → R n z are known vector functions, and w k ∈ R n x , v k ∈ R n z are mutually independent state and measurement white noises. The PDFs of the noises are Gaussian with zero means and known CMs w k and v k , respectively, i.e., p(w k ) = N {w k ; 0 n x ×1 , w k } * and p(v k ) = N {v k ; 0 n z ×1 , v k }, respectively, where 0 a×b denotes an a×b matrix of zeros. The PDF of the initial state is Gaussian and known as well, i.e., p(x 0 ) = N {x 0 ;x 0 , P 0 }. The initial state is independent of the noises.

Extended Kalman filter
Originally, the Kalman filter was derived using the orthogonality principle [13] in 1960. It yields the minimum mean square error estimate for the system (1) and (2) with linear functions f k and h k . For nonlinear functions f k and h k an approximation has to be used, such as that used in the EKF. The EKF is based on the first order Taylor series expansion (TE1). The TE1 of h k under the assumption of known state predictive meanx k|k−1 = E[x k |z k−1 ] (defining the linearization point) is given by where is the Jacobian matrix of h k (·) evaluated atx k|k−1 . Use of the TE1 approximation for the prediction update step is analogous. The TE1 of f k in (1) under assumption of known filtering meanx k|k = E[x k |z k ] is of the form where F k = ∂f(x k ) ∂x k x k =x k|k is the Jacobian matrix of f k (·) evaluated atx k|k . Approximations (3) and (4) are used in the algorithm of the EKF (see Algorithm 1).

Algorithm 1: Extended Kalman filter
Step 1: (initialization) Set the step k = 0 and define a priori initial condition by its first two momentŝ Step 2: (filtering, measurement update) The filtering meanx k|k and the CM of the estimate error are computed by means of where is the filter gain.
Step 3: (prediction, time update) The predictive meanx k+1|k and the CM of the estimate error Let k = k + 1. The algorithm then continues by Step 2.
The EKF performs well for systems with mildly nonlinear functions f k and h k , but if the measurement equation (2) is strongly nonlinear (such as in bearings-only tracking problems), the performance of the filter deteriorates. In such situations, the IEKF tends to provide more accurate estimates than the EKF.

Iterated extended Kalman filter
The idea of the IEKF [6] is to improve a reference trajectory, and consequently the estimate, in the presence of significant nonlinearities. Those improvements are achieved by local iterations of the EKF measurement update (see Algorithm 2). The iterations are usually stopped when there is no significant change in consecutive iterations or other criteria such as a maximum number of iterations are met.
Step 2: (measurement update iterations) Compute the Jacobi matrix at the best state estimate available, the Kalman gain, and the next iteration of the state estimate as Repeat Step 2 with i = i + 1 until a stopping criterion is met †. † The stopping-condition is usually represented by a maximum number of iterations or a negligible change between two consecutive iterations, e.g., where is a pre-specified threshold. Step 3: (updating the state and CM) Save the last iteration as the new filtering mean and compute the CM based on the last iteration, i.e.,x It is important to mention, that even the relinearization occurring in the IEKF algorithm does not ensure convergence of the filter nor the IEKF generally outperforms the EKF. Nevertheless, the IEKF measurement update possesses two very interesting properties: • It can be viewed as an application of the Gauss-Newton method [8].
• It generates the MAP estimatex MAP k|k = arg max

Improvements of the iterated extended Kalman filter
As has been mentioned above, the IEKF can be seen as an application of the GN method used for solving the nonlinear least square problem. Hence, improvements of the GN method enhancing performance of the method and its convergence can be used in the IEKF to improve quality of the estimates. Changing length of a step is one of the frequently used improvements. First, the MAP criterion, which is minimized by the IEKF, is introduced.

The MAP criterion
If a probabilistic description of the system (1) and (2) is used, the posterior PDF p(x k |z k ) is sought using prior PDF p(x k |z k−1 ). The meanx k|k−1 and the CM P k|k−1 computed in the time update of Algorithm 1 correspond to the prior PDF p(x k |z k−1 ), which is assumed to be Gaussian, and the meanx k|k and the CM P k|k computed in the measurement update of Algorithm 2 correspond to the posterior PDF p(x k |z k ). The posterior PDF p(x k |z k ) is proportional to the product of the likelihood p(z k |x k ) = p v k (z k − h k (x k )) and the prior PDF p(x k |z k−1 ) where the terms not depending on x k have been dropped for convenient purposes. The MAP estimate is then given bŷ Now, having the MAP criterion specified using (18), the GN method can be used to findx k|k minimizing the criterion. Ideally, each GN method iteration (i.e. the IEKF measurement update iteration) should decrease the criterion V k . The IEKF measurement update (16) can be rewritten in an implicit manner where the shift between the iterations i k is given by andx 0 k =x k|k−1 . The relations (19) and (20) then match the relations of the GN method minimizing the MAP criterion (18).

IEKF with variable step-length
Ideally, the value of the criterion V k should decrease with each iteration, but due to nonlinearity of h k this behavior cannot be guaranteed. Thus, it is beneficial to check whether the criterion is decreasing, i.e., or rather to ensure it. This is done by introducing a variable step-length. The parameter α i ∈ (0, 1 is used to govern a step-length for a fixed direction i k given by (20) in the following manner where the notationx L,i k was introduced to distinguish betweenx i k for a full step given by (19). There are several ways of determining the parameter α i [9] to ensure the condition (21), but the exact line search given by provides the best results at the cots of increased computational demands. The line search IEKF (IEKF-L) measurement update is then The IEKF-L algorithm is then given by substituting (14) by (24).

Quasi-Newton IEKF
Another approach improving the GN method is the Quasi-Newton technique (QN). The GN method approximates second order terms (Hessians) by first order terms (Jacobians) [14]. The QN attempts to compensate this approximation by introducing a correction term. The IEKF-QN equations, which were derived in [9], are as follows: and the matrix T i k is implicitly built usinĝ T i k = min 1, where with initial condition T 0 k = 0 nx×nx . The IEKF-QN algorithm is then given by substituting (14) and (13)

Levenberg-Marquardt IEKF
The Levenberg-Marquardt (LM) method [15] (also known as the trust region method) adds a damping parameter µ i to the GN method. The damping parameter controls interpolation between the GN and the steepest-descent. For a small µ i the LM method behaves like the GN method and for a large µ i like the steepest-descent method. The IEKF-LM relations are given bŷ The IEKF-LM algorithm is then given substituting (14) and (13) by  (35-38).
There are two parameters in the IEKF-LM algorithm, which have to be set. The α i is again to be found using the exact line search (23) because it influences the results most. The selection of the damping parameter µ i is rather tricky, because it influences the step-length too. Some discussion on this topic can be found in [9]. On the other hand, in our simulations an adaptation of both parameters was not observed to be of large effect, thus damping parameter was set constant.

Performance metrics
Behavior of a newly developed filter is frequently studied using benchmark problems where it is compared with the standard filters. As far as the criterion for the comparison is concerned, most authors rely only on the RMSE, which is related to the design criterion of most filters (minimum mean square error). However, the RMSE only evaluates quality of the first moment provided by the filter, the conditional mean of the state. In this paper besides the state estimatex k|k also comparison of the CM of the estimate error will be considered. The quality of the state estimate error CM provided by the filters will be compared using the NCI [12] as the credibility measure. Further, the RMSE of the state estimates will be supplemented with the CRLB [11], which represents an objective limit of cognizability of the system.

Root mean square error
The RMSE for the state estimatex k|k at time instant k calculated using M Monte Carlo simulations is defined as wherex k|k (i) = x k (i) −x k|k (i) is the estimate error andx k|k (i) is the estimate of x k (i) at the i-th Monte Carlo simulation.

Cramer-Rao lower bound
The filtering CRLB denoted as C k|k is an objective limit of cognizability of the system state, since it is a lower bound for the mean square error matrix (MSEM) k|k = E[x k|kx T k|k ] produced by a filter, i.e., Note that the inequality in (40) means that k|k − C k|k is positive definite. The MSEM k|k can be approximated using M Monte Carlo simulations by The comparison of the MSEM with the CRLB is beneficial in nonlinear filtering to show effects of approximations and linearization errors. For the system given by (1) and (2) the filtering CRLB [11] can be recursively computed by where the Jacobi matrices are functions of the true state x k and the initial condition is C 0|−1 = P 0|−1 . It shall be noticed that the relations (42) and (43) are formally identical to those of the CM computation in the EKF (8) and (11). The only difference is that the Jacobi matrices in the CRLB are computed at the true state and in the EKF in the state estimate. The CRLB will later help to determine how significant the approximation errors are. The MSEM and CRLB are often compared using the matrix trace, because the square root of MSEM trace is the RMSE, i.e.,

Noncredibility index
The NCI [12] compares the normalized estimation error squared (NEES) [7] of the estimator defined as k|k (i) =x T k|k (i)P −1 k|k (i)x k|k (i) with the NEES of a perfectly credible estimator given by * where k|k is the MSEM computed by (41). Finally, the NCI is defined as The NCI is capable of determining whether the estimator is credible, i.e., whether the CM provided by the estimator is close to the MSEM k|k . The closer it is, the smaller the NCI value is, i.e., the NCI of a perfectly credible estimator is zero. The NCI is tightly coupled with the inclination index (II), which is defined in the same way as the NCI with the only exception of omitting the absolute value, i.e., The II specifies whether the noncredibility of a filter is rather of the optimistic or pessimistic nature., i.e., whether the CM of the filter is smaller or larger than the MSEM, respectively.

Numerical illustrations
The analysis of the IEKFs with a variable step-length will be performed in this section. After specification of considered models, the appropriate setting of the step-length parameter will be discussed. Finally, the comprehensive analysis involving the RMSE, the CRLB, and the NCI will be given.

Specification of benchmark models
All numerical illustrations will be performed within the scope of two examples: the univariate nonstationary growth model [16] and the bearings-only tracking model [9]. Both examples utilize highly nonlinear measurement functions, where the IEKFs should outperform non-iterative Kalman filters. The Univariate non-stationary growth model (UNGM) is specific for its high degree of nonlinearity and bi-modality of the measurement, which causes difficulties for filters. The model is specified as where p(w k ) ∼ N {0, 1}, p(v k ) ∼ N {0, 1} and the initial condition is p(x 0 ) ∼ N {0.1, 1}. All simulations were started with x 0 = 0.1. The initial condition is thus set within a sensitivity region, which places high demands on the estimators. The Bearings only tracking (BOT) is represented by the motion model with measurements from two static radars providing bearing only. The state x k = [x 1,k , x 2,k ] T represents position of a tracked object in the Cartesian coordinates. The model is given by

5.2.
Step-length choice discussion For the recently proposed filters (the IEKF-L, the IEKF-QN, and the IEKF-LM) the parameter α has to be chosen at every iteration of each measurement update. The paper [9], where the filters were derived, contains a discussion about the step-length parameter. Nevertheless, in the simulations given in [9] only a fixed step-length α i = 0.5 was used. Hence, prior to the performance analysis of the IEKFs the attention will be focused on the step-length parameter setting itself as it is a prerequisite for the IEKFs.
First, a measurement update at time instant k = 0 in the UNGM example (denoted as UNGM-0) will be examined. Suppose the predictive statistics arex 0|−1 = 0.1, P 0|−1 = 1 and the measurement is z 0 = 1.3. The examined filters are: the EKF, the IEKF (α i = 1), the filters discussed in Section 3 with a fixed step-length α i = 0.5 denoted as IEKF-L 0.5, IEKF-QN 0.5, and IEKF-LM 0.5 and the filters discussed in Section 3 with exact line search, i.e. with α i optimized, denoted as IEKF-L, IEKF-QN, and IEKF-LM. The filters IEKF-LM and IEKF-LM 0.5 need setting of the dumping parameter µ i . A discussion can be found in [9], but to alleviate simultaneous optimization of two parameters, a fixed value µ i = 0.01 is set in this paper ‡. The state estimate and the MAP criterion evolutions over iterations for ‡ To find a suboptimal value of µ i , a small test was performed with following results: In case of the UNGM example, the best results appear for µ i ∈ [0, 0.01] whilst for µ i > 0.01 the results (time, RMSE and NCI) deteriorate linearly. In case of the BOT example, the minimum of the RMSE and the NCI is reached for µ i ∈ [0.01, 0.1].  Figures 1 and 2, respectively. In this particular example all filters except the EKF finally converge to the minimal criterion value and the only difference is a speed of convergence. The IEKF and the optimized variable step-length filters are superior to the IEKFs with a fixed step-length.  Second, a measurement update at time instant k = 1 will be examined in the UNGM example (denoted as UNGM-1). In the considered simulation the value of the predictive statistics were obtained asx 1|0 = 3.9, P 1|0 = 604 and measurement was z 1 = −0.73. The examined filters are the same as in the previous example, but their behavior is completely different. The results (evolution of the state estimate, the state error variance, the value of α i and the value of the MAP criterion) are depicted in Figures 3-6. An interesting finding is the fact, that only the IEKFs with the exact line search and the IEKF-QN 0.5 converge. The EKF, the IEKF, the IEKF-L 0.5 and the IEKF-LM 0.5 fail to converge in terms of the state estimate, its variance and even the criterion, thus they should be used with extreme caution.
As far as the computational costs of the filters are concerned, it is clear that finding an optimal α i in the IEKFs with exact line search is more costly than setting a fixed step-length (α i = 0.5 or α i = 1 for the IEKF). However, the filters with optimal α i may not be slower than the filters with a constant α i due to possible lower number of iterations performed. This little test served as a guideline for finding a robust parameter setting to the IEKFs, hence the comprehensive analysis can be performed now. In the BOT example the IEKF-L and the IEKF-LM possess one degree of magnitude lower RMSE than the EKF and the IEKF. The RMSE of the IEKF-QN is slightly worse. In the UNGM example the results of IEKFs with the exact line search are even approaching to the CRLB, contrary to the EKF and IEKF, which are far from the CRLB. The estimate error CM analysis was performed using the NCI and its extension, the II. In the BOT example the course of the NCI (Figure 9) and the II (Figure 11) is similar to the RMSE development, i.e., the IEKF-L and the IEKF-LM outperform the EKF, the IEKF in terms of credibility of the results. Moreover all filters are too optimistic in their performance self-assessment, because the II is positive, indicating that the CM of the estimate error is too small in a perspective of the MSEM. Contrary to this, in the UNGM example the NCI ( Figure 10) and the II (Figure 12) for the exact line search IEKFs tend to be perfectly credible from time instant k = 3 nay they are pessimistic in 2 time instants due to simulations starting from a point with zero variance contrary to the initial conditions for the filters.      To complete the picture the time average of the RMSE and the NCI and computational time normalized with respect to the EKF are summed up in the Table 1 for the BOT example and in the Table 2 for the UNGM example, respectively. The measured time was normalized with respect to time of the EKF.
The results confirm the superiority of the IEKF-L and the IEKF-LM both in terms of the RMSE and the NCI. The results question the fixed parameter choice for the IEKFs and the IEKF-QN in general.
Note that the simulations were performed using the GaFT toolbox [17].

Conclusions
The paper dealt with state estimation of nonlinear stochastic dynamic systems. Particularly, a comprehensive analysis of the three iterated extended Kalman filters proposed in [9] was performed.
The main goal was to verify that the filters provide not only better state estimates, but also the CM of the estimate errors. This was experimentally proved for the IEKF-L and the IEKF-LM using the noncredibility index, which compares the estimate error CM of the filter with the estimate error CM of a perfectly credible filter. Additionally, it was studied whether considering a fixed step-length leads to results comparable with optimized step-length. It was observed that the fixed step-length leads to slightly better results than the IEKF can provide, however the results with the optimal step-length were better in order of magnitude. In fact, the results of the IEKFs with the optimized step-length in the UNGM problem were approaching to the objective limit of cognizability, i.e., to the Cramer-Rao lower bound.