An Improved Smooth Variable Structure Filter for Robust Target Tracking

Abstract

As a new-style filter, the smooth variable structure filter (SVSF) has attracted significant interest. Based on the predictor-corrector method and sliding mode concept, the SVSF is more robust in the face of modeling errors and uncertainties compared to the Kalman filter. Since the estimation performance is usually insufficient in real cases where the measurement vector is of fewer dimensions than the state vector, an improved SVSF (ISVSF) is proposed by combining the existing SVSF with Bayesian theory. The ISVSF contains two steps: firstly, a preliminary estimation is performed by SVSF. Secondly, Bayesian formulas are adopted to improve the estimation for higher accuracy. The ISVSF shows high robustness in dealing with modeling uncertainties and noise. It is noticeable that ISVSF could deliver satisfying performance even if the state of the system is undergoing a sudden change. According to the simulation results of target tracking, the proposed ISVSF performance can be better than that obtained with existing filters.

1. Introduction

State estimation of dynamic systems has been widely used in various engineering fields, such as target tracking, navigation, signal processing, computer vision, automatic control, etc. [1,2]. However, a variety of noise and interference have made systems more complex and changeable. This makes accurate information about noise statistics and system models not readily available. Besides that, the system state may have a sudden change, which means that when a state encounters unknown external interference, it may change suddenly and dramatically in forms similar to the sinusoid wave and rectangular wave. As a result, efforts to develop new methods to improve system robustness and estimation accuracy have been under active consideration recently.

Various filters have been developed to estimate the system state value according to the measurements. The Kalman filter (KF) [3], the most widely used filter in linear Gaussian systems, is the optimal method under the criteria of minimum mean square error, maximum likelihood and maximum posterior. However, in nonlinear systems, the KF may be affected by divergence. So, a variety of filters have been developed for better estimation performance in a nonlinear system, which mainly can be divided into three categories. In the first category, the nonlinear system is simplified into a linear one. A typical example in this category is the extended Kalman filter (EKF) [4,5], in which the nonlinear system is simplified as a linear one by only utilizing the first-order Taylor expansion and discarding the higher-order terms. In the second category, the probability density distribution of a nonlinear function is replaced by selecting a certain number of sigma points, and the posterior probability density are calculated by these points. These methods have been termed “numerical integration methods” [6]. The two most widely used methods of this category are the unscented Kalman filter (UKF) and the cubature Kalman filter (CKF) [7]. The last category is based on particle filter methods, which work well in nonlinear and non-Gaussian systems but are time-consuming in iterative computation [8,9]. Given that, the particle filters may not meet the time requirement in some real cases. Some cutting-edge methods such as the generalized regression neural network have also been applied in target tracking [10,11].

Knowledge of the mathematical model and noise statistics is essential in a majority of existing methods, and the robustness of these methods would be severely reduced if inappropriate modeling or noise modeling are used. Therefore, filters of higher robustness have been proposed to deal with uncertainties of models and noise. For example, one of the most widely used methods in handling model uncertainty is an adaptive strategy using “multiple models” based on the Bayesian framework. Additionally, the strategy usually implements the interactive multi-model method [1] or the variable structure interactive multi-model [12]. In real cases, various noises need to be reduced, such as the heavy-tailed noise [13], non-Gaussian distributions noise [14] and Student’s t distribution noise [15]. The combination of noise models and the adaptive methods in [13,14,15] have achieved good results. However, all of the mentioned algorithms are built on the Kalman filter, and thus, some shortcomings still exist, such as sensitivity to parameters, etc. In order to improve the robustness of the filter in terms of model uncertainties and anti-noise interference ability, the smooth variable structure filter (SVSF) has been developed [16].

The SVSF [16] is a recently developed filter that is designed to handle the issue of model uncertainty and noise interference. The estimated state of the SVSF is constrained to fluctuate around the true state trajectory within a small region and shows high robustness and stability. The SVSF is simple in structure and feasible to implement. However, it is regarded as a suboptimal filter [7,17] due to its low accuracy. Therefore, [18] attempts have been made to refine the SVSF by deducing the its state error covariance. They show that the priori and posteriori state error covariance matrices of the SVSF are similar to the KF in linear systems, and are expected to extend to other applications. The authors of [19] further deduced the optimal bounded layer width of SVSF based on the posterior state covariance matrix. Based on this, the SVSF is combined with other filters (such as KF, EKF, UKF, and CKF), which can take advantage of both the robustness of the SVSF and the accuracy of these filters [7,17,20,21]. For example, combinations of SVSF with the UKF (UK-SVSF) or CKF (CK-SVSF) filtering systems [20] are better than separate methods, but will be affected by their own disadvantages and interference with each other. In [22], the dependence of the estimation performance on parameters is avoided by an uncertainty learning strategy. Additionally, the second-order SVSF [23] and other methods [20,24] have been proposed to improve the stability and robustness. Since its development, the SVSF has been applied in various applications [25], such as vehicle navigation [24,26,27,28,29,30,31], fault detection and diagnosis [32,33], battery management [34,35,36,37], and artificial intelligence [33,38,39]. However, it was found that the SVSF can be further improved if two shortcomings can be solved. (1) The SVSF is generally used in systems with one measurement corresponding to each state variable. When the dimension of the measurement vector is less than that of the state variables, the original SVSF cannot estimate the lower partition of the state vector. For example, tracking sensors usually measure the target position but do not record the velocity and acceleration of the target. Thus, [16] developed a similar Luenberger’s reduced order observer strategy, which constructs a reduced order estimator to estimate the lower partition of the state vector. However, the system is required to be completely observable and completely controllable, and meanwhile, the calculation process for the lower partition of the state vector needs to be independent, which greatly limits the versatility of the method. Another popular solution [17,21] is to construct an “artificial” measurement of the lower partition of the state vector, which is portable and widely used. However, the estimation of the lower partition of the state vector cannot achieve satisfactory results when the measurement noise intensity is high. (2) In short, compared with filters based on Bayesian theory, the SVSF is insufficient to suppress noise. If the noise can be further eliminated, the performance of the SVSF can be greatly improved and the SVSF could be advantageous in more applications.

An improved smooth variable structure filter (ISVSF), devoted to solving the above described shortcomings, is proposed here. It is built on the basis of the SVSF and inspired by the Bayesian theory [40]. In line with the SVSF strategy, the proposed ISVSF deduces the priori and posterior error covariance matrix of the state variables in the linear system. The Bayesian method is exploited to calculate the new state estimation by means of estimated state and error covariance of SVSF.

Three main advantages of the ISVSF are as follows. (1) The ISVSF can maintain high robustness even if the system state is undergoing sudden change or the modeling is deviating from the actual model. (2) Compared with the original SVSF, the exploited Bayesian method contributes to a higher estimation accuracy. Meanwhile, the obtained error covariance matrix is helpful in further improving application with other methods, such as the interacting multiple model methods or constructing smoother. (3) When the dimension of the state variables is larger than that of the measurements, the state error covariance will be exploited to estimate the lower partition of the state vector in ISVSF. ISVSF is a newer strategy than the previous method [16,17] of the SVSF system, and it has computational complexity almost identical to that of the Kalman filter.

In general, this paper proposes the real-time ISVSF as it could achieve the high accuracy of the Bayesian solution and robustness similar to that of the SVSF. The ISVSF proposed here also can be applied in systems with states undergoing sudden changes. However, our former work [41] proposed a non real-time smoother, which is also applied in maneuvering target tracking. The rest of the paper is structured as follows: The review of the SVSF will be presented in Section 2, the elaborate explanation of the proposed ISVSF algorithm in Section 3, the simulation process and an analysis in Section 4 and conclusions of the main findings in this paper in Section 5.

Table 1. The SVSF Strategies.

