Two-stage High-degree Cubature Information Filter

Abstract

In the system with unknown random bias, it is common to treat the bias as part of the system state, that is augmented state Kalman filter (ASKF) which will lead to overflow and can not work in computer system. To avoid use the ASKF, two-stage Kalman filter was proposed. It is generally known that the computational complexity of Kalman Filter is highly when all sensor measurements are processed centrally. In order to overcome this disadvantage, High-degree Cubature Information Filter (HCIF) is introduced. Based on HCIF, two-stage High-degree Cubature Information Filter (TSHCIF) is proposed for the nonlinear system with the unknown random bias in order to estimate the high-dimensional state and centralized process the measurement values. The key idea for TSHCIF is using the matrix transformation technique which is called Two-stage transformation in the framework of HCIF and make the covariance matrices block diagonal. The estimate of TSHCIF can be expressed as the output by the bias free filter and bias filter. Simulation results that validate the predicted efficiency improvements and it is shown that the proposed TSHCIF is mathematically equivalent to the augment HCIF (AHCIF).

Keywords

High-degree Cubature Information Filter (HCIF)Two-stage HCIF (TSHCIF)nonlinear two-stage transformation (T transformation)nonlinear system unknown random bias

1 Introduction

In order to solve the estimation of nonlinear system, nonlinear filtering and estimation based on Bayesian framework are deeply studied recent years. Extended Kalman filter (EKF) is used widely for nonlinear filtering at present [1]. The first-order approximation of the current state equations can be realized by local linearization [2]. But in highly nonlinear system, the high order terms are ignored in the linearization process, which is easy to cause the filter divergence. At the same time, EKF needs to calculate the Jacobian matrix, which increases the computational complexity. Unscented Kalman filter (UKF) is a kind of Gauss approximation filter based on point which uses sampling strategy approach nonlinear distribution [3, 4]. The probability density function of UKF is approximated by the weighted sum of a set of Sigma points passing through a nonlinear function, the filtering accuracy is higher than EKF, and the robustness is stronger. However, the performance of UKF is affected by the parameter values, which is only suitable for solving the state estimation of lower dimension system. The CKF based on the three-order spherical-radial cubature rule is proposed to solve the state estimating of high dimensional system [5]. The filtering accuracy of CKF is better than EKF and UKF because all points used have equal and positive weight value [6]. But the CKF estimation accuracy is still limited and the algorithm has some defects, which needs higher degree CKF to obtain higher precision [7, 8]. Based on the Genz integral method and the moment matching method, the rule of spherical and radial for arbitrary degree are derived in Literature [9 –13], a high degree cubature rule which can obtain arbitrary degree accuracy is established.

It is generally known that the computational complexity of Kalman Filter is highly when all sensor measurements are processed centrally [14, 15]. The information filtering is superior to the standard Kalman filter because of its easy initialization and less computation. It has been widely used in the field of multi-sensor information fusion and control [16]. an algorithm of Cubature information filter (CIF) can be get by embedding Cubature Kalman Filter (CKF) in the extended information filter (EIF) framework, and CIF achieves the state filter by transferring information vector and information matrix. Its estimation accuracy and filtering stability are better than EIF and unscented information filter (UIF), this advantage is more obvious in high dimensional filter [17].

The standard CKF requires an accurate system model and exact stochastic information. In reality environment, the models contains unknown constant or unknown random bias. When the state estimation of the dynamic system is unknown, the common method is to use the bias as a part of the state of the system, to estimate the deviation and the state at the same time, that is augmented state Kalman filter (ASKF) which will lead to overflow and can not work in computer system. To avoid the use of the ASKF, Friedland [18] proposed a two-stage filter to decouple the augmented filer into two parallel reduced-order filter. The bias-free filter is assumed that there is non-existence of the bias, but the bias filter adds the bias. Chien-Shu Hsieh [19] and J.Y. Keller [22] proposed an optimal two-stage Kalman estimator which is an extension of Friedland’s estimator and is optimal in general conditions. Hsieh and Chen [20] presented a general two-stage Kalman filter which provides the optimal estimate of the system state and can be applied to general dynamic systems which is time-varying and linear. The new filter can reduce the computational burdens. Hsieh [21] extended the linear general two-stage filter to nonlinear systems and proposed a general two-stage extended Kalman filter and it is mathematically equivalent to the extend Kalman filter. Xu et al. [23] presented a two-stage unscented Kalman filter which uses the forgetting factor to compensate the effects of incomplete information. Xueqin et al. [24] proposed a novel two-stage extended Kalman filter algorithm, the proposed approach is respectively applied to estimating bias faults and loss of effectiveness for reaction flywheels in satellite attitude control systems.

The structure of this paper is described as follows. Section 2 contains statement of the problem. In Section 3, the High-degree Cubature Information Filter (HCIF) is introduced in brief and is given in the form of three wire table. In Section 4, the two two-stage methods are compared firstly, then, it is derived the two-stage High-degree Cubature Information Filter (TSHCIF) using the nonlinear two-stage transformation (T transformation). The TSHCIF is used to derive a new nonlinear filter for estimating constant parameters in dynamic systems. Section 5 is the simulation of TSHCIF in high speed wheel robot. Section 6 has the conclusion and the last section is acknowledgment.

2 Problem Statement

Consider the following nonlinear discrete-time stochastic system represented by: $\begin{matrix} x_{k + 1} = f (x_{k}, b_{k}) + w_{k}^{x} \\ b_{k + 1} = b_{k} + w_{k}^{b} \\ z_{k} = h (x_{k}, b_{k}) + v_{k} \end{matrix}$ (1) Where x_k ∈ Rⁿ is the state vector, z_k ∈ R^m is the observation vector, b_k ∈ R^p is the bias vector, the nonlinear function f (·) and h (·) are state transition function and observation function, respectively, which are assumed to continuously differentiable with respect to x_k. The noise sequence $w_{k}^{x}$ , $w_{k}^{b}$ and v_k are zero mean uncorrelated Gaussian random sequences with $\begin{matrix} E [[\begin{matrix} w_{k}^{x} \\ w_{k}^{b} \\ v_{k} \end{matrix}] {[\begin{matrix} w_{j}^{x} \\ w_{j}^{b} \\ v_{j} \end{matrix}]}^{T}] = [\begin{matrix} Q_{k}^{x} & 0 & 0 \\ 0 & Q_{k}^{b} & 0 \\ 0 & 0 & R_{k} \end{matrix}] δ_{kj} \end{matrix}$ Where $Q_{k}^{x} > 0$ , $Q_{k}^{b} > 0$ , R_k > 0 and δ_ij is the Kronecker delta. The initial states x₀ and b₀ are assumed to be uncorrelated with the white noise processes $w_{k}^{x}$ , $w_{k}^{b}$ and v_k. We assume that the initial conditions x₀ and b₀ are Gaussian random variables with $\begin{matrix} E [x_{0}] = {\bar{x}}_{0}, E [(x_{0} - {\bar{x}}_{0}) (x_{0} - {\bar{x}}_{0})^{T}] = P_{0}^{x} > 0 \\ E [b_{0}] = {\bar{b}}_{k}, E [(b_{0} - {\bar{b}}_{0}) (b_{0} - {\bar{b}}_{0})^{T}] = P_{0}^{b} > 0 \\ E [(x_{0} - {\bar{x}}_{0}) (b_{0} - {\bar{b}}_{0})^{T}] = P_{0}^{xb} > 0 \end{matrix}$ Define $\begin{matrix} X_{k + 1} = [\begin{matrix} x_{k + 1} \\ b_{k + 1} \end{matrix}], f (X_{k}) = [\begin{matrix} f (x_{k}, b_{k}) \\ b_{k} \end{matrix}], w_{k} = [\begin{matrix} w_{k}^{x} \\ w_{k}^{b} \end{matrix}] \\ h (X_{k}) = h (x_{k}, b_{k}) \end{matrix}$ The model given by (1) may be rewritten as

