Set-Membership Based Hybrid Kalman Filter for Nonlinear State Estimation under Systematic Uncertainty

This paper presents a new set-membership based hybrid Kalman filter (SM-HKF) by combining the Kalman filtering (KF) framework with the set-membership concept for nonlinear state estimation under systematic uncertainty consisted of both stochastic error and unknown but bounded (UBB) error. Upon the linearization of the nonlinear system model via a Taylor series expansion, this method introduces a new UBB error term by combining the linearization error with systematic UBB error through the Minkowski sum. Subsequently, an optimal Kalman gain is derived to minimize the mean squared error of the state estimate in the KF framework by taking both stochastic and UBB errors into account. The proposed SM-HKF handles the systematic UBB error, stochastic error as well as the linearization error simultaneously, thus overcoming the limitations of the extended Kalman filter (EKF). The effectiveness and superiority of the proposed SM-HKF have been verified through simulations and comparison analysis with EKF. It is shown that the SM-HKF outperforms EKF for nonlinear state estimation with systematic UBB error and stochastic error.


Introduction
The nonlinear state estimation problem has received significant attention in the fields of process control [1], tracking guidance [2], system identification [3], sensor networks [4], navigation [5,6] and so on. It is well known that the more accurate the system model is, the more accurate the state estimation can be obtained. However, due to the complexity of systematic dynamics and the dynamically changing conditions of the environment, uncertainty is inevitably involved in the system model [7]. This uncertainty is commonly characterized by stochastic errors such as the Gaussian or non-Gaussian system noise. With the assumption that the statistical characteristics of system noise are known, the issue of nonlinear system state estimation has been extensively studied [6,[8][9][10]. However, in practical applications, the system uncertainty is mixed by stochastic errors as well as unknown but bounded (UBB) errors [11]. The UBB error, as implied by its name, refers to the systematic modelling uncertainty whose probability distribution is difficult to identify or even without the probabilistic nature. If the effect of UBB error on system dynamics is not considered in the filtering process, the state estimation will be biased or even diverged.
The Kalman filter (KF) is undoubtedly the most famous state estimation method within the Bayesian framework. As the system model is linear and the uncertainty is Gaussian distributed, KF is Definition 1. An ellipsoid ξ(a, P) is given by the set ξ(a, P) = x ∈ R n : (x − a) T (P) −1 (x − a) ≤ 1 , (1) where a is the center of the ellipsoid, P is symmetric positive definite shape matrix and x is any point within the ellipsoid.
where A is a parameterized matrix.
That is, ξ(a 1 , with a = a 1 + a 2 (7) where ρ ∈ (0, 1) defines the weights on P 1 and P 2 , which can be determined by minimizing the semi-axes of the ellipsoid, that is [11] ρ = trace(P 1 ) Some other alternatives for computing the value of ρ can be found in Reference [24].

Problem Formulation
Consider the following nonlinear discrete-time dynamical system where x k ∈ R n and z k ∈ R m , both f (·) and h(·) are assumed to be twice continuously differentiable functions; δ xG k ∈ R n and δ yG k ∈ R m are the zero-mean Gaussian white noises with covariance matrices Q xG k and R where o f (x k−1 −x k−1 ) denotes the higher-order remainder term in the Taylor series. The interval mathematics is used to bound the linearization error o f (x k−1 −x k−1 ). Suppose the ellipsoidal sets of the system state at time k − 1 is ξ x k−1 ,P k−1 . The extrema of this state ellipsoid are computed asx wherex i k−1 is the ith component ofx k−1 ; the subscripts "−" and "+" denote the minimum and maximum values; andP The state interval bound X k−1 forx k−1 is then defined as and the interval for the linearization error can be further determined by where Hes i (i = 1, 2, · · · , n) represents the Hessian matrix of f (·) at X k−1 .
Based on the results in Reference [21], the interval X R (k − 1) can be bounded using an outer After that, we denote the linearization error as δ xL k = o f (x k−1 −x k−1 ). By taking the linearization error into account, the process model (10) can be rewritten in the following linear form where k . Similar to (12)- (18), an outer bounding ellipsoid ξ 0 m×1 , R y k is easy to achieve to bound the linearization error of h(·) such that the measurement model (11) can be rewritten in the following linear form is the linearization error. For the dynamic system described by (18) and (19), we shall discuss in the following how to estimate the system state in the presence of both UBB and stochastic errors.

Optimal Kalman Gain for Nonlinear System with UBB Error and Stochastic Error
Since the linearization error is also unknown but bounded, we firstly combine it with the systematic UBB error to generate a new UBB error term through the Minkowski sum. For the process model given by (18), by summing the linearization error δ xL k and the systematic UBB error δ xU k , a new UBB error δ xU k can be defined as Similarly, for the measurement model given by (19), another new UBB error δ yU k is introduced by the sum of δ yL k and δ yU k Then, the dynamic system described by (18) and (19) can be further rewritten as where δ xG k , δ yG k , δ xU k and δ yU k are independent of each other. In the following, we shall derive the optimal Kalman gain based on the system described by (22) and (23).
Suppose the system state estimate at time k − 1 isx k−1 , whose error covariance isP k−1 . From the KF framework, the state estimate at time k can be obtained bŷ where K k is the Kalman gain that we are looking for to minimize the mean squared error of the state estimate.
To simplify the vector operations, we employ the notation (x) 2 = x · x T for any vector x throughout this paper. Since x k−1 −x k−1 is uncorrelated to δ xG k , δ yG k , δ xU k and δ yU k , it is verified from (22)~(24) that the error covariance ofx k can be expressed as Due to the set-membership of δ xU k and δ yU k as shown in (20) and (21), the last term in (25) can be computed as the Minkowki sum, that is, in which Sensors 2020, 20, 627 6 of 14 where ρ ∈ (0, 1) is determined according to (9).
Then, from (25) and (26), the trace of E (x k − x k ) 2 can be computed by In order to determine the optimal Kalman gain K k which minimizes the error covariance ofx k , = 0 has to be fulfilled. By use of the derivative rules for the trace, we obtain the derivations of the first and second terms of (28) as follows and Similar to (29) and (30), the derivations of the third and fourth terms of (28) can be readily given by and Thus, substituting (29) It should be noted that, by simultaneous treatment of the systematic UBB error, stochastic error and linearization error, the SM-HKF established based on the Kalman Gain K k in (33) can restrain the effects of both UBB error and stochastic error on nonlinear state estimation in a hybrid way, which makes the robust filtering a reality.
Remark 1. Suppose the dynamic system described by (10) and (11) does not involve UBB error and the linearization error is neglected, the Kalman Gain K k given in (33) can be simplified as which is exactly the Kalman Gain in EKF.

Remark 2.
In the KF framework, the calculation of the predicted state and its covariance matrix is actually an important intermediate step to achieve the ultimate state estimation, even though we do not display it particularly in the derivation of the optimal Kalman gain. Denote the predicted state asx k/k−1 = f (x k−1 ). From (22), the covariance matrix ofx k/k−1 can be represented aŝ where the result of (20) is employed.
Remark 3. When using (35) and (36) in practical engineering,P k/k−1 andP k are commonly calculated bŷ Although (37) and (38) may be of some conservatism, the usage of them can improve the convergence speed of SM-HKF, which is similar to the traditional Kalman filter.

The SM-HKF Algorithm
Considering the nonlinear discrete-time dynamic system described by (10) and (11) and their equivalent equations (22) and (23), the proposed SM-HKF algorithm can be summarized as follows.
Step 5. Repeat Steps 1 to 4 for the next time step.

Performance Evaluation
Simulations have been conducted to evaluate the performance of the proposed SM-HKF in the presence of UBB error and stochastic error in comparison with EKF. A two-dimensional target tracking model is considered, in which the state vector is composed of the vehicle position and velocity in East and North while the measurement vector the azimuth angle and slope distance of the vehicle.
Define the system state as , the process model and measurement model are given by where δ xG k and δ yG k are the zero-mean Gaussian white noises (i.e., stochastic errors); δ xU k and δ yU k are the UBB errors; and The covariance matrices of the Gaussian white noises in the process and measurement models are assumed to be where the constant t is the sampling interval. The shape matrix of the bounding ellipsoid describing the UBB error in the process model is where a x and a x are the accelerations in East and North introduced in the vehicle trajectory simulation.
Commonly, the measurement model can be established exquisitely based on the prior physical characteristics of measurement device and its accuracy can be further improved using a large amount of available measurement data [7]. Thus, we assume that there exist no UBB error in the measurement model which is given by The vehicle accelerations in East and North as well as the simulated vehicle trajectory are shown in Figures 1 and 2. The vehicle acceleration variations are presented in Table 1, where the increases and decreases of the vehicle accelerations in East and North are described by UBB error and the stochastic Sensors 2020, 20, 627 9 of 14 fluctuation is described by Gaussian white noise in the process model. The simulation time is 1200 s and the sampling interval is 1 s. characteristics of measurement device and its accuracy can be further improved using a large amount of available measurement data [7]. Thus, we assume that there exist no UBB error in the measurement model which is given by (46) The vehicle accelerations in East and North as well as the simulated vehicle trajectory are shown in Figures 1 and 2. The vehicle acceleration variations are presented in Table 1, where the increases and decreases of the vehicle accelerations in East and North are described by UBB error and the stochastic fluctuation is described by Gaussian white noise in the process model. The simulation time is 1200 s and the sampling interval is 1 s.   characteristics of measurement device and its accuracy can be further improved using a large amount of available measurement data [7]. Thus, we assume that there exist no UBB error in the measurement model which is given by (46) The vehicle accelerations in East and North as well as the simulated vehicle trajectory are shown in Figures 1 and 2. The vehicle acceleration variations are presented in Table 1, where the increases and decreases of the vehicle accelerations in East and North are described by UBB error and the stochastic fluctuation is described by Gaussian white noise in the process model. The simulation time is 1200 s and the sampling interval is 1 s.    UBB error, both EKF and SM-HKF can estimate the vehicle position in high accuracy because the systematic uncertainty is of a stochastic nature obeying the Gaussian distribution. However, due to its incapability in restraining the effect of UBB error on the filtering solution, EKF has a degraded performance in the presence of acceleration variations. This phenomenon can be observed in the time segments (500 s, 510 s) and (900 s, 910 s) for the position in East as well as in the time segments (300 s, 310 s) and (500 s, 510 s) for the position in North. In contrast, the proposed SM-HKF can track the vehicle position effectively even in the presence of UBB error, since the proposed SM-HKF determines the Kalman gin matrix under the criterion of minimum mean squared error with the consideration of both UBB error and stochastic error.   Figures 3 and 4 can also be observed. It is easy to verify that EKF results in relatively large estimation biases due to the lack of the robustness against UBB errors. In contrast, since it takes UBB error into account in the Kalman filtering procedure, the vehicle velocity estimated by SM-HKF is much more accurate than that by EKF.
In addition, by repeating the above simulation 50 times, the Monte Carlo method was also employed to evaluate the SM-HKF robustness comparing to EKF from the statistical perspective. Figures 7 and 8 show the estimation errors in terms of position and velocity obtained by EKF and SM-HKF, respectively. It can be seen that in the time segments without the UBB errors, the estimation error obtained by EKF is slightly larger than that by SM-HKF due to the negligence of the linearization error in the measurement model. However, in the time segments with UBB errors, the difference of estimation error between EKF and SM-HKF become quite evident. As shown in Table 2, the means of the root mean square error (RMSE) of the position errors obtained by SM-HKF are at least 74.4% smaller than those obtained by EKF and the means of the RMSE of the velocity errors by SM-HKF are at least 82.7% smaller than those by EKF. The proposed SM-HKF outperforms EKF significantly due to its capability to resist the influence of UBB error on system state estimation.   Figures 3 and 4 can also be observed. It is easy to verify that EKF results in relatively large estimation biases due to the lack of the robustness against UBB errors. In contrast, since it takes UBB error into account in the Kalman filtering procedure, the vehicle velocity estimated by SM-HKF is much more accurate than that by EKF.     In addition, by repeating the above simulation 50 times, the Monte Carlo method was also employed to evaluate the SM-HKF robustness comparing to EKF from the statistical perspective. Figures 7 and 8 show the estimation errors in terms of position and velocity obtained by EKF and SM-HKF, respectively. It can be seen that in the time segments without the UBB errors, the estimation error obtained by EKF is slightly larger than that by SM-HKF due to the negligence of the linearization error in the measurement model. However, in the time segments with UBB errors, the difference of estimation error between EKF and SM-HKF become quite evident. As shown in Table 2, the means of the root mean square error (RMSE) of the position errors obtained by SM-HKF are at least 74.4% smaller than those obtained by EKF and the means of the RMSE of the velocity errors by SM-HKF are at least 82.7% smaller than those by EKF. The proposed SM-HKF outperforms EKF significantly due to its capability to resist the influence of UBB error on system state estimation.     The above simulations and analysis demonstrate that the proposed SM-HKF can effectively inhibit the influences of both UBB error and stochastic error on system state estimation by adaptively adjusting the Kalman gain matrix under the criteria of minimum mean squared error, leading to the The above simulations and analysis demonstrate that the proposed SM-HKF can effectively inhibit the influences of both UBB error and stochastic error on system state estimation by adaptively adjusting the Kalman gain matrix under the criteria of minimum mean squared error, leading to the improved robustness against systematic uncertainty and the higher filtering accuracy than EKF for nonlinear state estimation.

Conclusions
This paper presents a new SM-HKF to address the issue of nonlinear state estimation with systematic uncertainty. This method derives the optimal Kalman gain to minimize the mean squared error of the state estimate in the framework of KF, leading to the capability in handling both UBB error and stochastic error simultaneously in the filtering procedure. It improves EKF using the concept of set-membership to resist the effect of UBB error on the filtering solution. It also avoids the loss of accuracy in EKF due to the negligence of the linearization error. Further, the proposed SM-HKF incorporates the set-membership estimation in the KF framework, leading to a promising solution for nonlinear state estimation under systematic uncertainty composed of both UBB error and stochastic error. The simulation results and comparison analysis demonstrate the effectiveness and superiority of the proposed SM-HKF in comparison with EKF.
Future research will focus on the in-depth theoretical analysis of the convergence of SM-HKF to facilitate the application of the proposed method in various fields.