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

Abstract

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.

1. 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 optimal in the sense of mean squared error [12]. Unfortunately, KF is only suitable for nominally linear systems due to its theoretical limitation. For nonlinear systems, a variant of KF, which is named as the extended Kalman filter (EKF), linearizes the nonlinear system model via a Taylor series expansion such that KF can be applied [13]. EKF has been used for many years as the benchmark in nonlinear state estimation. Some other candidates are the unscented Kalman filter (UKF) [5,6,14], cubature Kalman filter (CKF) [15] and particle filter (PF) [16]. However, since these nonlinear state estimation methods are based on the framework of KF, they also require the exact statistical knowledge of system uncertainty, which is difficult to achieve in engineering practice.

Research efforts have been devoted in improving the robustness of nonlinear filtering against unknown system uncertainty. These include the Huber’s M-Estimation based robust EKF [17], strong tracking UKF [7], covariance matching based adaptive UKF [18], Mahalanobis distance based robust CKF [19] and H-infinity based robust CKF [20], to name a few only. However, the existing techniques are mainly focused on the disturbances of the stochastic errors such as observation outliers, non-Gaussian noise and inaccurate noise statistics, rather than the disturbance of UBB error on nonlinear state estimation.

The set-membership filtering is a method to handle UBB error for nonlinear state estimation. The origination of the set-membership filtering can be dated back to the 1960s and such a problem has received recurring research interest in the past decade [11,21,22,23]. Different from the above nonlinear Kalman filters, the state estimation obtained by the set-membership filtering is a feasible set of possible states rather than a single value. This feasible set describes the range of the state estimate and guarantees that the estimation error is confined to a bounded region. Currently, the polytopes and ellipsoids are commonly used to describe the feasible set. Compared to the polytope method, which requires a great large number of inequalities to determine the feasible set, the ellipsoid method is more popular for online applications since it can represent the set with fewer pieces of information [21]. Therefore, for the sake of computational performance, the ellipsoid method is employed in this paper for feasible set description.

Among the existing studies on the set-membership filtering, the techniques reported in References [11] and [21] are quite constructive. In Reference [21], the concept of set-membership estimation was extended from a linear system to nonlinear system for the first time. An extended set-membership filter (ESMF) was also established via the linearization of the system model, in which the linearization error and system noise were combined into a new UBB noise term and was further addressed by use of the interval mathematics. However, this method does not take the substantive UBB errors of a system into account and the combination of the linearization error and system noise may degrade the state estimation accuracy since the statistical knowledge of system noise is omitted. In Reference [11], considering both UBB error and stochastic error in the Kalman filtering scheme, a new Kalman gain was obtained by minimizing the mean squared error of the system state based on set-membership. However, the technique presented in Reference [11] is only suitable for linear systems.

Motivated by the techniques reported in References [11] and [21], this paper proposes a set-membership based hybrid Kalman filter (SM-HKF) for nonlinear system state estimation in the presence of both UBB error and stochastic error. This method linearizes the nonlinear system model by a Taylor series expansion and then combines the linearization error with the systematic UBB error to generate a new UBB error term through the Minkowski sum. Further, it derives an optimal Kalman gain under the criterion of minimum mean squared error based on the framework of KF. The proposed SM-HKF extends the set-membership based Kalman filter in Reference [11] from linear systems to nonlinear systems. It also overcomes the limitation of EKF by treating the systematic UBB error, stochastic error and linearization error simultaneously. Simulations and comparison analysis have been conducted to evaluate the effectiveness of the proposed method.

2. Definitions on Ellipsoidal Sets

To facilitate the problem formulation and the derivation of SM-HKF, several definitions for describing ellipsoidal sets are introduced for notational convenience.

Definition 1.

An ellipsoid$\mathit{\xi }\left(\mathit{a},\mathit{P}\right)$is given by the set

$$\mathit{\xi }\left(\mathit{a},\mathit{P}\right)=\left\{\mathit{x}\in {ℝ}^n:{\left(\mathit{x}-\mathit{a}\right)}^{\mathrm{T}}{\left(\mathit{P}\right)}_{}^{-1}\left(\mathit{x}-\mathit{a}\right)\le 1\right\},$$

