An EKF-Based Fixed-Point Iterative Filter for Nonlinear Systems

In this paper, a fixed-point iterative filter developed from the classical extended Kalman filter (EKF) was proposed for general nonlinear systems. As a nonlinear filter developed from EKF, the state estimate was obtained by applying the Kalman filter to the linearized system by discarding the higher-order Taylor series items of the original nonlinear system. In order to reduce the influence of the discarded higher-order Taylor series items and improve the filtering accuracy of the obtained state estimate of the steady-state EKF, a fixed-point function was solved though a nested iterative method, which resulted in a fixed-point iterative filter. The convergence of the fixed-point function is also discussed, which provided the existing conditions of the fixed-point iterative filter. Then, Steffensen’s iterative method is presented to accelerate the solution of the fixed-point function. The final simulation is provided to illustrate the feasibility and the effectiveness of the proposed nonlinear filtering method.


Introduction
Interesting but unavailable signal variables can be estimated using a proper filter. In recent decades, filter design for various theoretical and applied systems has been a popular research topic in the fields of automatic control, target tracking, fault diagnosis, etc. [1][2][3][4][5].
The Kalman filter is designed for linear systems with noise-satisfying Gaussian distribution, and is an optimal filter due to the minimum mean square error (MMSE) [6]. The Kalman filter provides an estimate and prediction of the system state in real time. However, the classical Kalman filter must be applied to linear systems. For the filtering problem of nonlinear systems, a large number of effective filters have been developed on the basis of the classical Kalman filter.
The filters for nonlinear systems are mainly designed according to two principles. The first kind of nonlinear filter is designed by linearizing the system function, such as the celebrated extended Kalman filter (EKF) [7]. The second kind of nonlinear filter is designed on the basis of approximating the state statistics, such as the Unscented Kalman filter (UKF) [8][9][10], and the Cubature Kalman filter (CKF) [11][12][13]. Various nonlinear filters are designed in different ways to predict and update nonlinear system state estimates. In the state estimate prediction stage, according to EKF, the process function is linearized by discarding the second or higher order Taylor series items at the state estimate of the previous time instant. Then, the Kalman filter is utilized to predict the state estimate in terms of the linearized process function. In the state estimate updating stage, the measurement function is linearized by discarding the higher order items of its Taylor series at the predicted state estimate of the current time instant. Then, the Kalman filter is utilized to update the state estimate in terms of the linearized appropriate linear function; and (2) sample point-based nonlinear filtering methods, in which the statistics of the state estimate are usually approximated by the statistical result of the sample points, for instance. However, it is unavoidable that certain errors exist in the above two kinds of approximations and result in estimation errors of the above two kinds of nonlinear filters. In order to improve the nonlinear filtering accuracy of the EKF-based nonlinear filtering methods, taking advantage of fixed-point theory, an EKF-based fixed-point iterative filter is proposed in this paper.

EKF-Based Fixed-Point Filter
In this section, for the filtering problem of the general nonlinear system described in Equations (1) and (2), the classical EKF will firstly be introduced. Then, a fixed-point function will be provided to update the obtained state estimate by solving the fixed-point function. In Section 3.3, a nested iterative method is derived to solve the fixed-point filter. The convergence and existing condition of the fixed-point filter will be discussed in Section 3.4. Then, Steffensen's iterative method will be presented to accelerate the solution of the fixed-point function in Section 3.5.

Classic Extended Kalman Filter
In the EKF-based nonlinear filtering methods for the system described in Equations (1) and (2), the nonlinear system model is always represented by the Taylor series at a working point and linearized by discarding the second and higher order terms. Then, the well-known Kalman filter is used to estimate the state of the linearized system. The classical EKF can be summarized as follows. Assumption 1. The estimate and the estimation error variance of the state at the time instant k have been obtained and are denoted asx(k k), P(k k) , then the state at the time instant k + 1 will be estimated as follows.
(1) Time Updating Taking the state estimate as the working point, the Taylor series expression of the nonlinear process function is Applying the Kalman filter to the linearized system shown as the second approximate formula above, the state prediction at the time instant k + 1 and the prediction error variance can be obtained by is the Jacobian of f (x(k), k) at the working pointx(k k) .
(2) Measurement Updating: Taking the state prediction obtained above as the working point, the measurement function can be rewritten as The state estimate and the estimate error variance at time instant k + 1 can be derived as where is the Jacobian of h(x(k + 1), k + 1) at the working point x(k + 1 k) and K(k + 1) is the filter gain, which is represented as 3.2. Numerical Updating Method Based on the Fixed-Point Theory As mentioned above, in the EKF,x(k + 1 k + 1) is obtained to estimate the system state x(k + 1) on the basis of the measurements y(1), y(2), · · · , y(k + 1) . Due to the discarded second and higher order Taylor series items of the process function and the measurement function, not all of the information contained in the measurement could be utilized to obtainx(k + 1 k + 1) . In order to update this state estimate and make more full use of the unused information in the measurement, taking advantage of fixed-point theory, the following fixed-point function is constructed for a steady-state filter wherex(k + 1|k + 1) is the state of the above fixed-point filter, K * (k + 1) is the filter gain to be determined, and E v(k + 1) is the mean of the measurement noise, which was found to be zero in this paper.

