Quasi-Stochastic Integration Filter for Nonlinear Estimation

In practical applications, numerical instability problem, systematic error problem caused by nonlinear approximation, and nonlocal sampling problem for high-dimensional applications, exist in unscented Kalman filter (UKF). To solve these problems, a quasistochastic integration filter (QSIF) for nonlinear estimation is proposed in this paper. nonlocal sampling problem is solved based on the unbiased property of stochastic spherical integration rule, which can also reduce systematic error and improve filtering accuracy. In addition, numerical instability problem is solved by using fixed radial integration rule. Simulations of bearing-only tracking model and nonlinear filtering problem with different state dimensions show that the proposed QSIF has higher filtering accuracy and good numerical stability as compared with existing methods, and it can also solve nonlocal sampling problem effectively.


Introduction
Nonlinear filtering has been widely used in many applications.Generally, filtering problem can be addressed by using Bayesian estimation theory, which provides an optimal solution for dynamic state estimation problem by computing the posterior probability density function (PDF) [1,2].However, multidimensional integrals involved in the Bayesian estimation are typically intractable, and closed form solution to the Bayesian estimation is available only for a few special cases [3], for example, for a linear Gaussian system which leads to the well-known Kalman filter (KF) [4].In other cases, approximate methods are necessary to obtain suboptimal nonlinear filters.These methods can be divided into two groups: global and local methods [5,6].
Global methods do not make any explicit assumption about PDFs which are computed directly by using approximate methods [6], such as the point-mass filter using adaptive grids [7], the Gaussian mixture filter [8], the particle filter [9], and Quasi-Monte Carlo filter [10].Typically, global methods suffer from enormous computational demands.In addition, the performances of the particle filter and Quasi-Monte Carlo filter depend highly on the selection of proposal distributions [10].
Local methods derive nonlinear filters by assuming PDFs to be Gaussian, which leads to Gaussian filter (GF) [11].
The unscented transformation-(UT-) based unscented Kalman filter (UKF) is a typical Gaussian approximate filter and has been widely used due to its ease of implementation, modest computational cost, and appropriate performance [12,13].However, UKF suffers from three main problems in its practical applications: numerical instability problem [6,11], systematic error problem caused by nonlinear approximation [14][15][16], and nonlocal sampling problem for high-dimensional applications [17].
In order to address the numerical instability problem, Arasaratnam and Haykin proposed the cubature Kalman filter (CKF) based on the third-degree spherical-radial cubature rule [6].However, CKF is virtually a special case of UKF with parameter  = 0 and can only capture the third order information of Taylor series expansion for nonlinear approximation; thus large systematic errors may be produced.In addition, CKF suffers the nonlocal sampling problem for high-dimensional applications.
To reduce systematic errors caused by nonlinear approximation and improve filtering accuracy, Jia et al. proposed high-degree CKF which can capture the fifth order information of Taylor series expansion for nonlinear approximation by generalizing the third-degree spherical-radial cubature rule to arbitrary degree spherical-radial cubature rule [18].However, high order systematic errors still exist in high-degree CKF and the nonlocal sampling problem is not addressed in high-dimensional applications.Dunik et al. indicated that systematic errors exist in all Gaussian approximate filters for nonlinear estimation [14].To eliminate systematic errors caused by nonlinear approximation, Straka et al. proposed the stochastic integration filter (SIF) [14][15][16] based on the stochastic integral rule (SIR) [19].The posteriori mean and covariance computed by SIR tend to true posteriori mean and covariance in the sense of statistics because it can provide asymptotically unbiased integral computation; thus filtering accuracy is improved.However, a large number of round-off errors are introduced into integration computation using the third-degree SIR (3rd-SIR) because the negative weight is utilized in the 3rd-SIR; thus the filtering numerical stability cannot be ensured and the filtering accuracy will degrade greatly, even filtering divergence may appear in the third-degree SIF (3rd-SIF).
To address nonlocal sampling problem for highdimensional applications, Chang et al. proposed the transformed UKF (TUKF) by using the transformed sigma points to compute the nonlinear posteriori mean and covariance in the Gaussian filtering frame [17].As compared with CKF and UKF, high order terms of nonlinear posteriori mean and covariance computed by TUKF are much smaller, and the nonlocal sampling problem is addressed to some extent.However, similar to CKF algorithm, TUKF can only capture the third order information of Taylor series expansion for nonlinear approximation; thus large systematic errors still exist in TUKF.Besides, the high order information of transformed sigma points computing mean and covariance may be in the opposite direction of the true terms in some cases, which causes TUKF to produce much larger systematic errors.
In conclusion, the above-mentioned existing typical nonlinear filtering methods including UKF, CKF, high-degree CKF, TUKF, and 3rd-SIF cannot simultaneously address numerical instability problem, systematic error problem, and nonlocal sampling problem.To simultaneously address these problems, a quasi-stochastic integration rule (QSIR) with arbitrary degree accuracy is proposed in this paper based on fixed radial integral rule (FRIR) and stochastic spherical integral rule (SSIR).A novel quasi-stochastic integration filter (QSIF) with arbitrary degree accuracy is then obtained by applying the proposed QSIR to compute multidimensional integrals involved in GF.QSIF addresses the nonlocal sampling problem for high-dimensional applications and reduces systematic errors by SSIR.The numerical instability problem is solved by FRIR, thus filtering accuracy is improved.
To illustrate the superiority of the proposed QSIF algorithm, two numerical simulations are presented including bearings-only tracking and nonlinear filtering problem with different state dimensions.As can be seen from simulation results of bearings-only tracking, the proposed QSIF has higher filtering accuracy and better numerical stability than existing filtering algorithms.Simulation results of nonlinear filtering problem with different state dimensions show that the nonlocal sampling problem can be effectively addressed by the proposed QSIF.Note that, as a special case of SIF, the first-degree SIF (1st-SIF) proposed in [16] can also address numerical instability problem, systematic error problem, and nonlocal sampling problem.However, its estimation accuracy can be further improved, and, as shown in our simulations, the proposed 5th-QSIF has higher estimation accuracy than the 1st-SIF, and it provides a new way to solve the abovementioned problems.
The remainder of the paper is organized as follows.The GF for nonlinear estimation and its problems are introduced in Section 2. The QSIR based on FRIR and SSIR is proposed in Section 3. The specific third-degree QSIF (3rd-QSIF) and fifth-degree QSIF (5th-QSIF) are given in Section 4. Simulations are provided in Section 5. Concluding remarks are drawn in Section 6.