$\begin{array}{l} X_{k + 1} = f (X_{k}) + w_{k} \\ Z_{k} = h (X_{k}) + v_{k} \end{array}$ (2) Where $\begin{matrix} Q_{k} = E (w_{k} w_{j}) = [\begin{matrix} Q_{k}^{x} & 0 \\ 0 & Q_{k}^{b} \end{matrix}] δ_{kj} \end{matrix}$

3 High-degree Cubature Information Filter

According to the high-degree cubature rule, the fifth order cubature Kalman filter (HCKF) algorithm was obtained [7, 8]. By combining the HCKF and the EIF, the fifth order cubature information filter (HCIF) was obtained. Actually, it does not need to calculate Jacobian matrix and hence the EIF framework cannot be directly use [16].

In the framework of extended information filter (EIF), defined Fisher information vector ${\hat{y}}_{k | k}$ and information matrix Y_k|k as follows: $Y_{k | k} = P_{k | k}^{- 1}$ (3) ${\hat{y}}_{k | k} = P_{k | k}^{- 1} {\hat{x}}_{k | k} = Y_{k | k} {\hat{x}}_{k | k}$ (4) According [16 , 25–27], the HCIF estimator were obtained as follows: $Y_{k + 1 | k + 1} = Y_{k + 1 | k} + I_{k + 1}$ (5) $I_{k + 1} = Y_{k + 1 | k} P_{xz, k + 1 | k} R_{k + 1}^{- 1} P_{xz, k + 1 | k}^{T} Y_{k + 1 | k}^{T}$ (6) ${\hat{y}}_{k + 1 | k + 1} = {\hat{y}}_{k + 1 | k} + i_{k + 1}$ (7) $\begin{matrix} i_{k + 1} & = Y_{k + 1 | k} P_{xz, k + 1 | k} R_{k + 1}^{- 1} \\ (v_{k + 1} + P_{xz, k + 1 | k}^{T} Y_{k + 1 | k}^{T} {\hat{x}}_{k + 1 | k}) \end{matrix}$ (8) Where i_k+1 was the information vector contribution and Y_k+1 was its associated information matrix contribution [16, 26]. $Y_{k + 1 | k} = P_{k + 1 | k}^{- 1}$ (9) ${\hat{y}}_{k + 1 | k} = P_{k + 1 | k}^{- 1} {\hat{x}}_{k + 1 | k}$ (10)v_k+1 was the innovation vector: $\begin{matrix} v_{k + 1} = z_{k + 1} - {\hat{z}}_{k + 1 | k} \end{matrix}$ X_i,k|k was the cubature points (i = 0, 1, ⋯ , 2n²):

$X_{i, k | k} = S_{k | k} ξ_{i} + {\hat{x}}_{k | k}$ (11) where $P_{k | k} = S_{k | k} S_{k | k}^{T}$ and vector ξ_i were as follows [10, 13]: $\begin{matrix} ξ_{i} = {\begin{matrix} {[\begin{matrix} 0 & 0 & \dots & 0 \end{matrix}]}^{T}, i = 0 \\ \sqrt{n + 2} s_{i}^{+}, \\ i = 1, 2, \dots, n (n - 1) / 2 \\ - \sqrt{n + 2} s_{i - n (n - 1) / 2}^{+}, \\ i = n (n - 1) / 2 + 1, n (n - 1) / 2 + 2, \dots, n (n - 1) \\ \sqrt{n + 2} s_{i - n (n - 1)}^{-}, \\ i = n (n - 1) + 1, n (n - 1) + 2, \dots, 3 n (n - 1) / 2 \\ - \sqrt{n + 2} s_{i - 3 n (n - 1) / 2}^{-}, \\ i = 3 n (n - 1) / 2 + 1, 3 n (n - 1) / 2 + 2, \dots, 2 n (n - 1) \\ \sqrt{n + 2} e_{i - 2 n (n - 1)}, \\ i = 2 n (n - 1) + 1, 2 n (n - 1) + 2, \dots, n (2 n - 1) \\ - \sqrt{n + 2} e_{i - n (2 n - 1)}, \\ i = n (2 n - 1) + 1, n (2 n - 1) + 2, \dots, 2 n^{2} \end{matrix} \end{matrix}$ (12)

In the formula e_i denoted the n-dimensional unit vector and the i-th element was 1.

${\begin{matrix} s_{j}^{+} = \sqrt{1 / 2} (e_{m} + e_{n}), m < n, m, n = 1, 2, \dots, n \\ s_{j}^{-} = \sqrt{1 / 2} (e_{m} - e_{n}), m < n, m, n = 1, 2, \dots, n \end{matrix}$ $X_{i, k + 1 | k}^{*}$ was the propagated cubature points (i = 0, 1, 2n²):

$X_{i, k + 1 | k}^{*} = f (X_{i, k | k})$ (13)x_k+1|k was the predicted state:

${\hat{x}}_{k + 1 | k} = \sum_{i = 0}^{2 n^{2}} ω_{i} X_{i, k + 1 | k}^{*}$ (14) where

$\begin{matrix} ω_{i} = \\ {\begin{matrix} 2 / n + 2, i = 0 \\ 1 / (n + 2)^{2}, \\ i = 1, 2, \dots, 2 n (n - 1) \\ (4 - n) / (n + 2)^{2}, \\ i = 2 n (n - 1) + 1, 2 n (n - 1) + 2, \dots, 2 n^{2} \end{matrix} \end{matrix}$ (15)P_k+1|k was the predicted error covariance:

$\begin{matrix} P_{k + 1 | k} & = & \sum_{i = 0}^{2 n^{2}} ω_{i} (X_{i, k + 1 | k}^{*} - {\hat{x}}_{k + 1 | k}) \\ (X_{i, k + 1 | k}^{*} - {\hat{x}}_{k + 1 | k})^{T} + Q_{k} \end{matrix}$ (16)z_k+1|k was the predicted measurement:

${\hat{z}}_{k + 1 | k} = \sum_{i = 0}^{2 n^{2}} ω_{i} Z_{i, k + 1 | k}$ (17) where X_i,k+1|k was the updated cubature points (i = 0, 1, ⋯ , 2n²), $P_{k + 1 | k} = S_{k + 1 | k} S_{k + 1 | k}^{T}$ and Z_i,k+1|k was the propagated cubature points by measurement equation (i = 0, 1, ⋯ , 2n²) $X_{i, k + 1 | k} = S_{k + 1 | k} ξ_{i} + {\hat{x}}_{k + 1 | k}$ (18) $Z_{i, k + 1 | k} = h (X_{i, k + 1 | k})$ (19)P_xz,k+1|k was the cross-covariance matrix:

$\begin{matrix} P_{xz, k + 1 | k} & = & \sum_{i = 0}^{2 n^{2}} ω_{i} (X_{i, k + 1 | k}^{*} - {\hat{x}}_{k + 1 | k}) \\ (Z_{i, k + 1 | k}^{*} - {\hat{z}}_{k + 1 | k})^{T} \end{matrix}$ (20) In summary, ${\hat{y}}_{k + 1 | k} = Y_{k + 1 | k} \sum_{i = 0}^{2 n^{2}} ω_{i} X_{i, k + 1 | k}^{*}$ (21) $v_{k + 1} = z_{k + 1} - \sum_{i = 0}^{2 n^{2}} ω_{i} Z_{i, k + 1 | k}$ (22)

The detailed steps of the HCIF Algorithm were summarized in Table 1.

Table 1

The HCIF Algorithm

Time update

1) Evaluate S_k|k by factorize P_k|k.

2) Calculate the cubature points X_i,k|k using the new vector ξ_i (11) (12) and the propagated cubature points

X_{i, k + 1 | k}^{*}

by state equation with the new weight vector ω_i (15) in items (11) and (13), respectively.

3) Estimate the predicted state