(1)

where$\mathit{a}$is the center of the ellipsoid,$\mathit{P}$is symmetric positive definite shape matrix and$\mathit{x}$ is any point within the ellipsoid.

Definition 2.

The affine transition of the ellipsoid set can be computed as

$$\mathit{A}\mathit{\xi }\left(\mathit{a},\mathit{P}\right)=\mathit{\xi }\left(\mathit{A}\mathit{a},\mathit{A}\mathit{P}{\mathit{A}}^{\mathrm{T}}\right),$$

(2)

where$\mathit{A}$is a parameterized matrix.

Definition 3.

Supposing$\mathit{\xi }\left({\mathit{a}}_1,{\mathit{P}}_1\right)$and$\mathit{\xi }\left({\mathit{a}}_2,{\mathit{P}}_2\right)$are two ellipsoidal sets, the Minkowski sum of the ellipsoids is defined as

$$S_{\mathrm{S}}=\left\{\mathit{x}:\hspace{1em}\mathit{x}={\mathit{x}}_1+{\mathit{x}}_2,\hspace{1em}{\mathit{x}}_1\in \mathit{\xi }\left({\mathit{a}}_1,{\mathit{P}}_1\right),{\mathit{x}}_2\in \mathit{\xi }\left({\mathit{a}}_2,{\mathit{P}}_2\right)\right\}$$

(3)

and denoted by

$$S_{\mathrm{S}}=\mathit{\xi }\left({\mathit{a}}_1,{\mathit{P}}_1\right)\oplus \mathit{\xi }\left({\mathit{a}}_2,{\mathit{P}}_2\right).$$

(4)

Generally, the set $S_{\mathrm{S}}$ is not an ellipsoid. We can find an outer bounding ellipsoid ${\mathit{\xi }}_{\mathrm{S}}\left({\mathit{a}}^{\prime },{\mathit{P}}^{\prime }(\rho )\right)$ such that

$$S_{\mathrm{S}}\subseteq {\mathit{\xi }}_{\mathrm{S}}\left({\mathit{a}}^{\prime },{\mathit{P}}^{\prime }(\rho )\right).$$

(5)

That is,

$$\mathit{\xi }\left({\mathit{a}}_1,{\mathit{P}}_1\right)\oplus \mathit{\xi }\left({\mathit{a}}_2,{\mathit{P}}_2\right)\subseteq {\mathit{\xi }}_{\mathrm{S}}\left({\mathit{a}}^{\prime },{\mathit{P}}^{\prime }(\rho )\right),$$

(6)

with

$${\mathit{a}}^{\prime }={\mathit{a}}_1+{\mathit{a}}_2$$

(7)

$${\mathit{P}}^{\prime }(\rho )=\left(1+{\rho }^{-1}\right){\mathit{P}}_1+\left(1+\rho \right){\mathit{P}}_2,$$

(8)

where $\rho \in \left(0,1\right)$ defines the weights on ${\mathit{P}}_1$ and ${\mathit{P}}_2$, which can be determined by minimizing the semi-axes of the ellipsoid, that is [11]

$$\rho =trace{\left({\mathit{P}}_1\right)}^{\frac{1}{2}}\cdot trace{\left({\mathit{P}}_2\right)}^{-\frac{1}{2}}.$$

(9)

Some other alternatives for computing the value of $\rho$ can be found in Reference [24].

3. Set-Membership Based Hybrid Kalman Filter

3.1. Problem Formulation

Consider the following nonlinear discrete-time dynamical system

$${\mathit{x}}_k=f\left({\mathit{x}}_{k-1}\right)+{\mathit{\delta }}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}$$

(10)

$${\mathit{z}}_k=h\left({\mathit{x}}_k\right)+{\mathit{\delta }}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}},$$

(11)