Gaussian Filter and Problem Statement
2.1.Gaussian Filter.Consider a discrete nonlinear system written in the form of dynamic state space model as follows: where x  ∈ R  and z  ∈ R  represent the immeasurable state of the system and measurement at time , respectively.f(⋅) and h(⋅) are any known functions.w −1 is Gaussian process noise with zero mean and known covariance Q −1 .v  is Gaussian measurement noise with zero mean and known covariance R  and it is uncorrelated with the process noise.The nonlinear filter is to obtain the minimum variance estimate of the true but unknown system state based on the noisy observations at time ( ≤ ), that is, E[x  | Z  ] with Z  = {z  , 1 ≤  ≤ } [11].
If the PDF is well approximated by the Gaussian distribution, a Gaussian filter in the Kalman-like structure (using linear update rule) can be used to address the estimation task.The Gaussian distribution is uniquely characterized by its first two-order moments (mean and covariance), and the general Gaussian filter is formulated as [6] where It can be seen from the above update that the general GF consists of one-step prediction of state and measurement in (5) and measurement update in the sense of linear minimum mean square error (MSE) in ( 2)-( 4).The heart of the GF is to compute Gaussian weighted integrals in (5) and different Gaussian filters will be obtained by using different numerical methods.For example, the UKF can be obtained by using the UT to compute Gaussian weighted integrals in (5).However, the numerical instability problem, systematic error problem, and nonlocal sampling problem exist in the UKF.Next we will show systematic errors and nonlocal sampling problem by introducing the computation of the mean and covariance in cubature transform (CT) and the numerical instability problem by introducing the 3rd-SIR.