{\hat{x}}_{k + 1 | k}

(14).

4) Estimate the predicted error covariance P_k+1|k (16).

5) Compute the predicted information matrix Y_k+1|k (9) and the predicated information vector

{\hat{y}}_{k + 1 | k}

(10).

Measurement update

1) Factorize P_k+1|k gets S_k+1|k.

2) Calculate the cubature points X_i,k+1|k (18) and the propagated cubature points Z_i,k+1|k by measurement equation (19).

3) Estimate the predicted measurement

{\hat{z}}_{k + 1 | k}

(17).

4) Estimate the cross-covariance matrix P_xz,k+1|k (20).

5) Compute the updated information matrix Y_k+1|k+1 using (6), (9), (20).

6) Compute the updated information vector

{\hat{y}}_{k + 1 | k + 1}

in items of (8), and (20–22).

7) Estimate the updated state

{\hat{x}}_{k + 1 | k + 1}

(4) and the corresponding error covariance P_k+1|k+1 (3).

4 Two-stage High-degree Cubature Information Filter

In this section, we analyze the methods of the two-stage filter algorithm at first, then compare the advantages and disadvantages of the two methods, finally Two-stage high-degree Cubature Information filter (TSHCIF) is can be given. Through the summary of the literature, there are two kinds of methods can achieve the two-stage filter algorithm. One is the conventional method which is proposed by Friedland [18]. Friedland’s method is to decouple the augmented filter into two parallel reduced-order filters, the “bias-free” filter is based on the assumption that the bias is nonexistent, the bias filter, produces an estimate of the bias vector. The output of the “bias-free” filter is then corrected with the output of the “bias” filter. This method was proved that under an algebraic constraint the two-stage Kalman filter is optimal [18, 28], but the algebraic constraint is restrictive in practice, the conventional algorithm is most suboptimal. In the [29], two-stage Cubature Kalman filter (TSCKF) was proposed using the Friedland’s method. TSCKF uses traditional decoupled structure, bias is not considered in the bias-free filter and external bias-compensating input is not added in [19, 22]. Therefore, TSCKF is suboptimal unless the bias is deterministic and constant [28]. Another method is using the matrix transformation technique which is called two-stage transformation, in this transformation the algebraic constraint is not exist and the optimal performance is guaranteed [18 –22]. In the linear system, two-stage transformation is shown as the block digitalization of the matrix. But in the nonlinear system, two-stage transformation is shown as partial derivative function [21 , 24] which is digitalization of the covariance matrices. The key idea for Two-stage high-degree Cubature Information filter (THCIF) is using the Two-stage transformation in the framework of high-degree Cubature Information filter (HCIF) and make the covariance matrices block diagonal.

In the linear systems, two-stage Kalman Filter can be obtained through the formulas as follows [20]: $\begin{matrix} {\hat{X}}_{k | k - 1} = & T (U_{k}) {\bar{X}}_{k | k - 1} \\ {\hat{X}}_{k | k} = & T (V_{k}) {\bar{X}}_{k | k} \\ P_{k | k - 1} = & T (U_{k}) {\bar{P}}_{k | k - 1} T^{T} (U_{k}) \\ P_{k | k} = & T (V_{k}) {\bar{P}}_{k | k} T^{T} (V_{k}) \\ K_{k} = & T (V_{k}) {\bar{K}}_{k} \end{matrix}$ where $\bar{P} = diag {{\bar{P}}^{1}, {\bar{P}}^{2}}$ , and T transformation [20]: $T (G) = [\begin{matrix} I_{n - p} & G \\ 0 & I_{p} \end{matrix}]$

Lemma 1. To extend the two-stage Kalman Filter to nonlinear system used the method in [21] have the following formulas: ${\hat{X}}_{k | k - 1} = T (Φ, {\bar{X}}_{k | k - 1})$ (23) $P_{k | k - 1} = \frac{\partial T (Φ, {\bar{X}}_{k | k - 1})}{\partial {\bar{X}}_{k | k - 1}} {\bar{P}}_{k | k - 1} {(\frac{\partial T (Φ, {\bar{X}}_{k | k - 1})}{\partial {\bar{X}}_{k | k - 1}})}^{T}$ (24)