Symbol	Definition
^	Notation denoting an estimated variable, function, or model parameter
~	Notation denoting the error in an estimated variable, function, or model parameter
$\mathbf{F},\widehat{\mathbf{F}},\tilde{\mathbf{F}}$	System model, mathematical modeling, and modeling error
$\mathbf{H}$	measurement (out) matrix
${\mathbf{x}}_k,{\widehat{\mathbf{x}}}_k,{\tilde{\mathbf{x}}}_k$	System state, prior estimate state, state error
${\mathbf{e}}_{k+1|k}^{\mathit{svsf}},{\mathbf{e}}_{k+1|k+1}^{\mathit{svsf}}$	The priori and posterior measurement innovation of SVSF
${\mathbf{e}}_{k+1|k+1}$	A posterior error
$\mathit{diag}$	Diagonal of vector or matrix
$\mathit{sat}$	Saturation function
$\mathbf{Q},\mathbf{R}$	System and measurement covariance matrix
$|\mathbf{e}|$	Absolute value of $\mathbf{e}$
+	Pseudoinverse of some non-square matrix
$•$	Indication or Hadamard product
$T$	Transpose of a vector or sample rate
$\psi$	SVSF smoothing boundary layer
${\Re }^n$	Matrix dimension
${\mathbf{K}}_{k+1}^{\mathit{svsf}}$	SVSF gain matrix
${\mathbf{K}}_{k+1}$	Bayes’ rule gain
$\mathrm{E}$	Expectation
$f,h$	Nonlinear function
$\gamma$	SVSF “memory” or convergence
$p$	Probability density function
$\beta$	Existence subspace layer
$\mathbf{P}$	State error covariance matrix
${\mathbf{z}}_k$	Measurement (system output) matrix
$\mathbf{w},\mathbf{v}$	System noise and measurement noise vector

provides a list of nomenclature used throughout the paper.

2. The SVSF Strategies

2.1. The SVSF

In 2003, Habibi and Burton proposed a novel form of predictor-corrector filter, called the variable structure filter (VSF) [42]. The VSF uses sliding mode concepts to construct a switching gain, which makes the state estimates converge within a boundary of the real value to ensure system stability. Based on the VSF, the SVSF was proposed with a less complex gain in 2007 [16]. As shown in Figure 1, the true trajectory of the system has some fluctuations caused by dynamic interference or disturbance noises. The estimated state trajectory gradually approaches the real trajectory until it reaches the existence subspace. If it remains in the existence subspace, the estimated state is forced to switch back and forth on the real state trajectory by SVSF gain [24]. If the errors caused by modeling error and noise are bounded, then the state estimates will be kept within a limit. The SVSF strategy ensures stability and robustness in resolving modeling uncertainties and errors.

In the case of linear dynamic systems under a zero-mean and Gaussian noise, the state equation and observation equation of a dynamic system are as follows:

$$\{\begin{array}{l}{\mathbf{x}}_{k+1}=\mathbf{F}{\mathbf{x}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_{k+1}=\mathbf{H}{\mathbf{x}}_{k+1}+{\mathbf{v}}_{k+1}\end{array}$$

(1)

where $k$ is the discrete time, ${\mathbf{x}}_k\in {\Re }^n$ is the state vector, and $\mathbf{F}\in {\Re }^{n\times n}$ is the system model matrix or the state transition matrix. ${\mathbf{z}}_k\in {\Re }^m$ is the measurement, $\mathbf{H}\in {\Re }^{m\times n}$ is the linear measurement (out) matrix, ${\mathbf{w}}_k\in {\Re }^n$ and ${\mathbf{v}}_k\in {\Re }^m$ are the process noise vector and the measurement noise, respectively, and their covariances are represented by ${\mathbf{R}}_k$ and ${\mathbf{Q}}_k$, respectively.

The iterative process of SVSF contains two steps and can be summarized as follows:

The prediction stage: the predicted state estimate ${\widehat{\mathbf{x}}}_{k+1|k}$ is calculated according to the system model matrix $\mathbf{F}$ and the previous state estimate ${\widehat{\mathbf{x}}}_{k|k}$.

$${\widehat{\mathbf{x}}}_{k+1|k}=\mathbf{F}{\widehat{\mathbf{x}}}_{k|k}$$

(2)

A priori estimated measurement ${\widehat{\mathbf{z}}}_{k+1|k}$ is calculated according to the priori estimated state ${\widehat{\mathbf{x}}}_{k+1|k}$ and the measurement matrix $\mathbf{H}$ is as follows:

$${\widehat{\mathbf{z}}}_{k+1|k}=\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k}$$

(3)

The priori measurement innovations ${\mathbf{e}}_{k+1|k}^{\mathit{svsf}}$ are calculated in SVSF as:

$${\mathbf{e}}_{k+1|k}^{\mathit{svsf}}={\mathbf{z}}_{k+1}-{\widehat{\mathbf{z}}}_{k+1|k}$$

(4)

The updating stage: the SVSF’s corrective gain ${\mathbf{K}}_{k+1}^{\mathit{svsf}}$ [16] is calculated as follows:

$$\begin{array}{ll}{\mathbf{K}}_{k+1}^{\mathit{svsf}}=& {\mathbf{H}}^{+}\mathit{diag}\left[\right(|{\mathbf{e}}_{k+1}^{\mathit{svsf}}|+\gamma |{\mathbf{e}}_{k|k}|)\\ & •\mathit{sat}({\overline{\psi }}^{-1}{\mathbf{e}}_{k+1}^{\mathit{svsf}})\left]\right[\mathit{diag}({\mathbf{e}}_{k+1}^{\mathit{svsf}}){]}^{-1}\end{array}$$

(5)

where ${\mathbf{K}}_{k+1}^{\mathit{svsf}}$ is a gain function, ${\mathbf{H}}^{+}$ is the pseudoinverse matrix of $\mathbf{H}$, and the $\mathit{diag}$ function is to construct diagonal matrix. $|\mathbf{e}|$ is the absolute value of $\mathbf{e}$, $\gamma$($0\le \gamma <1$) is the SVSF’s “memory” (convergence rate), and $•$ represents the Hadamard product. $sat$ is a saturation function. Besides that,

$$\mathit{sat}\left({\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}\right)=\{\begin{array}{ll}1,& {\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}\ge 1\\ {\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}},& -1<{\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}<1\\ -1,& {\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}\le -1\end{array}$$

(6)

and

$${\overline{\psi }}^{-1}=\left[\begin{array}{ccc}\frac{1}{{\psi }_1}& 0& 0\\ 0& \ddots & 0\\ 0& 0& \frac{1}{{\psi }_m}\end{array}\right]$$

(7)

where ${\psi }_i$ ($i=1\dots m$) represents the width of the smoothing boundary layer in every dimension, the value setting is described in [16,19], and $m$ is the dimension of state estimation. The estimated state ${\widehat{\mathbf{x}}}_{k+1|k+1}$ is updated as follows:

$${\widehat{\mathbf{x}}}_{k+1|k+1}={\widehat{\mathbf{x}}}_{k+1|k}+{\mathbf{K}}_{k+1|k}^{\mathit{svsf}}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}$$

(8)

The posterior measurement estimate ${\widehat{\mathbf{z}}}_{k+1|k+1}$ and its innovation ${\mathbf{e}}_{k+1|k+1}$ are described as:

$${\widehat{\mathbf{z}}}_{k+1|k+1}=\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k+1}$$

(9)

$${\mathbf{e}}_{k+1|k+1}={\mathbf{z}}_{k+1}-{\widehat{\mathbf{z}}}_{k+1|k+1}$$

(10)

The influence of existence subspaces on estimation is shown in Figure 2. The width of the existence subspace $\beta$ is unknown and dynamic, and contains modeling errors and noises. Additionally, as indicated by [16], an upper bound can be selected according to a priori knowledge [17]. As shown in Figure 2, once the estimated state enters the existence subspace, the estimated state (by SVSF gain) will remain in the existence subspace and switch back and forth along the true state trajectory [17], and a high-frequency switching is called chattering [24]. The smoothing boundary layer $\psi$ can decrease the chattering amplitude and make the estimated state trajectory smoother. The smoothing boundary layer $\psi$ can reflect the level of interference and uncertainties. As shown in Figure 2a, when $\psi \le \beta$, the uncertain factors are underestimated [16] and the filter gain is more easily affected by the uncertain factors; therefore, the chattering appears. However, when the smoothing boundary layer is larger than the existence boundary layer ($\psi >\beta$), the chattering effects will be removed [24], as shown in Figure 2b.

SVSF is applied in systems where the state variables and measurement vector share the same dimension. Compared with the KF, SVSF has to rely on Luenberger’s strategy [16] or the “artificial” measurement [17] if measurements are less than states.

2.2. Review of Combining SVSF with Other Estimation Strategies