Systematic Errors. Suppose that x is an 𝑛-dimensional
Gaussian random vector with mean x and covariance P  .Another   -dimensional random vector y is related to x through the nonlinear function y = g(x).The CT can be used to calculate the mean and covariance of y.The mean and covariance of y using CT, that is, ŷCT and P CT  , are formulated as where √P  is the square root matrix of P  ; that is, √P  √P  T = P  .  = [0, 0, . . ., 1  , . . ., 0] T denotes a unit vector to the direction of coordinate axis .
Taylor series expansion is the most commonly used tool for the error analysis of local methods.Firstly, computational error of mean using CT will be considered.The Taylor expansion of the true mean of y is as follows [12,13]: where Δx  is the th element of the vector Δx.
The Taylor expansion of computational mean of y can be formulated as follows [14,15,20]: where () is the th element of the vector   .The systematic error  CT is defined as the difference between the true mean value shown in (7) and the estimated mean value calculated by CT in (9): Generally, the error  CT is nonzero [14].Similarly, it can be shown that the covariance matrix computations using the CT also contain systematic errors; thus systematic errors exist in CKF.Finally, it should be noted that systematic errors can be found in all local filters.To reduce systematic errors, highdegree CKF captures the fifth order information of nonlinear Taylor series expansion in (7) by using more sigma points, but high order errors still exist.Systematic errors in (11) in local filters are because of bias of the deterministic numerical integration for computing multidimensional integrals [14][15][16].To address this problem, Genz and Monahan proposed the SIR by using stochastic radial integral rule (SRIR) and SSIR to fulfill the Gaussian weighted integrals computation.Because the SIR is unbiased, that is, E[ SIR ] = 0, hence, the SIR can eliminate the systematic errors in (11) and improve filtering accuracy.However, the 3rd-SIF suffers from numerical instability problem which will be specified in Section 2.2.3.

Nonlocal Sampling Problem.
The fourth and higherorder moments (hom) of the th component for the computational posterior mean of CT in (9) can also be written as [17] As can be seen from ( 12), hom is proportional to the index power of state dimension  −1 and increases as state dimension increases.More seriously, the computational posterior mean and covariance using CT will have large high order information for high-dimensional problem coupled with strong nonlinear functions such as the exponent and trigonometric, which will degrade the filtering accuracy of CKF [17].This problem is regarded as the nonlocal sampling problem whose essence is that sigma points used in CKF are far away from the mean as the state dimension increases, which degrades the ability of approximating PDF.The TUKF proposed in [17] mitigates the nonlocal sampling problem by constructing transformed sigma points based on orthogonal transform and improves the filtering accuracy of CKF or UKF.However, similar to CKF algorithm, TUKF can only capture the third order information of Taylor series expansion for nonlinear approximation; thus large systematic errors still exist.Besides, the high order information of transformed sigma points computing mean and covariance may be in the opposite direction of the true terms in some cases, which causes TUKF to produce much larger systematic errors.
The mean of y computed by the 3rd-SIR is formulated as where  0 = 1−/ 2 and   = 1/2 2 with  ∼  2 ( + 2).Q is a uniformly random orthogonal matrix. is randomly chosen from  2 ( + 2); thus its value will be in the range [0 √ ] with a certain probability.Consequently,  0 = 1 − / 2 will be negative with a certain probability.In particular, if As a result, large round-off errors are introduced for integration computation [6,11,21,22], which may result in divergence of the 3rd-SIF method when the system noise is large.This problem is regarded as numerical instability problem and it also exists in UKF [6].CKF method which is based on deterministic spherical-radial cubature rule can avoid round-off errors of numerical computation and has good numerical stability.However, the accuracy of CKF is low and nonlocal sampling problem exists in CKF for highdimensional applications.
To mitigate numerical instability problem, systematic error problem, and nonlocal sampling problem in nonlinear estimation, a QSIR with arbitrary degree accuracy is proposed based on the FRIR and the SSIR in the paper.Then the QSIF with arbitrary degree accuracy will be obtained by applying the proposed QSIR to compute multidimensional integrals involved in GF.Next we will introduce the QSIR.