A Nested Iterative Solution for the Fixed-Point Function
In the fixed-point function (8), two unknown items must be determined simultaneously-The unknown state of the above fixed-point filter,x(k + 1|k + 1) and the filter gain to be determined K * (k + 1). In this section, a fixed-point filter is presented by solving the above fixed-point function (8), according to the following nested iterative computation process.
In the first iteration, take the state estimatex(k + 1 k + 1) obtained by the EKF as the initial value on the right side of the fixed-point function. Denote the initial state of the first iteration aŝ x 0 0 (k + 1|k + 1 ) =x(k + 1|k + 1), where the subscript 0 means the first iteration, while the superscript 0 is the initial value of this iteration, which increases in this iteration. Then we have the following iterative solution process of the fixed-point function with the initial filter gain K * 0 (k + 1)= K(k + 1).
i 0 0 (k + 1|k + 1 ) as the state estimate in the first iterative process.
Due tox the filter gain can be updated in the first iteration by In the second iteration, the initial value in the fixed-point numerical solving process isx is taken as the state estimate in the second iterative process, then the filter gain in the second iteration can be updated by For the lth iteration,x 0 l (k + 1|k + 1 )=x i l−1 l−1 (k + 1|k + 1 ) is taken as the initial value to solve the fixed-point numerical and is iteratively updated by the following Similarly is taken as the state estimate in the lth iterative process, then the filter gain in the lth iteration can be updated by is a given scalar), the final estimate output of the state at the time instant k + 1 is given bŷ The flowchart of the iterative computational process of the estimates is illustrated in Figure 1.

Convergence of the Fixed-Point Filter
In order to ensure that the constructed fixed-point filter is convergent, it is necessary to conditionally constrain the fixed-point equation. In this subsection, the convergence of the fixed-point function (8) will be discussed.
(2) There exists a positive scalar L, such that ( ) Based on this lemma, the convergence conditions of the fixed-point function (8) are given as follows.

Theorem 1. For a given filter
, the fixed-point function (8) has a fixed point in [a,b], if the measurement function satisfies the following two conditions [ ] Proof. Denote

Convergence of the Fixed-Point Filter
In order to ensure that the constructed fixed-point filter is convergent, it is necessary to conditionally constrain the fixed-point equation. In this subsection, the convergence of the fixed-point function (8) will be discussed.
(2) There exists a positive scalar L, such that Based on this lemma, the convergence conditions of the fixed-point function (8) are given as follows.
Theorem 1. For a given filter K * (k + 1), the fixed-point function (8) has a fixed point in [a,b], if the measurement function satisfies the following two conditions Proof. Denote Ifx(k + 1|k + 1) ∈ [a, b], according to Lemma 1, one has a ≤ ϕ(x(k + 1|k + 1)) ≤ b. It means that Then, the measurement function satisfies Using the upper bound in (17) and the lower bound in (18), one gets a +ŷ The Jacobian value of ϕ(x(k + 1|k + 1)) is obtained by Then, according to Lemma 1, the measurement function should satisfy Therefore, the fixed-point function (8) has a unique fixed point in [a,b], if the measurement function satisfies (23) and (25). The proof is completed.

Remark 3.
It is commonly known that not all fixed-point equations are convergent. The convergence conditions of the fixed-point function (8) are presented in Theorem 1, which is also the existing condition of the fixed-point iterative filter given in Section 3.3. If the convergence conditions in Theorem 1 are satisfied, the state estimate will be further updated by the fixed-point iterative filter shown in Section 3.3. Otherwise, the state estimate obtained by the EKF need not be updated and denoted as the final estimate at a certain time instant. The interval [a,b] can be set according to the actual demand, or by using the state prediction value with the 3-Delta rule. The norm could be 1-noum for the vector parameters.