where ${\mathit{x}}_k\in {ℝ}^n$ and ${\mathit{z}}_k\in {ℝ}^m$, both $f(\cdot )$ and $h(\cdot )$ are assumed to be twice continuously differentiable functions; ${\mathit{\delta }}_k^{x\mathrm{G}}\in {ℝ}^n$ and ${\mathit{\delta }}_k^{y\mathrm{G}}\in {ℝ}^m$ are the zero-mean Gaussian white noises with covariance matrices ${\mathit{Q}}_k^{x\mathrm{G}}$ and ${\mathit{R}}_k^{y\mathrm{G}}$; ${\mathit{\delta }}_k^{x\mathrm{U}}\in \mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\mathit{Q}}_k^{x\mathrm{U}}\right)$ and ${\mathit{\delta }}_k^{y\mathrm{U}}\in \mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\mathit{R}}_k^{y\mathrm{U}}\right)$ are the UBB errors involved in the process model and measurement model; and ${\mathit{\delta }}_k^{x\mathrm{G}}$, ${\mathit{\delta }}_k^{y\mathrm{G}}$, ${\mathit{\delta }}_k^{x\mathrm{U}}$ and ${\mathit{\delta }}_k^{y\mathrm{U}}$ are considered to be mutual independent.

Expanding (10) by a Taylor series about the system state estimate ${\widehat{\mathit{x}}}_{k-1}$, we have

$${\mathit{x}}_k=f\left({\widehat{\mathit{x}}}_{k-1}\right)+\frac{\partial f\left({\mathit{x}}_{k-1}\right)}{\partial {\mathit{x}}_{k-1}}|{}_{{\mathit{x}}_{k-1}={\widehat{\mathit{x}}}_{k-1}}\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\mathbf{o}}_f\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{\delta }}_{k-1}^{x\mathrm{G}}+{\mathit{\delta }}_{k-1}^{x\mathrm{U}},$$

(12)

where ${\mathbf{o}}_f\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)$ denotes the higher-order remainder term in the Taylor series.

The interval mathematics is used to bound the linearization error ${\mathbf{o}}_f\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)$. Suppose the ellipsoidal sets of the system state at time $k-1$ is $\mathit{\xi }\left({\widehat{\mathit{x}}}_{k-1},{\widehat{\mathit{P}}}_{k-1}\right)$. The extrema of this state ellipsoid are computed as

$${\widehat{\mathit{x}}}_{k-1,-}^i={\widehat{\mathit{x}}}_{k-1}^i-\sqrt{{\widehat{\mathit{P}}}_{k-1}^{i,i}} \mathrm{and} {\widehat{\mathit{x}}}_{k-1,+}^i={\widehat{\mathit{x}}}_{k-1}^i+\sqrt{{\widehat{\mathit{P}}}_{k-1}^{i,i}} (i=1,2,\cdots ,n),$$

(13)

where ${\widehat{\mathit{x}}}_{k-1}^i$ is the $i$th component of ${\widehat{\mathit{x}}}_{k-1}^{}$; the subscripts “−” and “+” denote the minimum and maximum values; and ${\widehat{\mathit{P}}}_{k-1}^{i,i}$ is the $i$th diagonal element of ${\widehat{\mathit{P}}}_{k-1}$.

The state interval bound ${\mathit{X}}_{k-1}^{}$ for ${\widehat{\mathit{x}}}_{k-1}^{}$ is then defined as

$${\mathit{X}}_{k-1}^i=\left[{\widehat{\mathit{x}}}_{k-1,-}^i,{\widehat{\mathit{x}}}_{k-1,+}^i\right] (i=1,2,\cdots ,n)$$

(14)

and the interval for the linearization error can be further determined by

$${\mathit{X}}_R^{}(k-1)=diag\{{\mathit{X}}_{k-1}^{\mathrm{T}}\}\left[\begin{array}{c}He{s}_1\\ He{s}_2\\ ⋮\\ He{s}_n\end{array}\right]{\mathit{X}}_{k-1}^{},$$

(15)

where $He{s}_i$($i=1,2,\cdots ,n$) represents the Hessian matrix of $f(\cdot )$ at ${\mathit{X}}_{k-1}^{}$.

Based on the results in Reference [21], the interval ${\mathit{X}}_{\mathit{R}}^{}(k-1)$ can be bounded using an outer bounding ellipsoid $\mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\overline{\mathit{Q}}}_k^x\right)$, in which