The development, improvement and application of the SVSF and its variants are discussed in the introduction [20]. Particularly, among the derivative methods of SVSF, the combination of the SVSF with different filtering is an effective solution to improve accuracy while preserving robustness even if model uncertainties exist [20]. To satisfy different demands, SVSF has been combined with EKF (EK-SVSF), UKF (UK-SVSF) and CKF (CK-SVSF). Those methods have the same structure, and the difference between SVSF and those combinations lies in the calculation of the gain as shown in Figure 3a. Given that, the calculated time varying smoothing boundary layer ${\psi }_{\mathit{vbl}}$ is compared with the fixed varying smoothing boundary layer ${\psi }_{\mathit{fixed}}$ to determine which gain will be utilized (e.g., SVSF or other filters); if ${\psi }_{\mathit{vbl}}\le {\psi }_{\mathit{fixed}}$,the KF-based gain will be used to obtain optimality. If ${\psi }_{\mathit{vbl}}>{\psi }_{\mathit{fixed}}$, the standard SVSF gain is used to keep robustness at the cost of estimation accuracy.

The UK-SVSF is one of popular methods in the combination strategies and has been applied in many different systems [20,21,43,44]. For better understanding of the combination strategy, the UK-SVSF is chosen and its specific process is summarized as follows. The UKF uses the $2n+1$ sigma points to estimate state. The sigma points and their corresponding weights are selected based on the following rules.

$${\mathbf{X}}_{0,k|k}={\widehat{\mathbf{x}}}_{k|k}$$

(11)

$${\mathbf{W}}_0=\frac{\kappa }{n+\kappa }$$

(12)

where the ${\mathbf{X}}_{0,k|k}$ is the first sigma point and ${\mathbf{W}}_0$ is its corresponding weight, $n$ is $n$-dimensional of state, $\kappa$ is a design value (a small value less than 1 or that can be calculated by $\kappa =3-n$), and the next $i=1,\dots ,n$ points and corresponding weights can be computed as:

$${\mathbf{X}}_{i,k|k}={\widehat{\mathbf{x}}}_{k|k}+{\left(\sqrt{\left(n+\kappa \right){\mathbf{P}}_{k|k}}\right)}_i$$

(13)

$${\mathbf{W}}_i=\frac{1}{2(n+\kappa )}$$

(14)

where ${\left(\sqrt{\left(n+\kappa \right){\mathbf{P}}_{k|k}}\right)}_i$ is the $i-\mathrm{th}$ column of the square root of $\left(n+\kappa \right){\mathbf{P}}_{k|k}$, and ${\mathbf{W}}_i$ is associated with the ${\mathbf{X}}_{i,k|k}$ sigma points. The rest of the $i=n+1,\dots ,2n$ points and their corresponding weights may be derived as:

$${\mathbf{X}}_{i,k|k}={\widehat{\mathbf{x}}}_{k|k}-{\left(\sqrt{\left(n+\kappa \right){\mathbf{P}}_{k|k}}\right)}_i$$

(15)

$${\mathbf{W}}_i=\frac{1}{2(n+\kappa )}$$

(16)

The propagation of these sigma points can be calculated as:

$${\widehat{\mathbf{X}}}_{i,k+1|k}=f({\widehat{\mathbf{X}}}_{i,k|k})$$

(17)

The predicted state estimate and predicted state error covariance are calculated as follows:

$${\widehat{\mathbf{x}}}_{k+1|k}={\displaystyle \sum _{i=0}^{2n}{\mathbf{W}}_i{\widehat{\mathbf{X}}}_{i,k+1|k}}$$

(18)

$${\mathbf{P}}_{k+1|k}={\displaystyle \sum _{i=0}^{i=2n}{\mathbf{W}}_i({\widehat{\mathbf{X}}}_{i,k+1|k}-{\widehat{\mathbf{x}}}_{k+1|k})}{({\widehat{\mathbf{X}}}_{i,k+1|k}-{\widehat{\mathbf{x}}}_{k+1|k})}^{\mathbf{T}}+{\mathbf{Q}}_{k+1}$$

(19)

The nonlinear measurement propagation can be calculated as follows:

$${\widehat{\mathbf{Z}}}_{i,k+1|k}=h\left({\widehat{\mathbf{X}}}_{i,k+1|k}\right)$$

(20)

where $h$ is the measurement function. The predicted measurement ${\widehat{\mathbf{z}}}_{k+1|k}$ and the priori measurement innovation ${\mathbf{e}}_{k+1|k}$ can be calculated as follows:

$${\widehat{\mathbf{z}}}_{k+1|k}={\displaystyle \sum _{i=0}^{2n}{\mathbf{W}}_i{\widehat{\mathbf{Z}}}_{i,k+1|k}}$$

(21)

$${\mathbf{e}}_{k+1|k}={\mathbf{z}}_k-{\widehat{\mathbf{z}}}_{k+1|k}$$

(22)

The predicted measurement covariance and the cross-covariance (between state and measurement) are calculated as follows:

$${\mathbf{P}}_{\mathbf{z}\mathbf{z},k+1|k}={\displaystyle \sum _{i=0}^{i=2n}{\mathbf{W}}_i({\widehat{\mathbf{Z}}}_{i,k+1|k}-{\widehat{\mathbf{z}}}_{k+1|k})}{({\widehat{\mathbf{Z}}}_{i,k+1|k}-{\widehat{\mathbf{z}}}_{k+1|k})}^{\mathbf{T}}+{\mathbf{R}}_{k+1}$$

(23)

$${\mathbf{P}}_{\mathbf{x}\mathbf{z},k+1|k}={\displaystyle \sum _{i=0}^{i=2n}{\mathbf{W}}_i({\widehat{\mathbf{X}}}_{i,k+1|k}-{\widehat{\mathbf{x}}}_{k+1|k})}{({\widehat{\mathbf{Z}}}_{i,k+1|k}-{\widehat{\mathbf{z}}}_{k+1|k})}^{\mathbf{T}}$$

(24)

From (23) and (24), the UKF gain can be computed as follows:

$${\mathbf{K}}_{\mathit{UKF},k+1}={\mathbf{P}}_{\mathbf{x}\mathbf{z},k+1|k}{\mathbf{P}}_{\mathbf{z}\mathbf{z},k+1|k}^{-1}$$

(25)

The time varying boundary layer width is calculated as follows:

$${\psi }_{\mathit{vbl}}=\left({\mathbf{A}}^{-1}\mathbf{H}{\mathbf{P}}_{k+1|k}{\mathbf{H}}^{\mathbf{T}}{\mathbf{P}}_{\mathbf{z}\mathbf{z},k+1|k}^{-1}\right)$$

(26)

where

$$\mathbf{A}=\mathit{diag}(|{\mathbf{e}}_{k+1|k}|+\gamma |{\mathbf{e}}_{k|k}|)$$

(27)

If ${\psi }_{\mathit{vbl}}\le {\psi }_{\mathit{fixed}}$, the gain is updated by ${\mathbf{K}}_{k+1}={\mathbf{K}}_{\mathit{UKF},k+1}$, otherwise when ${\psi }_{\mathit{vbl}}>{\psi }_{\mathit{fixed}}$, ${\mathbf{K}}_{k+1}={\mathbf{K}}_{k+1}^{\mathit{svsf}}$ is derived through the SVSF-based Equation (5). So, the state is updated as follows:

$${\widehat{\mathbf{x}}}_{k+1|k+1}={\widehat{\mathbf{x}}}_{k+1|k}+{\mathbf{K}}_{k+1}{\mathbf{e}}_{k+1|k}$$

(28)

Finally the covariance and the posteriori error are updated as follows:

$${\mathbf{P}}_{k+1|k+1}=(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H}){\mathbf{P}}_{k+1|k}{(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H})}^T+{\mathbf{K}}_{k+1}{\mathbf{R}}_{k+1}{\mathbf{K}}_{k+1}^T$$

(29)

$${\mathbf{e}}_{k+1|k+1}={\mathbf{z}}_{k+1}-h\left({\widehat{\mathbf{x}}}_{k+1|k+1}\right)$$

(30)

However, those methods still have disadvantages. When the dimension of the measurement vector is less than that of the state variables, the original SVSF cannot estimate the lower partition of the state vector, and thus the accuracy will be decreased. If ${\psi }_{\mathit{vbl}}>{\psi }_{\mathit{fixed}}$ appears frequently, the accuracy of the combination methods will decrease because the standard SVSF gain is used. However the proposed ISVSF can overcome this shortcoming.

3. Methodology for the Proposed ISVSF

3.1. Motivation of This Work

A generalized version of the SVSF cannot estimate the lower partition of the state vector when there are fewer dimensions of measurements than state, which can be overcome in Bayesian filters. This kind of filter, based on Bayesian theory, uses the state error covariance to obtain the probability density function and calculate the optimal gain to estimate the state value. Since the state error covariance can reflect a positive correlation among different dimensions of the state vector, even if the lower partition of the state vector has no corresponding measurements, it can still be precisely estimated. Inspired by this, the ISVSF was integrated with the Bayesian theory to estimate the lower partition of the state vector, which cannot been estimated in the SVSF.