Quasi-Stochastic Integral Rules
Definition 1.We introduce the following integral: where x = [ 1  2 ⋅ ⋅ ⋅   ] T ∈ R  and   (x) is a given weighting function.Equation ( 14) is a th-degree rule if it is exact for all monomials and there is at least one monomial of degree  + 1 for which ( 14) is not exact.
It is clear from the analysis in Section 2.1 that the key problem of GF is how to compute multidimensional integrals formulated as A crucial step before applying the spherical-radial cubature rule is to transform the integration variable from Cartesian coordinate system to spherical-radial system.Define x = s with s T s = 1; then where s = [ 1 ,  2 , . . .,   ] T ,   = {s ∈ R  :  2 1 + 2 2 +⋅ ⋅ ⋅+ 2  = 1}, and (s) is the spherical surface measure or an area element on   .Two types of integrals are contained in (16), that is, radial integral ∫ ∞ 0 g  () −1 exp(− 2 ) and spherical integral ∫   g(s)(s).
The integration rule proposed in this paper is QSIR, in which the spherical integral ∫   g(s)(s) is computed by using SSIR and radial integral ∫ ∞ 0 g  () −1 exp(− 2 ) is computed by using FRIR.Next we will introduce SSIR and FRIR, respectively.

Stochastic Spherical Integral Rule
Lemma 2 (see [18]).For the spherical integral I   (g  ) = ∫   g  (s)(s), the (2 + 1)th-degree full symmetric spherical integral rule is formulated as where I   denotes a spherical integral and I   ,2+1 denotes (2 + 1)th-degree full symmetric spherical integral rule.Here  p and G{u p } are defined as where   is a nonnegative integer.p = [ Lemma 3 (see [19,23]).If Q is a uniformly random orthogonal matrix, According to Lemma 2, the third-degree full symmetric spherical integral rule can be formulated as [18] Based on Lemma 3, the unbiased third-degree full symmetric SSIR can be formulated as Note that ( 20) is based on the third-degree full symmetric spherical integral rule and can capture the third order information at least, and it needs 2 spherical integral points, that is, ±Qe  ( = 1, 2, . . ., ).The existing first-degree SIR (1st-SIR) is based on the first-degree full symmetric spherical integral rule and can capture the first order information at least, and it needs 2 spherical integral points, that is, ±Q, where  is any point on   .Similarly, the unbiased fifthdegree full symmetric SSIR can be formulated as where In the calculation of the unbiased fifth-degree SSIR, we need 2 2 spherical integral points.
When   = 1, the radial integral rule that is exact for g  () = 1,  2 is formulated as [18] When   = 2, the radial integral rule that is exact for g  () = 1,  2 ,  4 is formulated as [18] As can be seen from ( 24) and ( 25) the above given FRIRs are not exact for odd-degree polynomials such as g  () = ,  3 .Fortunately, when the FRIRs are combined with the SSIRs to compute the integral (15), the combined sphericalradial rule vanishes for all odd-degree polynomials because SSIR vanishes by symmetry for any odd-degree.Hence, the arbitrary degree QSIR can be obtained by combining SSIR introduced in Section 3.1 and FRIR introduced in Section 3.2.Next we will formulate two QSIF methods based on the proposed QSIR.

Quasi-Stochastic Integral Filtering Methods
The third-degree quasi-stochastic integration algorithm (3rd-QSIA) and fifth-degree QSIA (5th-QSIA) will be specified in this section.We use them to compute Gaussian weighted multidimensional integrals in (5) and obtain the corresponding 3rd-QSIF and 5th-QSIF.
Step 1. Choose a maximum number of iterations  max .
Step 3. Repeat (until  =  max ) the following loop.(c) Compute the values  3 and  5 at current iteration: and use them to update the approximate variance and integral value as Step 4. Output the approximate integral value Ĩ and standard deviation   = √  of the 3rd-QSIA and 5th-QSIA.
Note that the variable  max provides a tradeoff between efficiency and accuracy, and its selection depends on application requirements.For the calculation of (b), Stewart (1980) gave an algorithm for generating from the uniform distribution (invariant Haar measure) over orthogonal matrices [23].Essentially, the approach is to generate an  ×  matrix X of standard norm variables and form the QR factorization: X = QR.Then Q has the right distribution.The algorithm for computing Ĩ and   is a modified version of a stable onepass algorithm [19].The output standard deviations  3 and  5 can be used to evaluate the exactness of the 3rd-QSIA and 5th-QSIA, respectively.
Based on GF frame and using the above 3rd-QSIA and 5th-QSIA to compute Gaussian weighted multidimensional integrals in (5), the 3rd-QSIF and 5th-QSIF can be developed.

Properties of Quasi-Stochastic Integration
Filters.QSIF is obtained by using QSIR to compute Gaussian weighted multidimensional integrals in (5); thus its properties depend completely on properties of QSIR.
Firstly, QSIF can reduce systematic errors and improve filtering accuracy by using unbiased spherical integral computation.The computational accuracy of Gaussian weighted multidimensional integral mainly depends on the computational accuracy of spherical integral [18,21,22].An unbiased spherical integral computation is important in nonlinear GF.
Secondly, QSIF has good numerical stability, similar to that of CKF.QSIR has the same weights as deterministic spherical-radial cubature rule because it uses FRIR to compute the radial integral.The 3rd-QSIR is a completely stable numerical integral rule because its weights are all positive.The 5th-QSIR is also completely stable when state dimension is less than four; that is,  ≤ 4.However, it is not completely stable when  ≥ 5 due to ∑ 2 Thirdly, high order information of computational mean and covariance of QSIF converges to true values and its high order errors do not increase as state dimension increases, so QSIF can mitigate the nonlocal sampling problem.
Finally, a comparison of computational complexity between the proposed filters and existing Gaussian approximate filters is shown in Table 1.The computational complexity of the proposed QSIF and existing SIF are all dependent on the iteration numbers, and the proposed 3rd-QSIF and existing 3rd-SIF are almost consistent in computational complexity.Note that, as a special case of SIF, the 1st-SIF can be deemed as MCKF with antithetic variates [16].Although its computational complexity in a single iteration is smaller than the proposed QSIF algorithms, it needs more iteration numbers to achieve equivalent accuracy as compared with 3rd-SIF, 3rd-QSIF, and 5th-QSIF.As will be shown in later simulations, the proposed 5th-QSIF has higher estimation accuracy than the 1st-SIF with equivalent computational complexity by choosing different iteration numbers for both algorithms.
In conclusion, the proposed QSIF not only has high accuracy and good numerical stability, but also can effectively mitigate the nonlocal sampling problem.Next the advantages of the proposed QSIF as compared with existing methods will be illustrated by two simulation examples.

Simulations
The high accuracy and good numerical stability of the proposed QSIF are illustrated by a bearings-only tracking simulation.A nonlinear filtering problem with different state dimensions is used to illustrate that the proposed QSIF can effectively mitigate the nonlocal sampling problem.

Bearings-Only
Tracking.The considered nonlinear model, describing the bearings-only tracking, is of the form [24] x  = [ 0.9 0 0 1 ] T and the associated covariance P 0|0 = [0.1 0; 0 0.1] T .Under the same initial conditions, we give simulation results of TUKF, MCKF with sampling points of 50, 1st-SIF with iteration numbers of 25, 3rd-SIF with iteration numbers of 5, 3rd-CKF, 5th-CKF, the proposed 3rd-QSIF, and 5th-QSIF with iteration numbers of 5. Note that the 1st-SIF, MCKF, and the proposed 5th-QSIF have almost consistent computational complexity in this simulation.Besides, we also give the conditional posterior Cramér-Rao low bound (CPCRLB) [25] for better comparison.To compare the performances of these filters, simulation results are presented in the form of figures and tables.In figures, all filtering methods are intuitively compared by using the MSE performance index defined as follows: In Table 2, all filtering methods are specifically compared by using the average MSE (AMSE) performance index defined as follows: where x  , is the th component of the true state in the th simulation at time  and x ,/ is its filtering estimate,  denotes the number of Monte Carlo simulations, and  denotes the simulation time.We set  as 100 seconds in simulation.
For a fair comparison, we do 1000 independent Monte Carlo runs.MSEs and CPCRLB of positions are shown in Figures 1 and 2, and AMSEs and single step running times are shown in Table 2. (Note that TUKF is identical to 3rd-CKF when state dimension  = 2.) As shown in Figures 1 and 2, the existing 3rd-SIF and MCKF diverge at time 30 s and 10 s, respectively, and the proposed 3rd-QSIF and 5th-QSIF outperform the existing 3rd-CKF and 5th-CKF in terms of filtering accuracy, respectively.From Table 2, we can see that the proposed 3rd-QSIF has almost consistent computation demands with the existing 3rd-SIF, and the proposed 5th-QSIF provides higher estimation accuracy than the existing 1st-SIF with almost consistent computation demands.

Nonlinear Problem with Different State Dimensions.
We consider a nonlinear filtering problem with different state dimensions to show the nonlocal sampling problem for different methods.The nonlinear model is formulated as [17] x  = 2 cos (x −1 ) + q −1 , where x  is assumed to be an -dimensional Gaussian random variable.q  ∼ (; 0 ×1 , I  ) is the Gaussian process noise, 0 ×1 denotes an -dimensional column vector with all its elements equal to 0. I  denotes an -dimensional identity matrix.k  is Gaussian measurement noise with zero mean and unity variance.The true initial state x 0 = 0.1×1 ×1 , where 1 ×1 denotes an -dimensional column vector with all its elements equal to 1.The initial conditions for all methods are x0 = 0 ×1 and P 0|0 = I  .We aim to compare the performances of 3rd-CKF, 5th-CKF, TUKF, 1st-SIF, 3rd-SIF, 3rd-QSIF, and 5th-QSIF for different state dimensions  ( = 1 : 30).The performance is compared by using the MSE defined as follows: where  denotes the number of Monte Carlo simulations and  denotes the simulation time.We set  as 100 seconds in simulation.x  1, is the first component of the true state in the th simulation at time  and x 1,/ is its filtering estimate.In fact, any element of x  can be chosen to illustrate the performance due to their symmetric status in x  and x  (1) is chosen in our simulation.
We do 50 independent Monte Carlo runs under the same initial conditions.MSEs for state dimensions  = 1 : 30 are shown in Figure 3, and single step running times of all filters for 30-dimensional system model are shown in Table 3.
As shown in Figure 3, TUKF outperforms the 1st-SIF, 3rd-SIF, 3rd-CKF, and 5th-CKF, especially for high state dimensions, and the proposed 3rd-QSIF and 5th-QSIF considerably outperform TUKF for moderate state dimensions in terms of filtering accuracy, and the proposed 3rd-QSIF, 5th-QSIF, and TUKF are almost consistent in filtering accuracy for high state dimensions.Besides, from Figure 3 and Table 3, we can see that the proposed 3rd-QSIF has higher filtering accuracy than the existing 1st-SIF with almost consistent computation demands.Simulation results illustrate that the proposed QSIF can effectively mitigate the nonlocal sampling problem.

Figure 3 :
Figure 3: MSE with different state dimensions.

Table 1 :
Comparison of computational complexity.

Table 3 :
Single step running times for 30-dimensional system model.
A QSIF method is proposed based on unbiased SSIR and FRIR in the paper.It can effectively solve the numerical instability problem, systematic error problem caused by nonlinear approximation, and nonlocal sampling problem for high-dimensional applications which exist in Gaussian filtering.Simulations of bearing-only tracking model and nonlinear filtering problem with different state dimensions show the advantages as compared with existing Gaussian filtering algorithms.