$${\left({\overline{\mathit{Q}}}_k^x\right)}^{i,i}=2{\left[{\left({\mathit{X}}_{\mathit{R}}^{}(k-1)\right)}^i\right]}^2$$

(16)

$${\left({\overline{\mathit{Q}}}_k^x\right)}^{i,j}={\mathbf{0}}_{n\times 1} (i\ne j).$$

(17)

After that, we denote the linearization error as ${\mathit{\delta }}_k^{xL}={\mathbf{o}}_f\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)$. By taking the linearization error into account, the process model (10) can be rewritten in the following linear form

$${\mathit{x}}_k=f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{\delta }}_k^{x\mathrm{L}}+{\mathit{\delta }}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}},$$

(18)

where ${\mathit{F}}_k=\frac{\partial f\left({\mathit{x}}_{k-1}\right)}{\partial {\mathit{x}}_{k-1}}|{}_{{\mathit{x}}_{k-1}={\widehat{\mathit{x}}}_{k-1}}$ and ${\mathit{\delta }}_k^{x\mathrm{L}}\in \mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\overline{\mathit{Q}}}_k^x\right)$.

Similar to (12)–(18), an outer bounding ellipsoid $\mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\overline{\mathit{R}}}_k^y\right)$ is easy to achieve to bound the linearization error of $h(\cdot )$ such that the measurement model (11) can be rewritten in the following linear form

$${\mathit{z}}_k=h\left({\widehat{\mathit{x}}}_{k/k-1}\right)+{\mathit{H}}_k({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k/k-1})+{\mathit{\delta }}_k^{y\mathrm{L}}+{\mathit{\delta }}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}},$$

(19)

where ${\widehat{\mathit{x}}}_{k/k-1}=f\left({\widehat{\mathit{x}}}_{k-1}\right)$ denotes the predicted state estimate, ${\mathit{H}}_k=\frac{\partial h\left({\mathit{x}}_k\right)}{\partial {\mathit{x}}_k}|{}_{{\mathit{x}}_k={\widehat{\mathit{x}}}_{k/k-1}}$ and ${\mathit{\delta }}_k^{y\mathrm{L}}\in \mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\overline{\mathit{R}}}_k^y\right)$ 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.

3.2. 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 ${\mathit{\delta }}_k^{x\mathrm{L}}$ and the systematic UBB error ${\mathit{\delta }}_k^{x\mathrm{U}}$, a new UBB error ${\overline{\mathit{\delta }}}_k^{x\mathrm{U}}$ can be defined as

$${\overline{\mathit{\delta }}}_k^{x\mathrm{U}}\in \mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\overline{\mathit{Q}}}_k^{x\mathrm{U}}\right)=\mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\overline{\mathit{Q}}}_k^x\right)\oplus \mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\mathit{Q}}_k^{x\mathrm{U}}\right).$$

(20)

Similarly, for the measurement model given by (19), another new UBB error ${\overline{\mathit{\delta }}}_k^{y\mathrm{U}}$ is introduced by the sum of ${\mathit{\delta }}_k^{y\mathrm{L}}$ and ${\mathit{\delta }}_k^{y\mathrm{U}}$

$${\overline{\mathit{\delta }}}_k^{y\mathrm{U}}\in \mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\overline{\mathit{R}}}_k^{x\mathrm{U}}\right)=\mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\overline{\mathit{R}}}_k^y\right)\oplus \mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\mathit{R}}_k^{y\mathrm{U}}\right).$$

(21)

Then, the dynamic system described by (18) and (19) can be further rewritten as

$${\mathit{x}}_k=f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}$$

(22)

$${\mathit{z}}_k=h\left({\widehat{\mathit{x}}}_{k/k-1}\right)+{\mathit{H}}_k({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k/k-1})+{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}},$$

(23)