Many practical algorithms based on the Bayesian filtering method are widely used, such as KF, EKF, UKF, PF, etc. From the Bayesian viewpoint, the estimation is to recursively calculate a certain degree of belief in the state ${\mathbf{x}}_{k+1}$ at the time $k+1$, by substituting ${\mathbf{x}}_k$ at time $k$ and ${\mathbf{z}}_{k+1}$ at time $k+1$.

In light of the system, the state-space model can be defined with the following equations at discrete-time.

Process equation:

$${\mathbf{x}}_{k+1}=f_k\left({\mathbf{x}}_k\right)+{\mathbf{w}}_k$$

(31)

Measurement equation:

$${\mathbf{z}}_{k+1}=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k$$

(32)

where $f_k:{\Re }^{n_x}\times {\Re }^{n_w}\to {\Re }^{n_x}$ is a function of the state ${\mathbf{x}}_k$, $h_k:{\Re }^{n_x}\times {\Re }^{n_v}\to {\Re }^{n_z}$ is also a known function,$n_{\mathbf{x}},n_{\mathbf{w}},n_{\mathbf{z}},n_{\mathbf{v}}$ are dimension of the state, process noise, measurement noise and measurement vector, respectively, and $\left\{{\mathbf{w}}_k\right\}$ and $\left\{{\mathbf{v}}_k\right\}$ are independent process and measurement noise sequences.

The initial probability density function (PDF) $p({\mathbf{x}}_0|{\mathbf{z}}_0)$ is known, and PDF $p({\mathbf{x}}_k|{\mathbf{z}}_{1:k})$ at time $k$ can be obtained by iterative computing. In the prediction stage, the prior PDF of the state at time $k+1$ can be calculated by (31) and the Chapman–Kolmogorov equation.

$$p({\mathbf{x}}_{k+1}|\mathbf{z}{)}_{1:k}={\displaystyle \int p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)}p({\mathbf{x}}_k|{\mathbf{z}}_{1:k})d{\mathbf{x}}_k$$

(33)

Based on a first-order Markov process, the $p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)$ can be calculated by (31) and the known statistics of ${\mathbf{w}}_k$.

In the updating stage, the measurement ${\mathbf{z}}_{k+1}$ is obtained at time $k+1$, and the state may be used to update the prior $p({\mathbf{x}}_{k+1}|{\mathbf{x}}_k)$ through Bayes’ rule:

$$p({\mathbf{x}}_{k+1}|{\mathbf{z}}_{1:k+1})=\frac{p({\mathbf{z}}_{k+1}|{\mathbf{x}}_{k+1}\left)p\right({\mathbf{x}}_{k+1}|{\mathbf{z}}_{1:k})}{p({\mathbf{z}}_{k+1}|{\mathbf{z}}_{1:k})}$$

(34)

where the normalizing constant is

$$p({\mathbf{z}}_{k+1}|{\mathbf{z}}_{1:k})={\displaystyle \int p({\mathbf{z}}_{k+1}|{\mathbf{x}}_{k+1})p({\mathbf{x}}_{k+1}|{\mathbf{z}}_{1:k})}d{\mathbf{x}}_{k+1}$$

(35)

$p({\mathbf{z}}_{k+1}|{\mathbf{x}}_{k+1})$ is obtained by (32) and the known statistics of ${\mathbf{v}}_k$. According to (34), the posterior density $p({\mathbf{x}}_{k+1}|{\mathbf{z}}_{1:k+1})$ at time $k+1$ is obtained. Equations (33) and (34) form the basis for KF, EKF, UKF, PF, etc. When the noise statistics and the model of (31), (32) are known, the Bayesian filter can achieve relatively satisfactory performance.

The ISVSF replaces Equation (31) with the SVSF to predict the state value and obtain the prior state PDF. Figure 3b is the flowchart of the proposed ISVSF, whose process can be divided into two steps. The main purpose of step 1 is to reduce the uncertainty of the modeling and serious external interferences. In this step, the state and its error covariance are estimated by the SVSF. To this end, the reformulated state error covariance of the SVSF can be used to calculate the state PDF. The results of state and the prior state PDF in step 1 can be utilized to calculate new estimation results by means of Bayes’ rule in step 2. Finally, the outputs are the revised estimated state and state covariance. The revised state value at the final stage contains the estimated lower partition of the state vector when $n_{\mathbf{x}}>n_{\mathbf{z}}$. Since the SVSF is also a predictor-correct estimator, its lower partition of the state vector can help improve the forecast precision of the model and eventually improve the estimation accuracy.

3.2. The Proposed ISVSF Derivation

The state error covariance matrix, which has many functions, is widely used in Bayesian filters. It can indicate the differences between the actual and the estimated values, and can also reflect the correlation among different state dimensions. The original SVSF is based on sliding mode concepts, and it has no state error covariance matrix and no use of the state error covariance matrix in the estimation process. Figure 4 shows the complete calculation process and iterative calculation steps of the proposed method in detail. As shown in Figure 4, the derivation of the SVSF covariance is added in the proposed method, and then used in a new gain calculation. The estimated state value of the SVSF is corrected by the new gain, and finally the new state and its error covariance can be obtained. Figure 4 is the ISVSF algorithm block diagram. A more sophisticated formula derivation in Figure 4 is as follows.

3.2.1. Step 1: The SVSF Estimation Process

Based on the linear system, the complete derivation process is shown as follows:

$${\mathbf{x}}_{k+1}=\mathbf{F}{\mathbf{x}}_k+{\mathbf{w}}_k$$

(36)

where ${\mathbf{x}}_k$ is the real value at time $k$, $\mathbf{F}$ is the model of the system, which may be variable and unknown, and ${\mathbf{w}}_k$ is the noise. For systems with uncertain models, the predictor of SVSF can be described as follows:

$${\widehat{\mathbf{x}}}_{k+1|k}=\widehat{\mathbf{F}}{\widehat{\mathbf{x}}}_{k|k}$$

(37)

where $\widehat{\mathbf{F}}$ is the estimated model by using mathematical modeling, ${\widehat{\mathbf{x}}}_{k+1|k}$ is the predicted value, and the a priori state error ${\tilde{\mathbf{x}}}_{k+1|k}$ is deduced by:

$${\tilde{\mathbf{X}}}_{k+1|k}={\mathbf{X}}_{k+1}-{\widehat{\mathbf{X}}}_{k+1|k}.$$

(38)

Substitution of (36) and (37) into (38) yields:

$$\begin{array}{ll}{\tilde{\mathbf{x}}}_{k+1|k}& =\mathbf{F}{\mathbf{x}}_k+{\mathbf{w}}_k-\widehat{\mathbf{F}}{\widehat{\mathbf{x}}}_{k|k}\\ & =(\mathbf{F}-\widehat{\mathbf{F}}){\mathbf{x}}_k+\widehat{\mathbf{F}}({\mathbf{x}}_k-{\widehat{\mathbf{x}}}_{k|k})+{\mathbf{w}}_k\end{array}.$$

(39)

Simplifying (39) by (38) results in the a priori state error ${\tilde{\mathbf{x}}}_{k+1|k}$ equation:

$${\tilde{\mathbf{x}}}_{k+1|k}=\tilde{\mathbf{F}}{\mathbf{x}}_k+\widehat{\mathbf{F}}{\tilde{\mathbf{x}}}_{k|k}+{\mathbf{w}}_k.$$

(40)

If there is no model mismatch here, or the system is linear ($\widehat{\mathbf{F}}\approx \mathbf{F}$), Equation (40) can be expressed as:

$${\tilde{\mathbf{x}}}_{k+1|k}=\mathbf{F}{\tilde{\mathbf{x}}}_{k|k}+{\mathbf{w}}_k$$

(41)

By (41), the predicted state error covariance ${\mathbf{P}}_{k+1|k}^{}$ is obtained:

$$\begin{array}{ll}{\mathbf{P}}_{k+1|k}& =\mathrm{E}\left\{\right[\mathbf{F}{\tilde{\mathbf{x}}}_{k|k}+{\mathbf{w}}_k\left]\right[{\tilde{\mathbf{x}}}^T{}_{k|k}{\mathbf{F}}^T+{\mathbf{w}}^T{}_k\left]\right\}\\ & =\mathrm{E}\left\{\right[\mathbf{F}{\tilde{\mathbf{x}}}_{k|k}{\tilde{\mathbf{x}}}^T{}_{k|k}{\mathbf{F}}^T+{\mathbf{w}}_k{\tilde{\mathbf{x}}}^T{}_{k|k}{\mathbf{F}}^T\\ & +\mathbf{F}{\tilde{\mathbf{x}}}_{k|k}{\mathbf{w}}^T{}_k+{\mathbf{w}}_k{\mathbf{w}}^T{}_k\left]\right\}\end{array}$$