Steffensen's Iterative Solution of the Fixed-Point Function
In Section 3.3, a nested iterative solution for the fixed-point filter was presented. In this section, Steffensen's iterative method is used to accelerate the solution of the fixed-point function (8). Steffensen's iterative method in the lth iteration is given as followŝ Let K * 0 (k + 1)= K(k + 1), then Steffensen's iterative method can be given by . . . where n 0 0 (k + 1|k + 1 ) as the state estimate of the fixed-point filter with the filter gain K * 0 (k + 1). Similar to (11), the filter gain can be updated by Substituting the new filter gain above, the fixed-point function is represented aŝ According to Steffensen's iterative method, one has 1 (k + 1|k + 1)))), i = 0, 1, · · · n 1 (34) If x n 1 1 (k + 1|k + 1 ) −x n 1 −1 1 (k + 1|k + 1) < ε, takex n 1 1 (k + 1|k + 1 ) as the state estimate of the fixed-point filter with the filter gain K * 1 (k + 1). Similarly, the filter gain can be updated by The fixed-point filter is constructed aŝ Steffensen's iterative solution of the fixed-point filter above is obtained by . . .
n j j (k + 1|k + 1 ) as the solution of the fixed-point filter with the filter gain K * j (k + 1). If K * t (k + 1) − K * t−1 (k + 1) < ε K , the state estimatex n t t (k + 1|k + 1) obtained by the fixed-point iteration process with the filter gain K * t (k + 1) is the final solution of the fixed-point filter.

Remark 4. Compared with the nested iterative solution method in Section 3.3, Steffensen's iterative method
can accelerate the solution process of the fixed-point function (8). Because of this, the filtering result of the two fixed-point filters may be slightly different.

Remark 5.
It should be noted that the conditions for judging whether the iterative process ends in the solution methods of the fixed-point function are to compare the differences between the results of two adjacent iterations. However, the final purposes of the updating methods are reducing the differences between the estimates and the active values of the system states. It is a pity that the active values of the system states are unavailable. Therefore, the difference between the active value and the prediction values of the measurement by different methods are compared in the simulation to determine whether the updating is necessary.

Simulation
In this section, two comparison simulation examples are provided to illustrate the feasibility and the effectiveness of the proposed EKF-based fixed-point iterative filters (the one provided in Section 3.3 is denoted as FEKF, while the one provided in Section 3.5 is denoted SFEKF), which are compared with the classical EKF and UKF methods.
Simulation I. Considering the following non-linear system where w(k + 1, k), v(k + 1) are respectively the process noise and the measurement noise, which satisfy the zero-mean Gaussian distributions with the variances Q = 0.1, R = 0.001. The initial state estimation and its error variance are respectively x(0) = 2, p(0) = 0.01. In order to reduce the effects of random noise on the comparison results, a Monte Carlo simulation was repeated 50 times. The simulation results were as follows.
As shown in Figure 2, compared with the absolute error curves of the classical EKF and UKF, the error curves of the proposed iterative fixed-point filters were lower at almost all 80 simulation time instants. The main cause of the higher absolute error curve of the EKF was the discarded higher order terms of the Taylor series in the linearization processes. Since the proposed fixed-point filters iteratively updated the estimates of the EKF, the absolute error curve was lower than that of the EKF. The UKF approximated the statistics of the state estimates by using unscented transformation, and the estimate accuracy of UKF was related to the filter parameters. The estimate accuracy of the UKF was the lowest, although several possible filter parameters were tested. The mean weightings W m i and variance weightings W c i of the UKF were respectively set as W m i = λ/(n + λ), i = 1 1/(2n + 2λ), i = 2, 3 ,  The fixed-point iterative filter based on the extended Kalman filter can compensate the influence of discarded higher order terms of the Taylor series. As a result, the filtering results of the methods proposed in this paper were better than others. The mean values of the absolute error of the four comparison methods in Table 1 illustrated the feasibility and effectiveness of the proposed methods. In the same simulation conditions, the mean absolute error of the FEKF was 0.0178 and the mean absolute error of the SFEKF was 0.0141, while the others were 0.0278 for the EKF, and 0.1426 for the UKF, respectively. As shown in Table 2, the computation complexity of the EKF was the smallest, and that of the UKF was the largest, due to the computation of the sigma points. The FEKF and SFEKF were designed to update the estimation results of the EKF, and accordingly, their computation complexities were larger than that of the EKF. Steffensen's iterative method was used to accelerate the solution of the fixed-point function (8), therefore, the CPU time cost of repeating Monte Carlo simulations 50 times was smaller with the SFEKF than with the FEKF. It is noted that the estimates obtained by the proposed methods were the closest to the actual values, as shown in Figure 3. Due to the influence of the discarded higher order items of the Taylor series system functions, the estimate accuracy of the EKF method was relatively poor. The influence was reduced using a fixed-point iterative filter. The measurement prediction results in Figure 4 also illustrated the feasibility and the effectiveness of the proposed fixed-point iterative filters. The fixed-point iterative filter based on the extended Kalman filter can compensate the influence of discarded higher order terms of the Taylor series. As a result, the filtering results of the methods proposed in this paper were better than others. The mean values of the absolute error of the four comparison methods in Table 1 illustrated the feasibility and effectiveness of the proposed methods. In the same simulation conditions, the mean absolute error of the FEKF was 0.0178 and the mean absolute error of the SFEKF was 0.0141, while the others were 0.0278 for the EKF, and 0.1426 for the UKF, respectively. As shown in Table 2, the computation complexity of the EKF was the smallest, and that of the UKF was the largest, due to the computation of the sigma points. The FEKF and SFEKF were designed to update the estimation results of the EKF, and accordingly, their computation complexities were larger than that of the EKF. Steffensen's iterative method was used to accelerate the solution of the fixed-point function (8), therefore, the CPU time cost of repeating Monte Carlo simulations 50 times was smaller with the SFEKF than with the FEKF. It is noted that the estimates obtained by the proposed methods were the closest to the actual values, as shown in Figure 3. Due to the influence of the discarded higher order items of the Taylor series system functions, the estimate accuracy of the EKF method was relatively poor. The influence was reduced using a fixed-point iterative filter. The measurement prediction results in Figure 4 also illustrated the feasibility and the effectiveness of the proposed fixed-point iterative filters.  Simulation II. Consider the following target tracking system, in which the target is in nearly constant velocity: The target was initially located at (0, 1400), with an initial velocity of (1.8, -9.5). A radar was located at the origin of the polar coordinate. The measurement equation was given by The variances of      Simulation II. Consider the following target tracking system, in which the target is in nearly constant velocity: The target was initially located at (0, 1400), with an initial velocity of (1.8, −9.5). A radar was located at the origin of the polar coordinate. The measurement equation was given by The variances of w x (k + 1, k) w y (k + 1, k) and v θ (k) v r (k) were 10 −4 0 0 10 −4 and 10 −2 0 0 10 .
The sampling period was 1. The simulation results are shown in Figures 5 and 6.  As shown in Figure 6, the proposed FEKF and SFEKF methods effectively tracked the target with the radar measurements. The two proposed methods obtained more accurate estimates of the target than the EKF for the updating method with the fixed-point theory, as shown in Figure 5 and Table 3.