where ${\mathit{\delta }}_k^{x\mathrm{G}}$, ${\mathit{\delta }}_k^{y\mathrm{G}}$, ${\overline{\mathit{\delta }}}_k^{x\mathrm{U}}$ and ${\overline{\mathit{\delta }}}_k^{y\mathrm{U}}$ 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$ is ${\widehat{\mathit{x}}}_{k-1}$, whose error covariance is ${\widehat{\mathit{P}}}_{k-1}$. From the KF framework, the state estimate at time $k$ can be obtained by

$${\widehat{\mathit{x}}}_k=f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{K}}_k\left({\mathit{z}}_k-h\left({\widehat{\mathit{x}}}_{k/k-1}\right)\right),$$

(24)

where ${\mathit{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 ${\left(\mathit{x}\right)}^2=\mathit{x}\cdot {\mathit{x}}^{\mathrm{T}}$ for any vector $\mathit{x}$ throughout this paper. Since ${\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}$ is uncorrelated to ${\mathit{\delta }}_k^{x\mathrm{G}}$, ${\mathit{\delta }}_k^{y\mathrm{G}}$, ${\overline{\mathit{\delta }}}_k^{x\mathrm{U}}$ and ${\overline{\mathit{\delta }}}_k^{y\mathrm{U}}$, it is verified from (22)~(24) that the error covariance of ${\widehat{\mathit{x}}}_k$ can be expressed as

$$\begin{array}{ll}& \mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k^{}-{\mathit{x}}_k^{}\right)}^2\right\}\\ =& \mathrm{E}\left\{{\left[\left(f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{K}}_k\left({\mathit{z}}_k-h\left({\widehat{\mathit{x}}}_{k/k-1}\right)\right)\right)-\left(f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)\right]}^2\right\}\\ =& \mathrm{E}\left\{{\left[\left({\mathit{K}}_k\left({\mathit{H}}_k({\mathit{x}}_k-{\widehat{\mathit{x}}}_{k/k-1})+{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}}\right)\right)-\left({\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)\right]}^2\right\}\\ =& \mathrm{E}\left\{{\left[\left({\mathit{K}}_k\left({\mathit{H}}_k\left({\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)+{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}}\right)\right)-\left({\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)\right]}^2\right\}\\ =& \mathrm{E}\left\{{\left[\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\mathit{\delta }}_k^{x\mathrm{G}}+{\mathit{K}}_k{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}+{\mathit{K}}_k{\mathit{\delta }}_k^{y\mathrm{G}}\right]}^2\right\}\\ =& \left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+{\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}+{\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{K}}_k{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}\right)}^2\end{array}$$

(25)

Due to the set-membership of ${\overline{\mathit{\delta }}}_k^{x\mathrm{U}}$ and ${\overline{\mathit{\delta }}}_k^{y\mathrm{U}}$ as shown in (20) and (21), the last term in (25) can be computed as the Minkowki sum, that is,

$$\begin{array}{ll}& \left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{K}}_k{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}\\ \in & \left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\overline{\mathit{Q}}}_k^{x\mathrm{U}}\right)\oplus {\mathit{K}}_k\mathit{\xi }\left({\mathbf{0}}_{m\times 1},{\overline{\mathit{R}}}_k^{x\mathrm{U}}\right)\\ =& \mathit{\xi }\left({\mathbf{0}}_{n\times 1},\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)\oplus \mathit{\xi }\left({\mathbf{0}}_{n\times 1},{\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}\right)\\ \subset & \mathit{\xi }\left({\mathbf{0}}_{n\times 1},\varpi (\rho )\right)\end{array}$$

(26)

in which

$$\varpi (\rho )=\left(1+{\rho }^{-1}\right)\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}$$

(27)

where $\rho \in \left(0,1\right)$ is determined according to (9).

Then, from (25) and (26), the trace of $\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k-{\mathit{x}}_k\right)}^2\right\}$ can be computed by

$$\begin{array}{ll}& trace\left(\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k^{}-{\mathit{x}}_k^{}\right)}^2\right\}\right)\\ =& trace\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)+trace\left({\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\right)\\ & +trace\left({\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{K}}_k{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}\right)}^2\right)\\ \le & trace\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)+trace\left({\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\right)+trace\left(\varpi (\rho )\right)\\ =& trace\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)+trace\left({\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\right)\\ & +trace\left(\left(1+{\rho }^{-1}\right)\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)+trace\left(\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}\right)\end{array}$$