where Φ and Ψ are two determined nonlinear functions. Similarly, there are some formulas as follows: ${\hat{X}}_{k | k} = T (Ψ, {\bar{X}}_{k | k})$ (25) $P_{k | k} = \frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}} {\bar{P}}_{k | k} {(\frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}})}^{T}$ (26)

Here, there are K_k as follows:

$K_{k} = \frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}} {\bar{K}}_{k} □$ The following formulas have following properties: $\frac{\partial T (Φ, {\bar{X}}_{k | k - 1})}{\partial {\bar{X}}_{k | k - 1}} = [\begin{matrix} I_{n - p} & U_{k} \\ 0 & I_{p} \end{matrix}] \equiv T (U_{k})$ (27)

$\frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}} = [\begin{matrix} I_{n - p} & V_{k} \\ 0 & I_{p} \end{matrix}] \equiv T (V_{k})$ (28) Among them, the nonlinear T transformation formula is as follows [21]:

$T (F, X) = [\begin{matrix} X_{1} + F (X^{2}) \\ X_{2} \end{matrix}]$ (29)

where X = {(X¹) ^T (X²) ^T} ^T, in which X¹ ∈ R^n-p and X² ∈ R^p, and F (X²) is a nonlinear function of the substate X².

Accordingly, we obtained:

$U_{k} = \frac{\partial Φ ({\bar{X}}_{k | k - 1}^{2})}{\partial {\bar{X}}_{k | k - 1}^{2}}, V_{k} = \frac{\partial Ψ ({\bar{X}}_{k | k}^{2})}{\partial {\bar{X}}_{k | k}^{2}}$ (30)Lemma 2.(block-partitioned matrix inversion lemma). Matrix A, B, C, have appropriate dimension and C is invertible [?]. ${[\begin{matrix} A & B \\ B^{T} & C \end{matrix}]}^{- 1} = [\begin{matrix} D & E \\ E^{T} & F \end{matrix}]$ (31)