Conclusions
As a development of the classic extended Kalman filter, the fixed-point iterative updating method was studied in this paper, drawing on fixed-point theory. On the basis of the extended Kalman filter, a fixed-point function was provided to update the state estimate obtained by the EKF and reduce the influence of the discarded higher order items of the Taylor series. The fixed-point function was solved by the nested iterative method and Steffensen's iterative method and resulted in two fixed-point iterative filters. The convergence conditions of the fixed-point function were also studied, as were the existing conditions of fixed-point iterative filters.   As shown in Figure 6, the proposed FEKF and SFEKF methods effectively tracked the target with the radar measurements. The two proposed methods obtained more accurate estimates of the target than the EKF for the updating method with the fixed-point theory, as shown in Figure 5 and Table 3. Table 3. The mean distances between the real value and estimates for the four comparison methods.

Conclusions
As a development of the classic extended Kalman filter, the fixed-point iterative updating method was studied in this paper, drawing on fixed-point theory. On the basis of the extended Kalman filter, a fixed-point function was provided to update the state estimate obtained by the EKF and reduce the influence of the discarded higher order items of the Taylor series. The fixed-point function was solved by the nested iterative method and Steffensen's iterative method and resulted in two fixed-point iterative filters. The convergence conditions of the fixed-point function were also studied, as were the existing conditions of fixed-point iterative filters.  As shown in Figure 6, the proposed FEKF and SFEKF methods effectively tracked the target with the radar measurements. The two proposed methods obtained more accurate estimates of the target than the EKF for the updating method with the fixed-point theory, as shown in Figure 5 and Table 3. Table 3. The mean distances between the real value and estimates for the four comparison methods.

Conclusions
As a development of the classic extended Kalman filter, the fixed-point iterative updating method was studied in this paper, drawing on fixed-point theory. On the basis of the extended Kalman filter, a fixed-point function was provided to update the state estimate obtained by the EKF and reduce the influence of the discarded higher order items of the Taylor series. The fixed-point function was solved by the nested iterative method and Steffensen's iterative method and resulted in two fixed-point iterative filters. The convergence conditions of the fixed-point function were also studied, as were the existing conditions of fixed-point iterative filters.