(28)

In order to determine the optimal Kalman gain ${\mathit{K}}_k$ which minimizes the error covariance of ${\widehat{\mathit{x}}}_k^{}$, the condition $\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k^{}-{\mathit{x}}_k^{}\right)}^2\right\}\right)\right)=\mathrm{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

$$\begin{array}{ll}& \frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)\right)\\ =& -\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left({\mathit{K}}_k{\mathit{H}}_k\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right)\right)\right)-\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}{\mathit{K}}_k^{\mathrm{T}}\right)\right)\\ & +\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left({\mathit{K}}_k{\mathit{H}}_k\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}{\mathit{K}}_k^{\mathrm{T}}\right)\right)+\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right)\right)\\ =& -2\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}+2{\mathit{K}}_k{\mathit{H}}_k\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}\end{array}$$

(29)

and

$$\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left({\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\right)\right)=2{\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}$$

(30)

Similar to (29) and (30), the derivations of the third and fourth terms of (28) can be readily given by

$$\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\left(1+{\rho }^{-1}\right)\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}\right)\right)=2\left(1+{\rho }^{-1}\right)\left(-{\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\mathit{H}}_k^{\mathrm{T}}+{\mathit{K}}_k{\mathit{H}}_k{\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\mathit{H}}_k^{\mathrm{T}}\right)$$

(31)

and

$$\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}\right)\right)=2\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}.$$

(32)

Thus, substituting (29)–(32) into the Equation $\frac{\partial }{\partial {\mathit{K}}_k}\left(trace\left(\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k^{}-{\mathit{x}}_k^{}\right)}^2\right\}\right)\right)=\mathbf{0}$, the optimal Kalman gain is yielded

$$\begin{array}{ll}{\mathit{K}}_k=& \left(\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}+\left(1+{\rho }^{-1}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\mathit{H}}_k^{\mathrm{T}}\right)\\ & \cdot {\left(\left(1+{\rho }^{-1}\right){\mathit{H}}_k{\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\mathit{H}}_k^{\mathrm{T}}+\left(1+\rho \right){\overline{\mathit{R}}}_k^{x\mathrm{U}}+{\mathit{H}}_k\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_k^{y\mathrm{G}}\right)}^{-1}\end{array}.$$

(33)

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 ${\mathit{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.

Further, with the obtained optimal Kalman gain, the covariance matrix of ${\widehat{\mathit{x}}}_k$ can be readily obtained by substituting (26) and (27) into (25)

$$\begin{array}{ll}{\widehat{\mathit{P}}}_k& =\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_k^{}-{\mathit{x}}_k^{}\right)}^2\right\}\\ & =\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+{\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}+\mathrm{E}\left\{{\left(\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{K}}_k{\overline{\mathit{\delta }}}_k^{y\mathrm{U}}\right)}^2\right\}\\ & \le \left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+{\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\\ & \hspace{1em}+\left(1+{\rho }^{-1}\right)\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}\end{array}$$

(34)

where $\rho \in \left(0,1\right)$.

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${\mathit{K}}_k$given in (33) can be simplified as

$${\mathit{K}}_k=\left(\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}\right)\cdot {\left({\mathit{H}}_k\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\mathit{H}}_k^{\mathrm{T}}+{\mathit{R}}_k^{y\mathrm{G}}\right)}^{-1},$$

(35)

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 as${\widehat{\mathit{x}}}_{k/k-1}=f\left({\widehat{\mathit{x}}}_{k-1}\right)$. From (22), the covariance matrix of${\widehat{\mathit{x}}}_{k/k-1}$can be represented as