(42)

where ${\mathbf{w}}_{\mathbf{k}}$ are process Gaussian noise sequences with zero means and covariance ${\mathbf{Q}}_{\mathbf{k}}$ is independent of state:

$$\{\begin{array}{l}{\mathbf{w}}_k~N(0,{\mathbf{Q}}_k)\\ \mathrm{E}\{{\mathbf{w}}_k{\mathbf{w}}_k^T\}={\mathbf{Q}}_k\\ \mathrm{E}\{{\mathbf{w}}_k{\tilde{\mathbf{x}}}^T{}_{k|k}{\mathbf{F}}^T\}=\mathrm{E}\{\mathbf{F}{\tilde{\mathbf{x}}}_{k|k}{\mathbf{w}}^T{}_k\}=0\end{array}$$

(43)

The posterior state error covariance at previous time is defined as:

$${\mathbf{P}}_{k|k}=\mathrm{E}\{{\tilde{\mathbf{x}}}_{k|k}{\tilde{\mathbf{x}}}_{k|k}^T\}.$$

(44)

Substitution of (43) and (44) into (42) yields:

$${\mathbf{P}}_{k+1|k}=\mathbf{F}{\mathbf{P}}_{k|k}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{Q}}_k.$$

(45)

Utilizing the predicted state estimate ${\widehat{\mathbf{x}}}_{k+1|k}$, the corresponding predicted measurement errors ${\mathbf{e}}_{k+1}^{\mathit{svsf}}$ are calculated by:

$${\mathbf{e}}_{k+1|k}^{\mathit{svsf}}={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k}.$$

(46)

Based on the SVSF, the gain ${\mathbf{K}}_{k+1}^{\mathit{svsf}}$ and the SVSF state estimate ${\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}$ are obtained as follows:

$$\begin{array}{ll}{\mathbf{K}}_{k+1}^{\mathit{svsf}}=& {\mathbf{H}}^{+}\mathit{diag}\left[\right(|{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}|+\gamma |{\mathbf{e}}_{k|k}|)\\ & •\mathit{sat}\left({\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}\right)\left]\right[\mathit{diag}({\mathbf{e}}_{k+1|k}^{\mathit{svsf}}){]}^{-1}\end{array}$$

(47)

$${\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}={\widehat{\mathbf{x}}}_{\mathbf{k}+1|\mathbf{k}}+{\mathbf{K}}_{k+1}^{\mathit{svsf}}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}$$

(48)

The SVSF estimation error covariance matrix ${\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}$ is updated as follows:

$${\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}=\mathrm{E}\{{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}})}^{\mathrm{T}}|{\mathbf{Z}}_{1:\mathbf{k}+1}\}.$$

(49)

The SVSF estimation error ${\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}$ is expressed as:

$${\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}={\mathbf{x}}_{\mathbf{k}+1}-{\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}.$$

(50)

Substitution of (48) and (46) into (50) yields:

$${\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}={\mathbf{x}}_{k+1}-{\widehat{\mathbf{x}}}_{k+1|k}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}({\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k}).$$

(51)

where the effect of measurement noise is not taken into consideration, but will be taken below, so by (51), the SVSF estimation error is computed as follows:

$$\begin{array}{ll}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}& ={\mathbf{x}}_{k+1}-{\widehat{\mathbf{x}}}_{k+1|k}^{}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}(\mathbf{H}{\mathbf{x}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k}^{})\\ & ={\tilde{\mathbf{x}}}_{k+1|k}^{}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k}^{}\\ & =(\mathbf{I}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k}^{}\end{array}$$

(52)

The SVSF estimation error covariance ${\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}$ is deduced by:

$$\begin{array}{ll}{\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}=& \mathrm{E}\{{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}})}^T\}\\ \hfill =& \mathrm{E}\left\{\right[(\mathbf{I}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}]\\ & \left[\right({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{)}^T{(\mathbf{I}-K_{k+1}^{\mathit{svsf}}\mathbf{H})}^T]\}\}\\ \hfill =& (\mathbf{I}-K_{k+1}^{\mathit{svsf}}\mathbf{H}){\mathbf{P}}_{k+1|k}^{}{(\mathbf{I}-K_{k+1}^{\mathit{svsf}}\mathbf{H})}^T\end{array}$$

(53)

3.2.2. Step 2: Revised by Bayesian Estimation Method

The measurements contain Gaussian noise ${\mathbf{v}}_{k+1}$ with zero means and covariance ${\mathbf{R}}_k$, so the measurement error covariance can be computed by:

$${\tilde{\mathbf{z}}}_{k+1|k+1}^{\mathit{svsf}}={\mathbf{z}}_{k+1}-{\widehat{\mathbf{z}}}_{k+1|k+1}^{\mathit{svsf}}=\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{v}}_{k+1}$$

(54)

$${\mathbf{P}}_{\mathbf{z}\mathbf{z}}=\mathrm{E}\left\{\right[{\tilde{\mathbf{z}}}_{k+1|k+1}^{\mathit{svsf}}{({\tilde{\mathbf{z}}}_{k+1|k+1}^{\mathit{svsf}})}^{\mathrm{T}}]|{\mathbf{Z}}_{1:k+1}\}$$

(55)

Substituting (54) into (55), measurement error covariance can be obtained as follows:

$$\begin{array}{ll}{\mathbf{P}}_{\mathbf{z}\mathbf{z}}=& \mathrm{E}\left\{\right[\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{v}}_{k+1}]{[\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{v}}_{k+1}]}^T\}\\ \hfill =& \mathrm{E}\{\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}})}^T{\mathbf{H}}^T+\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{v}}_{k+1}^T\\ & +{\mathbf{v}}_{k+1}({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{)}^T{\mathbf{H}}^T+{\mathbf{v}}_{k+1}{\mathbf{v}}_{k+1}^T\}\end{array}$$

(56)

where measurement noise ${\mathbf{v}}_{k+1}$ and state errors are independent of each other,

$$\{\begin{array}{l}{\mathbf{v}}_{k+1}~N(0,{\mathbf{R}}_{k+1})\\ \mathrm{E}\{{\mathbf{v}}_{k+1}\}=\mathrm{E}\{{\mathbf{v}}_{k+1}^T\}=0\\ \mathrm{E}\{{\mathbf{v}}_{k+1}{\mathbf{v}}_{k+1}^T\}={\mathbf{R}}_{k+1}\\ \mathrm{E}\{{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{v}}_{k+1}^T\}=\mathrm{E}\{{\mathbf{v}}_{k+1}^{}{\left({\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}\right)}^T\}=0\end{array}.$$

(57)

Substituting (57) into (56), the ${\mathbf{P}}_{\mathbf{z}\mathbf{z}}$ can be simplified as:

$${\mathbf{P}}_{\mathbf{z}\mathbf{z}}=\mathbf{H}{\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{H}}^T+{\mathbf{R}}_{k+1}$$

(58)

The cross-covariance between the measurement and the state value is as follows:

$$\begin{array}{ll}{\mathbf{P}}_{\mathbf{x}\mathbf{z}}& =\mathrm{E}\left\{\right[{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}\left]\right[{\tilde{\mathbf{z}}}_{k+1|k+1}{]}^T\}\\ & =\mathrm{E}\left\{\right[{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}\left]\right[\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{v}}_{k+1}{]}^T\}\\ & ={\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{H}}^T\end{array}$$

(59)

According to Bayes’ principle (34), the prediction gain ${\mathbf{K}}_{k+1}$ is computed as:

$${\mathbf{K}}_{k+1}={\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{H}}^T{\mathbf{P}}_{\mathbf{z}\mathbf{z}}^{-1}.$$

(60)

The measurement error of the SVSF process can be expressed as:

$$\begin{array}{ll}{\mathbf{e}}_{k+1|k+1}^{\mathit{svsf}}& ={\mathbf{z}}_{k+1}-{\mathbf{z}}_{k+1|k}^{\mathit{svsf}}\\ & ={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}\end{array}$$

(61)

The revised state value ${\widehat{\mathbf{x}}}_{k+1|k+1}$ can be computed as:

$${\widehat{\mathbf{x}}}_{k+1|k+1}={\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{K}}_{k+1}{\mathbf{e}}_{k+1|k+1}^{\mathit{svsf}}$$

(62)

Through (61) and (62), the new state error ${\tilde{\mathbf{x}}}_{k+1|k+1}$ can be derived as follows:

$$\begin{array}{ll}{\tilde{\mathbf{x}}}_{k+1|k+1}& ={\mathbf{x}}_{k+1|k+1}-{\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{K}}_{k+1}(\mathbf{H}({\mathbf{x}}_{k+1|k+1}-{\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}})+{\mathbf{v}}_{k+1})\\ & ={\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}-{\mathbf{K}}_{k+1}(\mathbf{H}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{v}}_{k+1})\\ & =(\mathbf{I}-\mathbf{K}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}-{\mathbf{K}}_{k+1}{\mathbf{v}}_{k+1}\end{array}.$$

(63)

The posterior covariance ${\mathbf{P}}_{k+1|k+1}$ can be written as:

$$\begin{array}{ll}{\mathbf{P}}_{k+1|k+1}=& \mathrm{E}[{\tilde{\mathbf{x}}}_{k+1|k+1}{\tilde{\mathbf{x}}}_{k+1|k+1}^T]\\ \hfill =& \mathrm{E}\{[(\mathbf{I}-K\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}-{\mathbf{K}}_{k+1}{\mathbf{v}}_{k+1}]\\ & {[(\mathbf{I}-\mathbf{K}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}-{\mathbf{K}}_{k+1}{\mathbf{v}}_{k+1}]}^T\}\\ \hfill =& \mathrm{E}\{[(\mathbf{I}-\mathbf{K}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{(\mathbf{I}-\mathbf{K}\mathbf{H})}^T-(\mathbf{I}-\mathbf{K}\mathbf{H}){\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{v}}_{k+1}^T{\mathbf{K}}_{k+1}^T\\ & -{\mathbf{K}}_{k+1}{\mathbf{v}}_{k+1}{\tilde{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}\mathbf{I}-\mathbf{K}\mathbf{H}{)}^T{]}^T+{\mathbf{K}}_{k+1}{\mathbf{v}}_{k+1}{\mathbf{v}}_{k+1}^T{\mathbf{K}}_{k+1}^T\}\end{array}.$$

(64)

Because measurement noise ${\mathbf{v}}_k$ and state errors are independent of each other. Substituting (57) into (64), ${\mathbf{P}}_{k+1|k+1}$ can be simplified to:

$${\mathbf{P}}_{k+1|k+1}=(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H}){\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H})}^T+{\mathbf{K}}_{k+1}{\mathbf{R}}_{k+1}{\mathbf{K}}_{k+1}^T.$$

(65)

The posterior measurement error can be expressed as:

$${\mathbf{e}}_{k+1|k+1}={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k+1}^{}$$

(66)

The ISVSF algorithm mentioned in this paper is summarized in equations from (36) to (66). The pseudo-code of Algorithm 1 is patched as follows:

Algorithm 1: The ISVSF algorithm

Input {${\mathbf{x}}_0,{\mathbf{P}}_0,\psi ,\mathbf{F},\mathbf{Q},\mathbf{R},\mathbf{H},{\mathbf{e}}_{0|0}$} and the sequence measurement {${\mathbf{z}}_1,{\mathbf{z}}_2\cdot \cdot \cdot {\mathbf{z}}_N$} For k = 1:N Step 1 SVSF estimation propagate the nominal state ${\widehat{\mathbf{x}}}_{k+1}=\mathbf{F}{\widehat{\mathbf{x}}}_k$ Propagate the error covariance ${\mathbf{P}}_{k+1|k}=\mathbf{F}{\mathbf{P}}_{k|k}\mathbf{F}+{\mathbf{Q}}_k$ ${\mathbf{e}}_{k+1|k}^{\mathit{svsf}}={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k}$ Compute the SVSF gain ${\mathbf{K}}_{k+1}^{\mathit{svsf}}={\mathbf{H}}^{+}\mathit{diag}(|{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}|+\gamma |{\mathbf{e}}_{k|k}|)•\mathit{sat}({\overline{\psi }}^{-1}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}){[\mathit{diag}({\mathbf{e}}_{k+1|k}^{\mathit{svsf}})]}^{-1}$ Update the state ${\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}={\widehat{\mathbf{x}}}_{k+1|k}+{\mathbf{K}}_{k+1}^{\mathit{svsf}}{\mathbf{e}}_{k+1|k}^{\mathit{svsf}}$ ${\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}=(\mathbf{I}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}\mathbf{H}){\mathbf{P}}_{k+1|k}{(\mathbf{I}-{\mathbf{K}}_{k+1}^{\mathit{svsf}}\mathbf{H})}^T$ Step 2 revised by Bayesian estimation: Compute the measurement error covariance ${\mathbf{P}}_{\mathbf{z}\mathbf{z}}=\mathbf{H}{\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{H}}^T+{\mathbf{R}}_{k+1}$ Compute the Bayesian gain ${\mathbf{K}}_{k+1}={\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{\mathbf{H}}^T{\mathbf{P}}_{\mathbf{z}\mathbf{z}}^{-1}$ ${\mathbf{e}}_{k+1|k+1}^{\mathit{svsf}}={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}$ Update the a posteriori error state ${\widehat{\mathbf{x}}}_{k+1|k+1}={\widehat{\mathbf{x}}}_{k+1|k+1}^{\mathit{svsf}}+{\mathbf{K}}_{k+1}{\mathbf{e}}_{k+1|k+1}^{\mathit{svsf}}$ Compute the posteriori error covariance ${\mathbf{P}}_{k+1|k+1}=(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H}){\mathbf{P}}_{k+1|k+1}^{\mathit{svsf}}{(\mathbf{I}-{\mathbf{K}}_{k+1}\mathbf{H})}^T+{\mathbf{K}}_{k+1}{\mathbf{R}}_{k+1}{\mathbf{K}}_{k+1}^T$ ${\mathbf{e}}_{k+1|k+1}^{}={\mathbf{z}}_{k+1}-\mathbf{H}{\widehat{\mathbf{x}}}_{k+1|k+1}^{}$ Output {${\widehat{\mathbf{x}}}_{k+1|k+1}$, ${\mathbf{P}}_{k+1|k+1}$, ${\mathbf{e}}_{k+1|k+1}$} End for

4. Simulation

4.1. A Classic Target Tracking Scenario

To verify the effectiveness of the proposed algorithm in a linear system, simulations are carried out in a two-dimensional space. The target position is provided by a radar system. The aircraft moves from the initial position of $[-\mathrm{25,000}\mathrm{m},-\mathrm{10,000}\mathrm{m}]$, with an initial velocity of $300\mathrm{m}/\mathrm{s}$ in the x-axis direction and $280\mathrm{m}/\mathrm{s}$ in the y-axis direction. Due to the existence of airflow disturbance, velocity adjustment and other factors, the target has random acceleration interference, which obeys a Gaussian distribution with standard deviation of $10\mathrm{m}/{\mathrm{s}}^2$. The target flies for $500\mathrm{s}$.

In two-dimensional space target tracking, the aircraft motion model can be modeled as a uniform motion (UM):

$${\mathbf{x}}_{k+1}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& T\\ 0& 0& 0& 1\end{array}\right]{\mathbf{x}}_k+\left[\begin{array}{cc}\frac{1}{2}{T}^2& 0\\ 0& T\\ \frac{1}{2}{T}^2& 0\\ 0& T\end{array}\right]{\mathbf{w}}_k$$

(67)

where $T$ refers to the sampling interval and is set as $T=1\mathrm{s}$, and ${\mathbf{w}}_k$ indicates the system noise, which is always unknown in most systems. The state vector ${\mathbf{x}}_k$ is deduced by:

$${\mathbf{x}}_k={[{\eta }_k,{\dot{\eta }}_k,{\xi }_k,{\dot{\xi }}_k]}^T,{\mathbf{w}}_k=[{\ddot{\eta }}_k,{\ddot{\xi }}_k]$$

(68)

where the ${\eta }_k$ and ${\xi }_k$ indicate positions in the x-axis and y-axis, respectively. ${\dot{\eta }}_k$ and ${\dot{\xi }}_k$ express the velocity along the x-axis and y-axis, respectively. Generally, radar only provides position information, which contains the real position and noise of the target. The measurement model of the system can be expressed as:

$${\mathbf{z}}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]{\mathbf{x}}_k+{\mathbf{v}}_k$$

(69)

In the simulation, some parameters of the filters need to be initialized. The measurement noise covariance $\mathbf{R}$ of the radar can be calculated by statistics. State covariance ${\mathbf{P}}_{0|0}$ and process noise covariance $\mathbf{Q}$ can be expressed as follows:

$$\mathbf{R}={200}^2\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

(70)

$$\mathbf{P}{=}_{0|0}\mathit{diag}\left(\right[10000,1000,10000,1000\left]\right)$$

(71)

$$\mathbf{Q}=L\ast \left[\begin{array}{cccc}\frac{T^3}{3}& \frac{T^2}{2}& 0& 0\\ \frac{T^2}{2}& T& 0& 0\\ 0& 0& \frac{T^3}{3}& \frac{T^2}{2}\\ 0& 0& \frac{T^2}{2}& T\end{array}\right]$$

(72)

where $L$ is the power spectral densities [17]. Additionally, the SVSF, UK-SVSF and ISVSF “memory” (convergence rate) $\gamma$ is set to 0.1 [17], which is tuned based on uncertain knowledge of the system such as the noise.

To compare the performances of different filters, the root mean square error (RMSE) and the averaged root mean square error (ARMSE) are chosen as performance metrics, such as in position; they are defined as follows:

$${\mathit{RMSE}}_{\mathit{pos},k}=\sqrt{\frac{1}{M}{\displaystyle {\sum }_{s=1}^M(({\eta }_k^s-{\widehat{\eta }}_k^s{)}^2+{({\xi }_k^s-{\widehat{\xi }}_k^s)}^2)}}$$

(73)

$${\mathit{ARMSE}}_{\mathit{pos},k}=\sqrt{\frac{1}{\mathit{MT}}{\displaystyle {\sum }_{k=1}^T{\displaystyle {\sum }_{s=1}^n({({\eta }_k^s-{\widehat{\eta }}_k^s)}^2+{({\xi }_k^s-{\widehat{\xi }}_k^s)}^2)}}}$$

(74)

where $({\eta }_k^s,{\xi }_k^s)$ and $({\widehat{\eta }}_k^s,{\widehat{\xi }}_k^s)$ are the true position and estimated position at the $s\mathit{th}$ Monte Carlo run, and $M$ is the total number of Monte Carlo runs, which are performed in simulation. The RMSE and ARMSE of velocity are similar in position.

4.1.1. Simulation under Unknown Noise

In some engineering applications, measurement noise may not obey Gaussian distribution and may be unknown due to the inference of external factors [8,9], such as the agile target observed in clutter, and sensors are unreliable. In the simulation, the actual measurement noise of the radar is expressed as follows:

$${\mathbf{v}}_k=\{\begin{array}{l}\mathbf{N}(0,{200}^2)\mathrm{w}.\mathrm{p}.0.90\\ \mathbf{N}(0,9\times {200}^2)\mathrm{w}.\mathrm{p}.0.10\end{array}$$

(75)

where “w.p.” is short for “with probability”, the Kalman filter is the optimal filter when the system and measurement noise are white, and the initial state conditions have known means and variances [45]. However, for agile targets, the process noise is difficult to model in advance, which means that the random acceleration of the target is unknown. The simulation of the random acceleration follows a Gaussian distribution with $10\mathrm{m}/{\mathrm{s}}^2$ standard deviation. The real trajectory is shown in Figure 5. Additionally, in the simulation, all filters use the uniform motion model, and the parameter $L$ of $\mathbf{Q}$ is set to 1 in the ISVSF, UK-SVSF and KF. The smooth boundary layer widths of the SVSF, UK-SVSF and ISVSF are set to $\psi =[2000,2000]\mathrm{m}$. The $\kappa$ of the UK-SVSF is set to $\kappa =3-n_{\mathbf{x}}$, and ${\psi }_{fixed}=3200\mathrm{m}$. Under the same measurement noise distributions, 400 Monte Carlo runs are performed.

Figure 5 shows the trajectory of one simulation result, including the real trajectory, the points of measurement and the trajectory processed by the filters. Additionally, the small diagram contained in Figure 5 is a partially enlarged one. It can be seen from Figure 5 that the trajectory obtained by the ISVSF is more approximate to the real trajectory. From Figure 6 and

Table 2. The position ARMSE on the x-axis and y-axis (m).

Different Methods	KF	SVSF	UK-SVSF	ISVSF
Position ARMSE on x-axis (m)	200	298	145	133
Position ARMSE on y-axis (m)	256	225	232	172
Velocity ARMSE on x-axis (m)	26	133	36	31
Velocity ARMSE on y-axis (m)	31	63	69	42

, the ARMSE of the ISVSF is the least compared with the KF, SVSF and UK-SVSF. For use of the KF, knowledge of an accurate mathematical model and the statistical characteristics of system noise and measurement noise is required to obtain the desired filtering effect. Compared with the SVSF, UK-SVSF and ISVSF, the performance of the KF is more unstable when the acceleration noise disturbance is underestimated and the measurement noise is unknown. The SVSF has a stable filtering effect, but its ability to eliminate noise should be improved. The ISVSF has a better filtering effect than the SVSF, as shown in Table 2, and the position ARMSE of the ISVSF is reduced by above 60% compared with the SVSF. The ISVSF not only has higher tracking accuracy compared with the SVSF but also has higher robustness than the KF under unknown noises. Although the UK-SVSF improves the robustness and accuracy under unknown noises, the velocity estimation is affected due to competition between the UKF and SVSF. Compared with the SVSF, the accuracy of the ISVSF is improved because of its highly accurate velocity estimation.

4.1.2. Results under the Condition of Different Smooth Boundary Layer Widths

Smooth boundary layer width is an important parameter in systems based on the SVSF, and its value setting will affect the stability and accuracy of the system. To verify this influence, the range of smooth boundary layer width is set from $200\mathrm{m}$ to $3500\mathrm{m}$ at intervals of $100\mathrm{m}$. From Figure 7, the results show that when the smooth boundary layer width is less than the existence subspace layer, the precision of the ISVSF and SVSF will be affected because of the existing chattering. With the smooth boundary layer width increasing, SVSF ARMSE decreases slightly first, and then increases sharply as shown in Figure 7. The reason is that the SVSF lacks estimation of the velocity dimension of the state, which causes the SVSF to commit a large error in model extrapolation. The fact that the accuracy of ISVSF is less susceptible to the smooth boundary layer width than SVSF can be attributed to the Bayesian filtering estimation, which can estimate velocity so that the predicted trajectory of ISVSF is closer to the real trajectory than that of SVSF on the prediction stage, which decrease prediction errors, ensuring the accuracy and thus maintaining the stability. How to effectively use the ISVSF to modify the velocity information will be described in the following passage. The combination with the Bayesian filtering process can eliminate the impact of the smooth boundary layer width and deliver more stable filtering results.

4.2. Simulation Results in Modeling Error

Given the high accuracy requirements of most filters for mathematical models, a system divergence will occur once the modeling of the filter is wrong. When tracking the maneuvering target, the system model is uncertain and often inconsistent with the actual model. $\mathbf{F}$ is state space model of the system matrix and ${\mathbf{F}}_c$ is the changing model; they are defined as follows:

$$\begin{array}{ll}\mathbf{F}=& \left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]\\ {\mathbf{F}}_c=& \left[\begin{array}{cccc}1& \mathit{sin}\left(\mathit{wT}\right)/w& 0& \left(\mathit{cos}\right(\mathit{wT})-1)/w\\ 0& \mathit{cos}\left(\mathit{wT}\right)& 0& -\mathit{sin}\left(\mathit{wT}\right)\\ 0& (1-\mathit{cos}(\mathit{wT}\left)\right)/w& 1& \mathit{sin}\left(\mathit{wT}\right)/w\\ 0& \mathit{sin}\left(\mathit{wT}\right)& 0& \mathit{cos}\left(\mathit{wT}\right)\end{array}\right]\end{array}$$

(76)