where ${\begin{matrix} D = (A - {BC}^{- 1} B^{T})^{- 1} \\ E = - {DBC}^{- 1} \\ F = C^{- 1} + C^{- 1} B^{T} {DBC}^{- 1} \end{matrix}$

Two-stage EKF (TSEKF) is realized by using nonlinear T transformation under the framework of EKF [21]. In the calculation process of EKF, in order to obtain the error covariance P_k|k and gain K_k, the Jacobian matrix is needed. But in CKF, it is needed to evaluate Jacobian matrix for measurement update, and hence the TSEKF framework can not be directly used. In order to solve this problem, in calculation process matrix block relationship needs to be found in the framework of CKF, and this relationship has the same effect of EKF matrix block. Based on this idea, a new matrix decoupling block method is proposed. In TSEKF, state matrix A_k is directly multiplied with the transformation matrix, the obtained matrix dimension is determined. Therefore, it is clearly that the matrix is divided into four blocks. In TSCKF, however, the whole matrix is obtained by a statistical method calculation, can not block matrix as simple as TSEKF. It needs to be divided according to the dimension of the state and bias.

Theorem 1. Two-stage HCIF (TSHCIF) estimator.

${\bar{X}}_{k | k}^{1}$ is the output of the modified bias-free filter.

$\begin{matrix} {\bar{X}}_{k | k - 1}^{1} = \sum_{i = 1}^{2 n^{2}} ω_{i} f^{1} (S_{k - 1 | k - 1} ξ_{i} + T (Ψ, {\bar{X}}_{k - 1 | k - 1}), u_{k - 1}) \\ - Φ ({\bar{X}}_{k | k - 1}^{2}) \\ {\bar{X}}_{k | k}^{1} = ({\bar{P}}_{k | k}^{1} + V_{k} {\bar{P}}_{k | k}^{2} V_{k}^{T}) ({\bar{y}}_{k | k}^{1} + Ψ ({\bar{y}}_{k | k}^{2})) + V_{k} {\bar{P}}_{k | k}^{2} {\bar{y}}_{k | k}^{2} \\ - Ψ ({\bar{X}}_{k | k}^{2}) \end{matrix}$

$\begin{matrix} + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2} - φ ({\bar{y}}_{k | k - 1}^{2}) \\ {\bar{y}}_{k | k}^{1} & = & {\bar{y}}_{k | k - 1}^{1} + φ ({\bar{y}}_{k | k - 1}^{2}) - ψ ({\bar{y}}_{k | k}^{2}) + ({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) L \\ [v_{k}^{1} + P_{xz, k | k - 1}^{T} (({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] + U_{k} {\bar{Y}}_{k | k - 1}^{2} L [v_{k}^{2} + P_{xz, k | k - 1}^{T} \\ ({\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] \\ {\bar{P}}_{k | k - 1}^{1} & = & M_{k - 1}^{11} + Q_{k - 1}^{11} - U_{k} (M^{12} + Q^{12})^{T} \\ {\bar{P}}_{k | k}^{1} & = & ({\bar{Y}}_{k | k}^{1})^{- 1} - V_{k} {\bar{P}}_{k | k}^{2} V_{k}^{T} \\ {\bar{Y}}_{k | k - 1}^{1} & = & ({\bar{P}}_{k | k - 1}^{1})^{- 1} - U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} \\ {\bar{Y}}_{k | k}^{1} & = & {\bar{Y}}_{k | k - 1}^{1} + {\bar{Y}}_{k | k - 1}^{1} N {\bar{Y}}_{k | k - 1}^{1} \\ + [I + ({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) N] U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} (U_{k}^{T} N {\bar{Y}}_{k | k - 1}^{1} + N {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) - V_{k} {\bar{Y}}_{k | k}^{2} V_{k}^{T} \end{matrix}$

${\bar{X}}_{k | k}^{2}$ is the output of the bias filter. $\begin{matrix} {\bar{X}}_{k | k - 1}^{2} & = & \sum_{i = 1}^{2 n^{2}} ω_{i} f^{2} (S_{k - 1 | k - 1} ξ_{i} + T (Ψ, {\bar{X}}_{k - 1 | k - 1}), u_{k - 1}) \\ {\bar{X}}_{k | k}^{2} & = & {\bar{P}}_{k | k}^{2} V_{k}^{T} ({\bar{y}}_{k | k}^{1} + Ψ ({\bar{y}}_{k | k}^{2})) + {\bar{P}}_{k | k}^{2} {\bar{y}}_{k | k}^{2} \\ {\bar{y}}_{k | k - 1}^{2} & = & {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} (({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2} \\ {\bar{y}}_{k | k}^{2} & = & {\bar{y}}_{k | k - 1}^{2} + {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} L \\ [v_{k}^{1} + P_{xz, k | k - 1}^{T} (({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] + {\bar{Y}}_{k | k - 1}^{2} L [v_{k}^{2} + P_{xz, k | k - 1}^{T} \\ ({\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] \\ {\bar{P}}_{k | k - 1}^{2} & = & M_{k - 1}^{22} + Q_{k - 1}^{22} \\ {\bar{P}}_{k | k}^{2} & = & ({\bar{Y}}_{k | k}^{2})^{- 1} + V_{k}^{T} ({\bar{Y}}_{k | k}^{1})^{- 1} V_{k} \\ {\bar{Y}}_{k | k - 1}^{2} & = & ({\bar{P}}_{k | k - 1}^{2})^{- 1} + U_{k} ({\bar{P}}_{k | k - 1}^{1})^{- 1} U_{k}^{T} \\ {\bar{Y}}_{k | k}^{2} & = & {\bar{Y}}_{k | k - 1}^{2} + {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} {NU}_{k} {\bar{Y}}_{k | k - 1}^{2} + {\bar{Y}}_{k | k - 1}^{2} N {\bar{Y}}_{k | k - 1}^{2} \end{matrix}$

Where $\begin{matrix} L & = & P_{xz, k | k - 1} R_{k}^{- 1} \\ N & = & P_{xz, k | k - 1} R_{k}^{- 1} P_{xz, k | k - 1}^{T} \end{matrix}$

The blending matrices U_k and V_k are given as follows: $\begin{matrix} U_{k} & = & (M_{k - 1}^{12} + Q_{k - 1}^{12}) (M_{k - 1}^{22} + Q_{k - 1}^{22})^{- 1} \\ V_{k} & = & [({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) {NU}_{k} {\bar{Y}}_{k | k - 1}^{2} \\ + & U_{k} {\bar{Y}}_{k | k - 1}^{2} (I + N {\bar{Y}}_{k | k - 1}^{2})] ({\bar{Y}}_{k | k}^{2})^{- 1} \end{matrix}$

Proof. According to the rules of nonlinear T transformation in (29), the information vector ${\hat{y}}_{k}$ and information matrix Y_k are obtained as follows: ${\hat{y}}_{k | k - 1} = T (Φ, {\bar{y}}_{k | k - 1})$ (32) ${\hat{y}}_{k | k} = T (Ψ, {\bar{y}}_{k | k})$ (33) $Y_{k | k - 1} = \frac{\partial T (Φ, {\bar{X}}_{k | k - 1})}{\partial {\bar{X}}_{k | k - 1}} {\bar{Y}}_{k | k - 1} {(\frac{\partial T (Φ, {\bar{X}}_{k | k - 1})}{\partial {\bar{X}}_{k | k - 1}})}^{T}$ (34) $Y_{k | k} = \frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}} {\bar{Y}}_{k | k} {(\frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}})}^{T}$ (35)

The left-hand side of (23), (25) are substituted by (14), (4) and right-hand using (29), obtains

$\begin{matrix} [\begin{matrix} {\bar{X}}_{k | k - 1}^{1} + Φ ({\bar{X}}_{k | k - 1}^{2}) \\ {\bar{X}}_{k | k - 1}^{2} \end{matrix}] \\ = \sum_{i = 1}^{2 n^{2}} ω_{i} f (S_{k - 1 | k - 1} ξ_{i} + T (Ψ, {\bar{X}}_{k - 1 | k - 1}), u_{k - 1}) \end{matrix}$ (36)

$\begin{matrix} [\begin{matrix} {\bar{X}}_{k | k}^{1} + Ψ ({\bar{X}}_{k | k}^{2}) \\ {\bar{X}}_{k | k}^{2} \end{matrix}] \\ = \frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}} {\bar{P}}_{k | k} {(\frac{\partial T (Ψ, {\bar{X}}_{k | k})}{\partial {\bar{X}}_{k | k}})}^{T} \\ [\begin{matrix} {\bar{y}}_{k | k}^{1} + Ψ ({\bar{y}}_{k | k}^{2}) \\ {\bar{y}}_{k | k}^{2} \end{matrix}] \end{matrix}$ (37)

Where u_k is an external bias-compensating input [19]. Expanding (36), (37) by using (28),(33), gets $\begin{matrix} {\bar{X}}_{k | k - 1}^{1} = \\ \sum_{i = 1}^{2 n^{2}} ω_{i} f^{1} (S_{k - 1 | k - 1} ξ_{i} + T (Ψ, {\bar{X}}_{k - 1 | k - 1}), u_{k - 1}) \\ - Φ ({\bar{X}}_{k | k - 1}^{2}) \end{matrix}$ (38) $\begin{matrix} {\bar{X}}_{k | k}^{1} = \\ ({\bar{P}}_{k | k}^{1} + V_{k} {\bar{P}}_{k | k}^{2} V_{k}^{T}) ({\bar{y}}_{k | k}^{1} + Ψ ({\bar{y}}_{k | k}^{2})) + V_{k} {\bar{P}}_{k | k}^{2} {\bar{y}}_{k | k}^{2} \\ - Ψ ({\bar{X}}_{k | k}^{2}) \end{matrix}$ (39) $\begin{matrix} {\bar{X}}_{k | k - 1}^{2} = \\ \sum_{i = 1}^{2 n^{2}} ω_{i} f^{2} (S_{k - 1 | k - 1} ξ_{i} + T (Ψ, {\bar{X}}_{k - 1 | k - 1}), u_{k - 1}) \end{matrix}$ (40) ${\bar{X}}_{k | k}^{2} = {\bar{P}}_{k | k}^{2} V_{k}^{T} ({\bar{y}}_{k | k}^{1} + Ψ ({\bar{y}}_{k | k}^{2})) + {\bar{P}}_{k | k}^{2} {\bar{y}}_{k | k}^{2}$ (41)

where $\begin{matrix} f (•) & = & [(f^{1} (•))^{T} (f^{2} (•))^{T}]^{T} \end{matrix}$ According to the dimension of the block matrix, the equation (22) is divided into the two part of the appropriate.

$\begin{matrix} v_{k} & = & [\begin{matrix} v_{k}^{1} \\ v_{k}^{2} \end{matrix}] \\ = & [\begin{matrix} z_{k} - \sum_{i = 1}^{2 n^{2}} ω_{i} h^{1} (S_{k | k - 1} ξ_{i} + T (φ, {\bar{X}}_{k | k - 1}, u_{k})) \\ z_{k} - \sum_{i = 1}^{2 n^{2}} ω_{i} h^{2} (S_{k | k - 1} ξ_{i} + T (φ, {\bar{X}}_{k | k - 1}, u_{k})) \end{matrix}] \end{matrix}$ (42)

where $\begin{matrix} h (•) & = & [(h^{1} (•))^{T} (h^{2} (•))^{T}]^{T} \end{matrix}$ According (9) and (10), can get $\begin{matrix} {\bar{y}}_{k | k - 1}^{1} = \\ [({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2}))] \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2} - φ ({\bar{y}}_{k | k - 1}^{2}) \end{matrix}$ (43) $\begin{matrix} {\bar{y}}_{k | k - 1}^{2} = \\ {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} (({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2} \end{matrix}$ (44)

Order $\begin{matrix} L = P_{xz, k | k - 1} R_{k}^{- 1} \end{matrix}$ Using (5)–(10), (32)–(35) show that:

$\begin{matrix} {\bar{y}}_{k | k}^{1} = {\bar{y}}_{k | k - 1}^{1} + φ ({\bar{y}}_{k | k - 1}^{2}) - ψ ({\bar{y}}_{k | k}^{2}) + ({\bar{Y}}_{k | k - 1}^{1} \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) L [v_{k}^{1} + P_{xz, k | k - 1}^{T} (({\bar{Y}}_{k | k - 1}^{1} \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] + U_{k} {\bar{Y}}_{k | k - 1}^{2} \\ L [v_{k}^{2} + P_{xz, k | k - 1}^{T} ({\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} ({\bar{X}}_{k | k - 1}^{1} \\ + φ ({\bar{X}}_{k | k - 1}^{2})) + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] \end{matrix}$ (45)

$\begin{matrix} {\bar{y}}_{k | k}^{2} = {\bar{y}}_{k | k - 1}^{2} + {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} L [v_{k}^{1} + P_{xz, k | k - 1}^{T} \\ (({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] + {\bar{Y}}_{k | k - 1}^{2} L [v_{k}^{2} + P_{xz, k | k - 1}^{T} \\ ({\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} ({\bar{X}}_{k | k - 1}^{1} + φ ({\bar{X}}_{k | k - 1}^{2})) \\ + {\bar{Y}}_{k | k - 1}^{2} {\bar{X}}_{k | k - 1}^{2})] \end{matrix}$ (46)

According to (16), order $\begin{matrix} M_{k - 1} = \\ \sum_{i = 1}^{2 n^{2}} ω_{i} (X_{i, k | k - 1}^{*} - {\hat{x}}_{k | k - 1}) (X_{i, k | k - 1}^{*} - {\hat{x}}_{k | k - 1})^{T} \end{matrix}$ there is $\begin{matrix} M_{k - 1} = [\begin{matrix} M_{k - 1}^{11} & M_{k - 1}^{12} \\ (M_{k - 1}^{12})^{T} & M_{k - 1}^{22} \end{matrix}] \end{matrix}$ obtained $\begin{matrix} P_{k | k - 1} & = & M_{k - 1} + Q_{k - 1} \\ = & [\begin{matrix} M_{k - 1}^{11} + Q_{k - 1}^{11} & M_{k - 1}^{12} + Q_{k - 1}^{12} \\ (M_{k - 1}^{12} + Q_{k - 1}^{12})^{T} & M_{k - 1}^{22} + Q_{k - 1}^{22} \end{matrix}] \end{matrix}$ using (24), (27) yields ${\bar{P}}_{k | k - 1}^{1} = M_{k - 1}^{11} + Q_{k - 1}^{11} - U_{k} (M_{k - 1}^{12} + Q_{k - 1}^{12})^{T}$ (47) ${\bar{P}}_{k | k - 1}^{2} = M_{k - 1}^{22} + Q_{k - 1}^{22}$ (48) $U_{k} = (M_{k - 1}^{12} + Q_{k - 1}^{12}) (M_{k - 1}^{22} + Q_{k - 1}^{22})^{- 1}$ (49) According (3), (26), (31), (35) ${\bar{P}}_{k | k}^{1} = ({\bar{Y}}_{k | k}^{1})^{- 1} - V_{k} {\bar{P}}_{k | k}^{2} V_{k}^{T}$ (50) ${\bar{P}}_{k | k}^{2} = ({\bar{Y}}_{k | k}^{2})^{- 1} + V_{k}^{T} ({\bar{Y}}_{k | k}^{1})^{- 1} V_{k}$ (51) According to (5), (6), (34), (35), order $\begin{matrix} N & = & P_{xz, k | k - 1} R_{k}^{- 1} P_{xz, k | k - 1}^{T} \end{matrix}$

we have $\begin{matrix} {\bar{Y}}_{k | k}^{1} = {\bar{Y}}_{k | k - 1}^{1} + {\bar{Y}}_{k | k - 1}^{1} N {\bar{Y}}_{k | k - 1}^{1} \\ + [I + ({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) N] U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} (U_{k}^{T} N {\bar{Y}}_{k | k - 1}^{1} + N {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) \\ - V_{k} {\bar{Y}}_{k | k}^{2} V_{k}^{T} \end{matrix}$ (52) $\begin{matrix} {\bar{Y}}_{k | k}^{2} = {\bar{Y}}_{k | k - 1}^{2} + {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T} {NU}_{k} {\bar{Y}}_{k | k - 1}^{2} \\ + {\bar{Y}}_{k | k - 1}^{2} N {\bar{Y}}_{k | k - 1}^{2} \end{matrix}$ (53) $\begin{matrix} V_{k} = [({\bar{Y}}_{k | k - 1}^{1} + U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}) {NU}_{k} {\bar{Y}}_{k | k - 1}^{2} \\ + U_{k} {\bar{Y}}_{k | k - 1}^{2} (I + N {\bar{Y}}_{k | k - 1}^{2})] ({\bar{Y}}_{k | k}^{2})^{- 1} \end{matrix}$ (54)

Using (31) again in equation (9), we can get ${\bar{Y}}_{k | k - 1}^{1} = ({\bar{P}}_{k | k - 1}^{1})^{- 1} - U_{k} {\bar{Y}}_{k | k - 1}^{2} U_{k}^{T}$ (55) ${\bar{Y}}_{k | k - 1}^{2} = ({\bar{P}}_{k | k - 1}^{2})^{- 1} + U_{k} ({\bar{P}}_{k | k - 1}^{1})^{- 1} U_{k}^{T}$ (56) It remains to solve the problem of obtaining Φ and Ψ. This can be done by using (30) and the backward difference equation as follows: $Φ ({\bar{X}}_{k | k - 1}^{2}) = Φ ({\bar{X}}_{k - 1 | k - 2}^{2}) + U_{k} ({\bar{X}}_{k | k - 1}^{2} - {\bar{X}}_{k - 1 | k - 2}^{2})$ (57) $Ψ ({\bar{X}}_{k | k}^{2}) = Ψ ({\bar{X}}_{k - 1 | k - 1}^{2}) + V_{k} ({\bar{X}}_{k | k}^{2} - {\bar{X}}_{k - 1 | k - 1}^{2})$ (58)

The proof is finished. □

The TSHCIF Algorithm are summarized in Table 2.

Table 2

The TSHCIF Algorithm

Time update

1) Evaluate S_k-1|k-1 by factorize P_k-1|k-1.

2) Calculate the cubature points X_i,k-1|k-1 using the new vector ξ_i (11)(12) and the propagated cubature points

X_{i, k | k - 1}^{*}

by state

Equation with the new weight vector ω_i(15) in items (11) and (13), respectively.

3) estimate the predicted state

{\bar{X}}_{k | k - 1}^{1}

(38) and

{\bar{X}}_{k | k - 1}^{2}

(40).

4) Estimate the predicted error covariance

{\bar{P}}_{k | k - 1}^{1}

(47),

{\bar{P}}_{k | k - 1}^{2}

(48), and U_k in (49).

5) Compute the predicted information matrix

{\bar{Y}}_{k | k - 1}^{1}

(55),

{\bar{Y}}_{k | k - 1}^{2}

(56) and the predicated information vector

{\hat{y}}_{k | k - 1}^{1}

(43) and

{\hat{y}}_{k | k - 1}^{2}

(44).

Measurement update

1) Factorize P_k|k-1 gets S_k|k-1.

2) Calculate the cubature points X_i,k|k-1(18) and the propagated cubature points Z_i,k|k-1 by measurement equation (19).

3) Estimate the predicted measurement

{\hat{z}}_{k | k - 1}

(17).

4) Estimate the cross-covariance matrix P_xz,k|k-1(20), calculate L and N.

5) Compute the updated information matrix

Y_{k | k}^{1}

Y_{k | k}^{2}

using (27), (28), (55), (56).

6) Compute the updated information vector

{\hat{y}}_{k | k}^{1}

{\hat{y}}_{k | k}^{2}

in items of (38) (40), (43), (44), (55), (56).

7) Estimate the updated state

{\bar{X}}_{k | k}^{1}

(39),

{\bar{X}}_{k | k}^{2}

(41) and the corresponding error covariance

{\bar{P}}_{k | k}^{1}

(50),

{\bar{P}}_{k | k}^{2}

(51).

5 Simulation Examples

In order to prove the validity of the proposed algorithm, the TSHCIF is used to estimate the attitude, speed and swing of the high speed wheel robot. The control process of high speed wheel robot using real-time Kinematic global positioning system and inertial sensors. All results are the mean of 100 Monte-Carlo simulations done on a computer with 2.4 GHz CPU and 4.00 GB RAM.

Consider the following discrete-time nonlinear system of the high speed wheel robot: $\begin{matrix} x_{k + 1} & = & f (x_{k}) + B_{k} b_{k} + ω_{k}^{x} \\ b_{k + 1} & = & b_{k} + ω_{k}^{b} \\ z_{k} & = & h (x_{k}) + ν_{k} \end{matrix}$ where $\begin{matrix} f (x_{k}) & = & [\begin{matrix} X_{k} + Δ t V_{l, k} cos (θ_{k}) - Δ t V_{y, k} sin (θ_{k}) \\ Y_{k} + Δ t V_{l, k} sin (θ_{k}) - Δ t V_{y, k} cos (θ_{k}) \\ V_{l, k} + Δ t V_{y, k} r_{k} + Δ t α_{x, k} \\ V_{y, k} - Δ t V_{l, k} r_{k} + Δ t α_{y, k} \\ θ_{k} + Δ t r_{m, k} \end{matrix}] \end{matrix}$ Δt is the sampling time for discrete systems, and Δt = 0.1. x_k is the state vector and x_k = [X_k Y_k V_l,k V_y,k θ_k] ^T, which is uncorrelated with the bias b_k and x₀ ∼ N (2, 0 . 05²). The process noise vector is $ω_{k}^{x} \sim N (0, 0 . 05^{2})$ . b_k is the bias and use $ω_{k}^{b} \sim N (0, 0 . 05^{2})$ .

The measure function is $\begin{matrix} h (x_{k}) & = & [\begin{matrix} X_{k} \\ Y_{k} \\ cos (θ_{k}) V_{l, k} - sin (θ_{k}) V_{y, k} \\ sin (θ_{k}) V_{l, k} - cos (θ_{k}) V_{y, k} \\ θ_{k} \end{matrix}] \end{matrix}$ z_k = [z_X,k z_Y,k z_{V_l,k} z_{V_y,k} z_θ,k] ^T is the observation vector which is consist of absolute position, velocity and position. And the observation noise is ν_k ∼ N (0, 0 . 05²).

Time varying parameters of the time k are provided by the accelerator and gyroscope. It is assumed that the instantaneous yaw rate r_k can be observed by the low noise gyroscope, therefore, selected r_k = r_m.

All results are the mean of 1000 Monte-Carlo simulations, and the average absolute error $\sum_{k = 1}^{k} x_{k} - {\hat{x}}_{k | k} / t$ (t is the simulation times) is used to describe estimation accuracy instatistical significance.

Figure 1 shows comparison of estimated value based on HCIF and TSHCIF for the position, trajectory of X, Y axis and angle.

Fig.1

Comparison of simulation results by two algorithms.

The Figs. 2–6 show that the accuracies of HCIF and TSHCIF are almost equal on the state vector. And Table 3 further point out the tiny differences between the two algorithms, the absolute value of the state vector differences are 0.0232, 0.0238, 0.0014, 0.0012, 0.0663 and it is too small to be neglected for practical applications. In fact, the differences are produced from the numerical computation error of digital computers. Therefore, it is concluded that the estimation accuracy of HCIF and TSHCIF is the same.

Fig.2

X-position error.

Fig.3

Y-position error.

Fig.4

X-velocity error.

Fig.5

Y-velocity error.

Fig.6

Angle error.

Table 3

Performances of the HCIF and TSHCIF filters

Average absolute error	HCIF	TSHCIF
X positron(m)	38.2529	38.2761
Y position(m)	37.5638	37.5876
X velocity(m/min)	134.4358	134.4372
Y velocity(m/min)	25.1331	25.1343
Angle(degree)	47.5969	47.6632

Figure 7 shows the comparison of bias estimation value by augmented HCIF and TSHCIF and Fig. 8 shows the estimation accuracy of the two algorithms. According to these result, the deflectable difference still appear, the reason was discussed before and cannot influence the equivalence of the two algorithms. We can once again conclude that the estimation accuracy of the two algorithms is the same.

Fig.7

Simulation results of Bias.

Fig.8

Bias error.

6 Conclusion

In this note, a nonlinear two-stage transformation is introduced to generalize the linear two-stage Kalman filtering technique. Using this new nonlinear two-stage transformation, the GTSEKF, which is the nonlinear counterpart of the previous proposed GTSKF, is proposed to In this paper, in order to reduce computational complexity, High-degree Cubature Information Filter (HCIF) is introduced. Based on frame of HCIF, using this new nonlinear two-stage transformation, we proposed the two-stage high-degree Cubature Information fileter (TSHCIF) for nonlinear system with unknown random bias. TSHCIF includes two-reduced order filter which use the method of making the covariance matrices block diagonal. It is shown that the proposed TSHCIF is mathematically to the HCIF and it is less computationally intensive than CKF. it prevents Augmented state Cubature Informantion Filter (ACIF) which brings overflow error and solves high-dimensional nonlinear filter problem with minimal computational effort.

Statement

The authors declare that there is no conflict of interest regarding the publication of this paper.

Footnotes

Acknowledgments

This work was partially supported by National Nature Science Fund of China (NSFC) (Grant No. 61403229, 61503213), Public Projects of Zhejiang Province (Grant No. 2017C31126).

References

Caballero-Gil

and Fuster-Sabater

, A wide family of nonlinea r filter functions with a large linear span, Information Sciences164(1-4) (2003), 197–207.

Sebesta

and Boizot

, A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J], IEEE Transactions on Industrial Electronics61(1) (2014), 495–503.

Julier

S.J.

, The scaled unscented transformation, American Control Conference Jefferson City, American, 2002, pp. 4555–4559.

Lefebvre

, Bruyninckx

and DeSchutter

, A new method for the nonlinear transformation of means and covariances in filters and estimators, IEEE Trans on Automatic Control47(8) (2002), 1406–1408.

Arasaratnam

and Haykin

, Cubature kalman filters, IEEE Transactions on Automatic Control54(6) (2009), 1254–1269.

Feng

and Lijun

, Initial alignment of large azimuth misalignment angle in SINS based on CKF, Chinese Journal of Scientific Instrument33(2) (2012), 327–333.

Long

, Naigang

, Feng

, et al., High-degree cubature Kalman filter and its application in target tracking, Journal of Harbin Engineering University37(4) (2016), 573–578.

Xijing

, Guangbin

, Lixin

, et al., Fifth degree cubature Kalman filter algorithm and its application, Infrared and Laser Engineering44(4) (2015), 1377–1381.

Tao

and Ming

, Hypersonic entry vehicle state estimation using high-degree cubature Kalman filter, AIAA Atmospheric Flight Mechanics Conference Atlanta, 2014, pp. 2383–2394.

10.

Bin

, Xin

and Yang

, The high-degree cubature Kalman filter, 51st IEEE Conference on Decision and Control, Maui, USA, 2012, pp. 4095–4100.

11.

Sizhong

, Lan

and Yang

, A sufficient condition for the existence of a k-factor excluding a given r-factor, Applied Mathematics and Nonlinear Science2(1) (2017), 13–20.

12.

Bortolan

M.C.

and Rivero

, Non-autonomous perturbations of a non-classical non-autonomous parabolic equation with subcritical nonlinearity, Applied Mathematics and Nonlinear Science2(1) (2017), 31–66.

13.

Bin

, Xin

and Cheng

, High-degree cubature Kalman filter, Automatica49(2) (2013), 510–518.

14.

Chuanbo

, Yunze

, Yurong

, et al., A reduced-order approach to filtering for systems with linear equality constraints, Neurocomputing193(12) (2016), 219–226.

15.

Gan

and Harris

C.J.

, Comparison of two measurement fusion methods for Kalman-filter-based multisensor data fusion, IEEE Transactions on Aerospace and Electronic Systems37(1) (2001), 273–280.

16.

Quanbo

, Daxing

and Chenglin

, Cubature information filters with correlated noises and their applications in decentralized fusion, Signal Procession94 (2014), 434–444.

17.

Jun

, Yu

, Wei

, et al., Traching ballistic target based on square-root Cubature Information Filter, Journal of CAEIT10(5) (2015), 527–532.

18.

Friedland

, Treatment of bias in recursive filtering, IEEE Transactions on Automatic ControlAC-14 (1969), 359–367.

19.

Hsieh

C.S.

and Chen

F.C.

, Optimal solution of the two-stage Kalman estimator, IEEE Transactions on Automatic Control44(1) (1999), 194–199.

20.

Hsieh

C.S.

and Chen

F.C.

, General two-stage kalman filters, IEEE Transactions on Automatic Control48(4) (2000), 819–824.

21.

Hsieh

C.S.

, General two-stage extended kalman filters, IEEE Transactions on Automatic Control48(2) (2003), 289–293.

22.

Keller

J.Y.

and Darouach

, Optimal two-stage Kalman filter in the presence of random bias, Automatica33(9) (1997), 1745–1748.

23.

, Jing

and Dinirovski

G.M.

, Two-stage Unscented Kalman Filter for Nonlinear System in the Presence of Unknown Random Bias, American Control Confernece, Washington, American, 2008, pp. 3530–3535.

24.

Xueqin

, Rui

, Wancheng

, et al., A novel two-stage extended Kalman filter algorithm for reaction flywheels fault estimation, Chinese Journal of Aeronautics29(2) (2016), 462–469.

25.

Chandra

K.P.B.

, Gu

D.W.

and Postlethwaite

, Cubature information filter and its applications, 2011 American Control Conference, San Francisco, CA, USA, 2011, pp. 3609–3614.

26.

Lee

D.J.

, Nonlinear estimation and multiplesensor fusion using unscented information filtering, IEEE Signal Processing Letters15 (2008), 861–864.

27.

Bharaani Chandra

K.P.

, Gu

D.W.

and Postlethwaite

, Square root cubature information filter, IEEE Sensors Journal13(2) (2013), 750–758.

28.

Alouani

A.T.

, Xia

, Rice

T.R.

, et al., On the optimality of two-stage state estimation in the presence of random bias, IEEE Transactions on Automatic Control38(8) (1993), 1279–1282.

29.

, Meilei

, Zhuyun

, et al., Two-Stage Cubature Kalman Filter for Nonlinear System with Random Bias, 2014 IEEE International Conference on Multisensor Fusion and Information (MFI 2014), Beijing, China, 2014, pp. 47–55.

30.

Chongzhao

, Multi-source Information Fusion, Tsinghua University Press, 2006, pp. 243–244.

31.

Quanbo

, Chenglin

and Shandong

, Cubature Kalman fusion for bearings only tracking networks, 3rd IFAC International Conference on Intelligent Control and Automation Science, Chengdu, China, 2013.