$$\begin{array}{l}{\widehat{\mathit{P}}}_{k/k-1}=\mathrm{E}\left\{{\left({\widehat{\mathit{x}}}_{k/k-1}-{\mathit{x}}_k^{}\right)}^2\right\}\\ \hspace{1em}=\mathrm{E}\left\{{\left[f\left({\widehat{\mathit{x}}}_{k-1}\right)-\left(f\left({\widehat{\mathit{x}}}_{k-1}\right)+{\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)\right]}^2\right\}\\ \hspace{1em}=\mathrm{E}\left\{{\left[\left({\mathit{F}}_k\left({\mathit{x}}_{k-1}-{\widehat{\mathit{x}}}_{k-1}\right)+{\overline{\mathit{\delta }}}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}\right)\right]}^2\right\}\\ \hspace{1em}={\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}+\mathrm{E}\left\{{\left({\overline{\mathit{\delta }}}_k^{x\mathrm{U}}\right)}^2\right\}\\ \hspace{1em}\le {\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}+{\overline{\mathit{Q}}}_k^{x\mathrm{U}}\end{array}$$

(36)

where the result of (20) is employed.

Remark 3.

When using (35) and (36) in practical engineering,${\widehat{\mathit{P}}}_{k/k-1}$and${\widehat{\mathit{P}}}_k$are commonly calculated by

$${\widehat{\mathit{P}}}_{k/k-1}={\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}+{\overline{\mathit{Q}}}_k^{x\mathrm{U}}$$

(37)

$$\begin{array}{ll}{\widehat{\mathit{P}}}_k& =\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)\left({\mathit{F}}_k{\widehat{\mathit{P}}}_{k-1}{\mathit{F}}_k^{\mathrm{T}}+{\mathit{Q}}_k^{x\mathrm{G}}\right){\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+{\mathit{K}}_k{\mathit{R}}_k^{y\mathrm{G}}{\mathit{K}}_k^{\mathrm{T}}\\ & +\left(1+{\rho }^{-1}\right)\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right){\overline{\mathit{Q}}}_k^{x\mathrm{U}}{\left({\mathit{K}}_k{\mathit{H}}_k-\mathit{{\rm I}}\right)}^{\mathrm{T}}+\left(1+\rho \right){\mathit{K}}_k{\overline{\mathit{R}}}_k^{x\mathrm{U}}{\mathit{K}}_k^{\mathrm{T}}\end{array}.$$

(38)

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.

3.3. 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 1. Give the state estimate ${\widehat{\mathit{x}}}_{k-1}$ and its error covariance ${\widehat{\mathit{P}}}_{k-1}$.

Step 2. Calculate the predicted state estimate by ${\widehat{\mathit{x}}}_{k/k-1}=f\left({\widehat{\mathit{x}}}_{k-1}\right)$.

Step 3. Compute the optimal Kalman gain given by (33), in which (20) and (21) are used.

Step 4. Calculate the state estimation ${\widehat{\mathit{x}}}_k$ and its covariance matrix ${\widehat{\mathit{P}}}_k$ by (24) and (38), respectively.

Step 5. Repeat Steps 1 to 4 for the next time step.

4. 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 ${\mathit{x}}_k={\left[\begin{array}{cccc}{s}_{\mathrm{E}}& s_{\mathrm{N}}& v_{\mathrm{E}}& v_{\mathrm{N}}\end{array}\right]}^{\mathrm{T}}$, the process model and measurement model are given by

$${\mathit{x}}_k=f\left({\mathit{x}}_{k-1}\right)+{\mathit{\delta }}_k^{x\mathrm{U}}+{\mathit{\delta }}_k^{x\mathrm{G}}$$

(39)

$${\mathit{z}}_k=h\left({\mathit{x}}_k\right)+{\mathit{\delta }}_k^{y\mathrm{U}}+{\mathit{\delta }}_k^{y\mathrm{G}},$$

(40)

where ${\mathit{\delta }}_k^{x\mathrm{G}}$ and ${\mathit{\delta }}_k^{y\mathrm{G}}$ are the zero-mean Gaussian white noises (i.e., stochastic errors); ${\mathit{\delta }}_k^{x\mathrm{U}}$ and ${\mathit{\delta }}_k^{y\mathrm{U}}$ are the UBB errors; and