In the target tracking, $\mathbf{F}$ represents uniform motion and ${\mathbf{F}}_c$ represents a uniform turning motion with an angular velocity of $w$. The initial position of the target is $[-\mathrm{25,000}\mathrm{m},-\mathrm{10,000}\mathrm{m}]$, and the target moves in a straight line at a uniform velocity of $[320\mathrm{m}/\mathrm{s},20\mathrm{m}/\mathrm{s}]$ for $100\mathrm{s}$. Then, the maneuvering target turns at a rate of $-3^{°}/\mathrm{s}$ for $60\mathrm{s}$. Next, the target moves in a straight line at a uniform velocity for $90\mathrm{s}$, and maneuvers at a rate of $-2^{°}/\mathrm{s}$ for $90\mathrm{s}$. Finally the target flies straight for 160 s until the end. No matter whether they are on the x-axis and y-axis, noises of measurements obey a Gaussian distribution with standard deviation of 200 m. In order to assess its performance, a total of 300 Monte Carlo runs are carried out. The simulation is conducted to test the robustness of three strategies in modeling errors.

In the simulation, the smooth boundary layer widths of the ISVSF, UK-SVSF and SVSF are set to $\psi =[1200\mathrm{m},1200\mathrm{m}]$, the $L$ of the KF and UK-SVSF are set to 1, the $\kappa$ of UK-SVSF is set to $\kappa =3-n_{\mathbf{x}}$, ${\psi }_{fixed}$ is set to ${\psi }_{fixed}=3200$ and ${\mathbf{x}}_0=[-\mathrm{25,000}\mathrm{m},220\mathrm{m}/\mathrm{s},-\mathrm{10,000}\mathrm{m},60\mathrm{m}/\mathrm{s}]$, the $L$ of the ISVSF is set to 1000, and other parameters are similar to the above simulation. The simulation results are as follows: Figure 8 shows the trajectory of one simulation result, including the real trajectory, and the points of measurement and the trajectory are processed by three filters. Figure 8 and Figure 9a,b show that, when the model is unchanged, the KF would achieve the best performance in velocity and position estimation; when the modeling of the filter is different from the motion model of the target, the position RMSE of the ISVSF, UK-SVSF and SVSF would increase slightly, while the position RMSE of the KF would increase greatly. As shown in Figure 9b, the original SVSF does not estimate the velocity, and the velocity error of SVSF is induced by the difference between the initial velocity of the SVSF and the actual velocity of the target, but the KF and ISVSF can eliminate this error through iterative calculation. Compared with the SVSF, the ISVSF adds the Bayesian solution process and can thereby estimate velocity more accurately. It can be seen from the

Table 3. The position or velocity ARMSE.

Different Methods	KF	SVSF	UK-SVSF	ISVSF
Position ARMSE (m)	769	296	269	176
Velocity ARMSE (m/s)	86	253	245	55

that the velocity ARMSE of the ISVSF is 3.6 times less than that of the SVSF. The result also shows that the UK-SVSF has improved estimation accuracy and stability. When the modeling has an error under the maneuvering condition, the UK-SVSF method mainly uses the SVSF to estimate state, and the error in estimated velocity is large because the SVSF is deficient in estimating velocity, and a certain competition and interference also exist between the UKF and SVSF. Compared with the ISVSF, the UK-SVSF performs better under normal conditions, while the ISVSF is better if modeling uncertainty exists. To sum up, the ISVSF, UK-SVSF and SVSF all have good robustness. However, compared with the UK-SVSF and SVSF, the ISVSF has better filtering performance.

4.3. A Comprehensive Simulation

Some uncertain factors could influence systems and change system parameters or states. For example, if the target is impacted by external factors such as strong external impact, disturbance or occlusions, its state will change greatly when it is detected again. In the fault diagnosis system, the parameters and state of the characterization system will change dramatically once a fault occurs [8]. That is, in some particular case, one may expect a substantial deviation of the assumed system description from the true one [7]. An application example is shown in Appendix A. The comprehensive simulation is presented as follows. It contains different kinds of maneuvering and sudden changes caused by interference. The purpose is to test the robustness and accuracy of the proposed ISVSF. Additionally, a comparison has been made between the ISVSF and other methods to estimate states without measurement in the case where the measurement vector is less than the state vector.

In the simulation, the actual measurement noise is expressed as follows:

$${\mathbf{v}}_k=\{\begin{array}{l}\mathbf{N}(0,{200}^2)\mathrm{w}.\mathrm{p}.0.90\\ \mathbf{N}(0,9\times {200}^2)\mathrm{w}.\mathrm{p}.0.10\end{array}$$

(77)

The movement parameters for a maneuvering target in this simulation are listed in

Table 4. Simulation scenario for maneuvering target.

Maneuver	Duration	Maneuver	Duration
initial state [−15,000 m,320 m/s,−10,000 m,20 m/s]	0 s	constant velocity	190–250 s
constant velocity	1–49 s	coordinated turn motion with $\omega =3^{\circ }/s$	251–299 s
a sudden change on the y-axis z = z-4000 m	50 s	constant velocity	300–380 s
constant velocity	50–80 s	acceleration with x-axis a = −20 m/s2, y-axis a = 10 m/s2	381–384 s
coordinated turn motion with $\omega =-5^{\circ }/s$	81–119 s	constant velocity	385–399 s
a sudden change on the y-axis z = z-3000 m	120 s	a sudden change, x-axis z = z-1000 m, y-axis z = z-5000 m,	400 s
coordinated turn motion with $\omega =-5^{\circ }/s$	121–134 s	constant velocity	401–420 s
constant velocity	135–170 s	coordinated turn motion with $\omega =4^{\circ }/s$	421–460 s
acceleration with x-axis a = −10 m/s2	171–189	constant velocity	461–500 s

, and the real trajectory is shown in Figure 10.

In the simulation, the SVSF with Luenberger’s strategy (SVSF-L) [24,26] and the SVSF with “artificial” velocity measurements (SVSF-V) [17,21] are applied to estimate velocity. The SVSF and UK-SVSF have large estimation errors when the target maneuvers, so they are not shown in the simulations. The position smooth boundary layer width of the ISVSF, SVSF-L and SVSF-V are set to $[3000,3000]$, the velocity smooth boundary layer width of SVSF-L and SVSF-V are set to $[5400,5400]$, and other parameters are similar to those in above simulation 4.2. In order to assess its performance, 1000 Monte Carlo simulations were carried out.

Figure 10 shows the trajectories of one simulation, including the real trajectory, measurements and the trajectories obtained by different methods. All filters use the uniform motion model. It can be seen from Figure 10 and Figure 11 that those methods based on the SVSF have good robustness in systems with modeling errors. Compared with the ISVSF and SVSF-L, the SVSF-V is more vulnerable to noise and maneuvering because its “artificial” velocity measurement is obtained by dividing the position differences in one cycle by the sampling time, so that the “artificial” velocity is easily affected by noise and thus produce a larger errors. From Figure 11, when sudden position changes occur at 50 s, 120 s and 400 s, both the SVSF-L and SVSF-V have large velocity estimation errors, especially the SVSF-L. All of the position estimation errors of the SVSF-L, SVSF-V and ISVSF are smaller than the change in position values. However, the proposed ISVSF is more stable and can quickly converge to a steady state. That can be attributed to the fact that in the first step, the ISVSF can adapt to a sudden change in position state without affecting the covariance of the velocity dimension, which ensures that in the second step, the estimation performance of the ISVSF obtains a more accurate velocity. In the case of weak maneuvering, the ISVSF and SVSF-L have minor estimation errors compared with the SVSF-V. The three methods based on the SVSF all have their own advantages when estimating states without measurement in the case where the measurement vector is less than the state vector, and the three methods can be chosen according to different requirements, Besides that, the ISVSF has another advantage in that there is no need to set smoothing layer width for unmeasured states. It can be concluded from

Table 5. The position and velocity ARMSE (1000 simulations).

Different Methods	KF	SVSF-V	SVSF-L	ISVSF
Position ARMSE (m)	1041	389	245	206
Velocity ARMSE (m/s)	113	135	102	59

that the ISVSF has a smaller ARMSE than the other methods under 1000 simulations. To sum up, the ISVSF has good robustness and accuracy.

5. Conclusions

This paper proposes an improved SVSF (ISVSF) that combines Bayesian theory and the SVSF. The ISVSF uses the SVSF theory to make preliminary estimations, then, using the derived covariance and Bayesian theory, makes further estimations. It is capable not only of maintaining the robustness of the SVSF but also of retaining the advantages of the Bayesian filtering system. Moreover, it can estimate the lower dimension states that have no corresponding measurement value. A performance comparison of the ISVSF, UK-SVSF, KF and SVSF indicates that the ISVSF achieves improved performance under the conditions of modeling error and unknown noise. Even if state undergoes a sudden change in the presence of external factors, the ISVSF still shows satisfactory performance. We hope that further research can be conducted to unleash the great potential of the ISVSF and SVSF in applications involving more complex and changeable systems.