$$f\left({\mathit{x}}_{k-1}\right)=\left[\begin{array}{cccc}1& 0& t& 0\\ 0& 1& 0& t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{\mathit{x}}_{k-1}$$

(41)

$$h\left({\mathit{x}}_k\right)=\left[\begin{array}{c}\sqrt{s_{\mathrm{E}}{}^2+s_{\mathrm{N}}{}^2}\\ \arctan \left(s_{\mathrm{N}}/s_{\mathrm{E}}\right)\end{array}\right]$$

(42)

The covariance matrices of the Gaussian white noises in the process and measurement models are assumed to be

$${\mathit{Q}}_k^{x\mathrm{G}}=\left[\begin{array}{cccc}\raisebox{1ex}{$t^3$}\!\left/ \!\raisebox{-1ex}{$30$}\right.& 0& \raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$20$}\right.& 0\\ 0& \raisebox{1ex}{$t^3$}\!\left/ \!\raisebox{-1ex}{$30$}\right.& 0& \raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$20$}\right.\\ \raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$20$}\right.& 0& \raisebox{1ex}{$t$}\!\left/ \!\raisebox{-1ex}{$10$}\right.& 0\\ 0& \raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$20$}\right.& 0& \raisebox{1ex}{$t$}\!\left/ \!\raisebox{-1ex}{$10$}\right.\end{array}\right]$$

(43)

$${\mathit{R}}_k^{z\mathrm{G}}=\left[\begin{array}{cc}\raisebox{1ex}{${0.3}^2$}\!\left/ \!\raisebox{-1ex}{$10$}\right.& 0\\ 0& \raisebox{1ex}{${0.1}^2$}\!\left/ \!\raisebox{-1ex}{$10$}\right.\end{array}\right]$$

(44)

where the constant $t$ is the sampling interval.

The shape matrix of the bounding ellipsoid describing the UBB error in the process model is

$${\mathit{Q}}_k^{x\mathrm{U}}=\left[\begin{array}{cccc}{a}_x\raisebox{1ex}{$t^3$}\!\left/ \!\raisebox{-1ex}{$3$}\right.& 0& a_x\raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.& 0\\ 0& a_y\raisebox{1ex}{$t^3$}\!\left/ \!\raisebox{-1ex}{$3$}\right.& 0& a_y\raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.\\ a_x\raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.& 0& a_{x}t& 0\\ 0& a_y\raisebox{1ex}{$t^2$}\!\left/ \!\raisebox{-1ex}{$2$}\right.& 0& a_{y}t\end{array}\right]$$

(45)

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

$${\mathit{R}}_k^{y\mathrm{U}}=\mathbf{0}$$

(46)

The vehicle accelerations in East and North as well as the simulated vehicle trajectory are shown in Figure 1 and Figure 2. The vehicle acceleration variations are presented in

Table 1. Vehicle acceleration variations.

Time Segment	Vehicle Acceleration Variations
	East	North
(1–10 s)	Increase: (1.5 m/s2)/s	Increase: (1.0 m/s2)/s
(301–310 s)	Increase: (0.1 m/s2)/s	Decrease: (1.0 m/s2)/s
(501–510 s)	Decrease: (1.0 m/s2)/s	Decrease: (1.2 m/s2)/s
(901–910 s)	Increase (1.5 m/s2)/s	Increase: (0.2 m/s2)/s
Others	Stochastic fluctuation obeys N(0, 0.22)

, 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.

Figure 3 and Figure 4 depict the position estimations of the vehicle achieved by EKF and the proposed SM-HKF. It can be seen that, in the time segments without acceleration variations, that is, without 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.

Figure 5 and Figure 6 show the vehicle velocities estimated by EKF and SM-HKF in the time segment (490 s, 520 s) involving significant UBB errors, where a similar phenomenon as in Figure 3 and Figure 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. Figure 7 and Figure 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 RMSEs of estimation errors obtained by EKF and SM-HKF during the time segment with unknown but bounded (UBB) errors.

Methods		Positions (m)	Velocities (m/s)
EKF	East	18.30	9.56
	North	16.51	9.17
SM-HKF	East	4.68	1.64
	North	4.04	1.59

, 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 improved robustness against systematic uncertainty and the higher filtering accuracy than EKF for nonlinear state estimation.

5. 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.