Iterated posterior linearization filters and smoothers with cross-correlated noises

Journal Pre-proof Iterated posterior linearization filters and smoothers with cross-correlated noises Yanhui Wang, Hongbin Zhang, Yang Li

PII: DOI: Reference:

S0019-0578(20)30003-3 https://doi.org/10.1016/j.isatra.2020.01.008 ISATRA 3449

To appear in:

ISA Transactions

Received date : 21 May 2018 Revised date : 11 December 2019 Accepted date : 3 January 2020 Please cite this article as: Y. Wang, H. Zhang and Y. Li, Iterated posterior linearization filters and smoothers with cross-correlated noises. ISA Transactions (2020), doi: https://doi.org/10.1016/j.isatra.2020.01.008. This is a PDF file of an article that has undergone enhancements after acceptance, such as the addition of a cover page and metadata, and formatting for readability, but it is not yet the definitive version of record. This version will undergo additional copyediting, typesetting and review before it is published in its final form, but we are providing this version to give early visibility of the article. Please note that, during the production process, errors may be discovered which could affect the content, and all legal disclaimers that apply to the journal pertain.

© 2020 Published by Elsevier Ltd on behalf of ISA.

*Title page showing Author Details

Journal Pre-proof Title: Iterated

Posterior

Linearization

Filters

and

Smoothers

with

Cross-Correlated Noises

pro of

Author names and affiliations: Yanhui Wang 1

School of Electrical Engineering and Automation, Hefei University of

Technology, Hefei, Anhui, 230009, P.R. China. 2

School of Information and Communication Engineering, University of

P.R. China.

Corresponding author: Hongbin Zhang

lP

[email protected]

re-

Electronic Science and Technology of China, Chengdu, Sichuan, 611731,

urn a

School of Information and Communication Engineering, University of Electronic Science and Technology of China, Chengdu, Sichuan, 611731, P.R. China.

[email protected]

Jo

Li Yang

School of Information and Communication Engineering, University of Electronic Science and Technology of China, Chengdu, Sichuan, 611731, P.R. China. [email protected]

*Highlights (for review)

Journal Pre-proof 1. This paper represents several novel iterated posterior linearization methods to tackle the estimation problem for nonlinear state-space models with cross-correlated noises, which is troublesome to other

pro of

algorithms. 2. The proposed iterated algorithms feature concise derivation, higher estimation accuracy and stronger robustness.

3. The presented iterated approaches can achieve an effective balance between estimation accuracy and computational complexity by the

Jo

urn a

lP

re-

iterated operations.

Journal Pre-proof *Blinded Manuscript - without Author Details Click here to view linked References

pro of

Iterated Posterior Linearization Filters and Smoothers with Cross-Correlated Noises

Abstract

In the article, several concise and efficient iterated posterior linearization filtering and smoothing methodologies are proposed for nonlinear systems with cross-correlated noises. Based on the Gaussian approximation (GA), the presented methods are derived via performing statistical linear regressions (SLRs) of the nonlinear state-space models w.r.t the current posterior distribution in

re-

an iterated way. Various posterior linearization methods can be developed by employing different approximation computation approaches for the Gaussian-weighted integrals encountered in SLRs. These new estimation methods enjoy not only the accuracy and robustness of the GA filter but also the lower computational complexity. Estimation performances of the designed methods are illus-

lP

trated and compared with conventional estimation schemes by two common numerical examples. Keywords: Bayesian filtering, Iterated smoothing, Statistical linear regression, Nonlinear systems, Cross-correlated noises

urn a

1. Introduction

In the recent decades, nonlinear state estimation has been gaining widespread attention in many practical applications [1, 2]. In order to conveniently derive the general solution for the nonlinear estimation, the process and measurement noises, i.e., the system or model noises, are always assumed 5

to be independent. For this case, the Bayesian approach [3] is the most prevalent solution, which can recursively calculate the posterior probability density functions (PDFs). However, due to the intractability of multi-dimensional integrals encountered in the Bayesian approach, the closed-form

Jo

expressions of such PDFs are generally unavailable and numerous Gaussian approximation (GA) filtering and smoothing methods [4–9] are developed to get the approximative numerical solutions. 10

It is worth noting that the GA filters can accurately approximate the posterior PDF only when the posterior PDF is unimodal.

Preprint submitted to Journal of ISA Transactions Templates

December 11, 2019

Journal Pre-proof

However, in many practical engineering applications, the independence assumption of system noises can not be guaranteed and they may be cross-correlated simultaneously or one (sampling) period apart, which will severely weaken the performances of the conventional estimation methods based on the noise independence assumption. Such estimation problem with cross-correlated noises

pro of

15

has drawn considerable attention and become a challenging research topic recently [10–13]. For nonlinear models with synchronous cross-correlated noises, such as the measurement-based output feedback closed-loop control systems, there are three filtering methods including the de-correlating filter [14], the Gaussian approximation (GA) recursive filter (GARF) [15] and the correlated Gaus20

sian approximate filter (CGAF) [16] that can be used to solve this estimation problem. Research proves that the conventional de-correlating filter is equivalent to GARF for linear Gaussian systems [17], and, for nonlinear systems, they are equal to each other in the linear minimum mean

re-

square error (LMMSE) sense [18]. The accuracy of CGAF has proven to be superior to those of its corresponding de-correlating filter and GARF in some applications when the Gaussian-weighted 25

integrals involved in them are calculated by the same Gaussian approximate rules [16]. However, CGAF is sensitive to the correlation parameter. In other words, the advantages of estimation ac-

lP

curacy and robustness can not be guaranteed when the correlation parameter changes only a little, which seriously limits its scope of applications.

Furthermore, the cases that system noises are one-step cross-correlated are also common in 30

many engineering applications. For instance, to discretize a continuous nonlinear system, an addi-

urn a

tional noise contribution will be introduced into the measurement model from the discrete sampling process of the state [19]. Another well-known instance is an airplane flying through the random wind field. The effect of wind in the airplane’s acceleration can be modeled as the process noise, which will be one-step correlated with the measurement noise because any anemometer mounted 35

on the airplane is also corrupted by wind [20]. In order to deal with such estimation problem possessing asynchronous cross-correlated noises, a linear Kalman filter (KF) solution is derived for the linear systems [19, 20], while the systematic solutions to the nonlinear models are rarely studied. Despite the existing Gaussian approximate approaches [21] can be used to solve this kind of

40

Jo

nonlinear estimation problem, such methods will be confronted with the low estimation accuracy and weak robustness, especially for some applications with unimodal Gaussian PDF. Therefore, when confronted with such estimation problem, there is a great demand on exploring more efficient methods to improve the estimation accuracy and robustness.

2

Journal Pre-proof

Besides the standard nonlinear GA filters and smoothers [4–9], there are many iterated estimation ways to obtain the approximative numerical solutions of the Bayesian method, such as iterated 45

extended KF (IEKF) [4, 14], iterated unscented KF (IUKF) [22], iterated posterior linearization

pro of

filter (IPLF) [23], and iterated posterior linearization smoother (IPLS) [24]. The primary distinction between IEKF (IUKF) and EKF (UKF) [4, 5] is the measurement update. The former updates the state to the maximum a posteriori (MAP) estimation obtained by one measurement iteration approximation, whereas the latter calculates the updated state as a linear combination of predic50

tion and innovation, i.e., an approximate conditional mean. Correspondingly, the filtering mean is approximated by a LMMSE-based estimator in the state update of IPLF and the filtering error is approximated as the mean square error (MSE). LMMSE-based filters generally have better filtering accuracy than MAP-based approximations since the estimation performances are usually assessed

55

re-

by square error [23]. Only the measurement functions are relinearized in IPLF while the process models are also relinearised in IPLS [24], which is the crucial distinction between IPLF and IPLS. It has been proved that the iterated approaches are far superior to the corresponding non-iterative methods in many practical applications [25–27].

lP

Enlightened by the advantages of IPLF, an improved estimation performance may be anticipated for nonlinear models possessing cross-correlated noises if statistical linear regressions (SLRs) of 60

the system functions are performed with respect to (w.r.t) the current optimal distribution. Specifically, for the case of simultaneous cross-correlated noises, a de-correlating IPLF can be derived

urn a

directly for nonlinear models since the noise correlation can be eliminated by reconstructing a pseudo process equation in the de-correlating framework and then the conventional nonlinear GA filters can be applied. For the case of one-step cross-correlated noises, some modifications are needed 65

before IPLF is implemented to obtain the nonlinear estimation solution. Firstly, a statistical linear error propagation is derived by implementing SLRs of nonlinear systems and the multidimensional integrals involved in the SLRs are approximated by the GA approaches [5–7]. Secondly, the proposed recursion is executed based on the latest posterior approximation in an iterative way. The motivation comes from the fact that SLRs of nonlinear functions make it easy for the recursion of state estimation with cross-correlated noises to be derived, and the posterior iterated operator

Jo

70

provides a more accurate estimation. Based on these improvements, an effective balance between computational complexity and precision can be expected for these newly proposed approaches. The rest of this paper is outlined below. The general recursive frameworks are derived for

3

Journal Pre-proof

nonlinear models possessing cross-correlated noises in Section 2. In Section 3, a de-correlating IPLF 75

and a modified IPLF are derived for nonlinear models possessing simultaneous and one-step crosscorrelated noises, respectively. To further improve the estimation performance, the corresponding

pro of

smoothing approaches are also proposed. In Section 4, the estimation performances are illustrated and analysed by two common numerical examples. Section 5 draws some conclusions and the further recommendation is presented in Section 6.

80

2. General recursive frameworks for nonlinear models with cross-correlated noises The considered discrete nonlinear systems are modeled by the process (dynamic) and measurement (observation) formulas below

(1)

zk = h(xk ) + vk ,

(2)

re-

xk = f (xk−1 ) + wk−1

where the mappings f (·) and h(·) are nonlinear, xk is an nx -dimensional vector to be estimated, zk

lP

is an nz -dimensional observation, the white noises wk−1 and vk are Gaussian and have covariances Qk−1 and Rk , respectively. The system noises may be cross-correlated simultaneously satisfying E[wk vlT ] = Sk δkl or one-step cross-correlated satisfying E[wk−1 vlT ] = Sk δkl . Here E stands for the expected value operator and δkl denotes the Kronecker delta. The prior PDF is p(x0 ) = ¯ 0 , P0 ), which is uncorrelated with system noises. N (x0 ; x

urn a

The estimation goal is to get the recursive Gaussian approximation (GA) distributions N (xk ; ˆ k|k−1 , Pk|k−1 ) and N (xk ; x ˆ k|k , Pk|k ) to the predicted function p(xk |Zk−1 ) and posterior function x

p(xk |Zk ), respectively. Here, the set Zk = {zi }ki=1 . In other words, the main task is to compute the first two moments of the above two Gaussian distributions as accurate as possible. When system noises are cross-correlated concurrently, the desired state estimation can be transformed into an equivalent estimation with uncorrelated noises by reconstructing a pseudo-process equation [28]. Add a zero vector to the right of formula (1) to get

Jo

85

xk = f (xk−1 ) + wk−1 + Dk−1 (zk−1 − hk−1 (xk−1 ) − vk−1 )

(3)

This results in the modified process and measurement equations xk = (f (xk−1 ) − Dk−1 hk−1 (xk−1 )) + Dk−1 zk−1 + (wk−1 − Dk−1 vk−1 ) 4

(4)

Journal Pre-proof

zk = h(xk ) + vk ,

(5)

which has new process noise of (wk−1 − Dk−1 vk−1 ) with zero mean and the term Dk−1 zk−1 can be

pro of

regarded as an additional deterministic input. The motivation behind these manipulations is that the matrix Dk−1 can be selected to make (wk−1 − Dk−1 vk−1 ) uncorrelated with vk . That is to say   T E (wk−1 − Dk−1 vk−1 )vk−1 = Sk−1 − Dk−1 Rk−1

which is zero if we select

−1 Dk−1 = Sk−1 Rk−1

(6)

(7)

With this choice, the modified process and measurement formulas can be described as (8)

zk = h(xk ) + vk .

(9)

The new process noise autocorrelation is

re-

−1 −1 −1 xk = (f (xk−1 ) − Sk−1 Rk−1 hk−1 (xk−1 )) + Sk−1 Rk−1 zk−1 + (wk−1 − Sk−1 Rk−1 vk−1 )

(10)

lP

  −1 T E (wk−1 − Dk−1 vk−1 )(wk−1 − Dk−1 vk−1 )T = Qk−1 − Sk−1 Rk−1 Sk−1 ,

which is less than the Qk−1 term arising from the original process noise wk−1 .

Enlightened by the KF’s estimation solution to the linear systems possessing one-step crosscorrelated noises, a natural idea is that the state estimation in nonlinear case can be tackled if the

urn a

nonlinear functions can be accurately approximated in linear form. A common approximation of nonlinear function used in the literature is shown below f (xk ) ≈ Fk xk + ak + ek

(11)

h(xk ) ≈ Hk xk + bk + gk ,

(12)

where the linearization parameters (Fk , Hk , and so forth) have appropriate dimensions, ek and gk are Gaussian white noises and their covariances are Λk and Ωk , respectively, which can be regarded as the terms for assessing the approximation errors.

Jo

90

In the enabling linear approximation of nonlinear function with associated PDF, SLR method provides a linearization approximation (affine) function with minimum mean square error (MMSE). SLR has the ability to obtain the affine function with high linearization approximate precision, which is described as below.

5

Journal Pre-proof

¯ P ), SLR of f (x) Definition 1. For nonlinear mapping f (x) with Gaussian PDF p(x) = N (x; x, w.r.t p(x) can be summarized as follows [29]

(14)

Λ = Φ − F P F T,

(15)

Φ= 95

Z

Z

f (x)p(x)dx

(16)

¯ ¯ T p(x)dx (x − x)(f (x) − y)

(17)

¯ (x) − y) ¯ T p(x)dx. (f (x) − y)(f

(18)

re-

Ψ=

Z

(13)

¯ a = y¯ − H x

where y¯ =

pro of

F = ΨT P −1

For nonlinear mapping f (x), a best affine approximation f (x) ≈ F x + a can be achieved with MMSE and Λ is its mean square error (MSE) matrix [29].

In most practice nonlinear cases, computing the required moments of SLR (16)–(18) directly

lP

is intractable and the multidimensional integrals can be approximately calculated by the sigmapoint approaches. One frequently used approximation technique is shown as Algorithm 1, whose sigma-points and weights are selected according to a suitable GA method [5–7]. Algorithm 1 SLR of nonlinear function using Gaussian approximation approach ¯ P. Input: Nonlinear function f (x) and a Gaussian PDF p(x) possessing the first two moments x,

urn a

Output: The returned affine approximation parameters (F , a, Λ). ¯ and P , and select the corresponding weights w1 , . . . , wm . 1: Generate m sigma-points χ1 , . . . , χm according to x 2: Estimate the propagated sigma-points Yi = f (χi )

i = 1, . . . , m.

3: Approximate the moments (16)–(18) as

y¯ ≈

Jo

Ψ≈ Φ≈

m X i=1 m X

i=1 m X i=1

wi Yi ¯ ¯ T wi (χi − x)(Y i − y) ¯ ¯ T wi (Yi − y)(Y i − y)

4: Calculate (F , a, Λ) from equations (13)–(15). 100

¯ P ) to stand for SLR of f (x) w.r.t For notational clarity, we use (F , a, Λ) = SLR(f (x), x, ¯ P ) and (F , a, Λ) are the returned approximation parameters. Gaussian PDF N (x; x, 6

Journal Pre-proof

ˆ k|k by updating the previous estimaTo proceed on, it is desired to get the current estimation x ˆ k−1|k−1 based on all currently available measurements. With the aid of linear approximation, tion x 105

the general recursive estimation framework can be derived directly for nonlinear models possessing

pro of

one-step cross-correlated noises, which is summarized as Theorem 1.

ˆ k|k , Pk|k ) of p(xk |Zk ) is obtained by the formulas below Theorem 1. The GA distribution N (x; x ˆ k|k = x ˆ k|k−1 + Kk (zk − Hk x ˆ k|k−1 − bk ) x


Pk|k = Pk|k−1 − Kk Hk Pk|k−1 HkT + Ωk + Rk + Hk Sk + SkT HkT KkT
−1 Kk = (Pk|k−1 HkT + Sk ) Hk Pk|k−1 HkT + Ωk + Rk + Hk Sk + SkT HkT

re-

where Kk is gain and

ˆ k|k−1 = Fk−1 x ˆ k−1|k−1 + ak−1 x T Pk|k−1 = Fk−1 Pk−1|k−1 Fk−1 + Λk−1 + Qk−1

ˆ k|k−1 , Pk|k−1 ) (Hk , bk , Ωk ) = SLR(h(x), x

lP

ˆ k−1|k−1 , Pk−1|k−1 ). (Fk−1 , ak−1 , Λk−1 ) = SLR(f (x), x Proof. The analogous theorem has been proved in the relevant literature [14, 19]. Similar to Theorem 1, for nonlinear models possessing simultaneous cross-correlated noises, the analogous recursive framework can be obtained immediately by applying the posterior linearization

urn a

filter (PLF) [23] to the equations (8) and (9), which is depicted as Theorem 2. ˆ k|k , Pk|k ) by the Theorem 2. The GA of p(xk |Zk ) is expressed as a normal distribution N (x; x following formulae

ˆ k|k = x ˆ k|k−1 + Kk (zk − Hk x ˆ k|k−1 − bk ) x


Pk|k = Pk|k−1 − Kk Hk Pk|k−1 HkT + Ωk + Rk KkT
−1 Kk = Pk|k−1 HkT Hk Pk|k−1 HkT + Ωk + Rk

Jo

110

where Kk is gain and

−1 Dk−1 = Sk−1 Rk−1 ∗ Fk−1 = Fk−1 − Dk−1 Hk−1

7

Journal Pre-proof

∗ ˆ k|k−1 = Fk−1 ˆ k−1|k−1 + Dk−1 (zk−1 − bk−1 ) + ak−1 x x ∗ ∗ T Pk|k−1 = Fk−1 Pk−1|k−1 (Fk−1 )T + Λk−1 + Qk−1 + Dk−1 (Ωk−1 − Rk−1 )Dk−1

pro of

ˆ k|k−1 , Pk|k−1 ) (Hk , bk , Ωk ) = SLR(h(x), x ˆ k−1|k−1 , Pk−1|k−1 ). (Fk−1 , ak−1 , Λk−1 ) = SLR(f (x), x

If the affine functions can accurately approximate nonlinear system functions, i.e., the terms Λ = 0 and Ω = 0, an alternative filtering formulation of Theorem 2, which produces a priori mean and covariance, can be expressed as follows after some tedious algebraic manipulations

re-

ˆ k+1|k = Fk x ˆ k|k−1 + Kk (zk − Hk x ˆ k|k−1 − bk ) + ak x
−1 T Kk + Qk−1 Pk+1|k = Fk Pk|k−1 FkT − Kk Hk Pk|k−1 HkT + Rk
−1 Kk = (Fk Pk|k−1 HkT + Sk ) Hk Pk|k−1 HkT + Rk .

(19) (20) (21)

Here Kk is called the modified Kalman gain. Except for formula (21), equations (19) and (20) are identical to their counterparts of the filter with independent system noises, which can be obtained

lP

directly from equations (19)–(21) by setting the cross-correlation term Sk = 0. The only effect of the simultaneous correlation of system noises is to modify the Kalman gain. From equation (20), 115

it is clear that the error covariance is decreased owing to more useful information provided by the cross-correlation term Sk .

urn a

3. Development of the filters and smoothers with correlated noises 3.1. The de-correlating iterated estimation methods with simultaneous cross-correlated noises Inspired by the development of de-correlating estimation framework as well as the superiority of 120

IPLF, a de-correlating IPLF method can be derived for nonlinear models possessing simultaneous ˆ 0|0 = x0 and P0|0 = P0 . The cross-correlated noises. Assume the initial estimation moments are x de-correlating IPLF algorithm is summarized as Algorithm 2.

Jo

To take full advantage of the measurement information, SLR of observation function is executed w.r.t current posterior estimation in each iteration (except the first iteration) update of Algorithm 2. 125

In the de-correlating IPLF, the best available approximation of posterior distribution is taken into account to perform iterated SLRs of nonlinear functions.

8

Journal Pre-proof

Algorithm 2 The de-correlating iterated posterior linearization filter ˆ 0|0 , P0|0 and the number of iterations J. Input: Initial moments x ˆ k|k , Pk|k (k = 0, . . . , N ). Output: Posterior moment estimations x 1: for k = 1 → N do

1 =P ˆ k|k ˆ k|k , Pk|k by Theorem 2 and set Pk|k ˆ 1k|k = x Compute x k|k , x

3:

for j = 2 → J do

4:

j−1 ˆ j−1 (Hkj , bjk , Ωjk ) = SLR(h(x), x , Pk|k ) k|k

5:

ˆ jk|k x

6:

j−1 Kkj = Pk|k (Hkj )T j j−1 Pk|k = Pk|k − Kkj

7:

=

ˆ j−1 x k|k

+

Kkj (zk h

−

ˆ j−1 Hkj x k|k

−

pro of

2:

. First iteration

. Using Algorithm 1

bjk )

i−1 j−1 Hkj Pk|k (Hkj )T + Ωjk + Rk h i j−1 Hkj Pk|k (Hkj )T + Ωjk + Rk (Kkj )T

8:

end for

9:

J . ˆ k|k = x ˆJ Set x , Pk|k = Pk|k k|k

re-

10: end for

Based on a reconstructed pseudo process equation and Rauch-Tung-Striebel (RTS) smoother [30], the de-correlating RTS iterated posterior linearization smoother (RTS-IPLS) algorithm can be obtained directly for nonlinear models possessing simultaneous cross-correlated noises, which is sum-

lP

marized as Algorithm 3.

Algorithm 3 The de-correlating RTS iterated posterior linearization smoother ˆ 0|0 , P0|0 and the number of iterations J. Input: Initial moments x Output: Posterior moment estimations uk|k , Wk|k (k = 0, . . . , N ). ˆ k|k , Pk|k by Algorithm 2. 1: Compute the first two moments x ˆ N |N , WN |N = PN |N . 2: uN |N = x 4: 5: 6:

−1 Ck = Pk|k (Fk∗ )T Pk+1|k

ˆ k|k + Ck (uk+1|k+1 − x ˆ k+1|k ) uk|k = x

Wk|k = Pk|k + Ck (Wk+1|k+1 − Pk+1|k )CkT

7: end for 130

. Smoothing

urn a

3: for k = N − 1 to 0 do

. Filtering

3.2. The proposed iterated filter and smoother with one-step cross-correlated noises

Jo

On account of Theorem 1, for nonlinear models possessing one-step cross-correlated noises, a modified IPLF method can be derived in a similar way to Algorithm 3, which is summarized in Algorithm 4. Considering that the effect of cross-correlated noises has been partially incorporated 135

into the step 2 of Algorithm 4 and the subsequent iterations primarily rely on the measurements, the Kalman gain is modified as step 6 of Algorithm 4 in the following iterations. 9

Journal Pre-proof

Algorithm 4 The modified iterated posterior linearization filter ˆ 0|0 , P0|0 and the number of iterations J. Input: Initial moments x ˆ k|k , Pk|k (k = 0, . . . , N ). Output: Posterior moment estimations x 1: for k = 0 → N do

1 =P ˆ k|k ˆ k|k , Pk|k by Theorem 1 and set Pk|k ˆ 1k|k = x Compute x k|k , x

3:

for j = 2 → J do

4:

j−1 ˆ j−1 (Hkj , bjk , Ωjk ) = SLR(h(x), x , Pk|k ) k|k

5:

ˆ jk|k x

6:

j−1 Kkj = Pk|k (Hkj )T j j−1 Pk|k = Pk|k − Kkj

7:

=

ˆ j−1 x k|k

+

Kkj (zk h

−

ˆ j−1 Hkj x k|k

−

pro of

2:

. First iteration

. Using Algorithm 1

bjk )

i−1 j−1 Hkj Pk|k (Hkj )T + Ωjk + Rk + Hkj Sk + SkT (Hkj )T h i j−1 Hkj Pk|k (Hkj )T + Ωjk + Rk + Hkj Sk + SkT (Hkj )T (Kkj )T

8:

end for

9:

J . ˆ k|k = x ˆJ Set x , Pk|k = Pk|k k|k

re-

10: end for

Unlike the de-correlating RTS-IPLS, the RTS smoother no longer holds for nonlinear models possessing one-step cross-correlated noises in the sense of mathematics. Fortunately, on the basis of SLR approximation of nonlinear systems, the desired smoother can be interpreted as a function of two linear filters [31], which results in a so-called forward-backward posterior linearization smoother

lP

(FB-PLS). One of these two linear filters is the forward filter that works forward over measurements to the point of interest, which has been shown as Theorem 1. The other one referred to as the backward information filter runs backward over measurements to the same point, which is described as Theorem 3.

urn a

Theorem 3. The GA of posterior distribution p(xb,k |Zk ) using the information vector yb,k|k and matrix Yb,k|k has the following form

T Yb,k|k = Yb,k|k−1 +(Hb,k +Yb,k|k−1 Sk ) Rk +Ωb,k −SkT Yb,k|k−1 Sk T Lb,k = (Hb,k + Yb,k|k Sk )(Rk + Ωb,k + Hb,k Sk )−1
yb,k|k = I + Lb,k SkT yb,k|k−1 + Lb,k (zk − bb,k )  −1 Mb,k = Yb,k+1|k+1 Yb,k+1|k+1 + (Qk + Λb,k )−1

T yb,k|k−1 = Fb,k (I − Mb,k ) yb,k+1|k+1 − Yb,k+1|k+1 ab,k

Jo

140

T Yb,k|k−1 = Fb,k (I − Mb,k )Yb,k+1|k+1 Fb,k

ˆ b,k+1|k+1 , Pb,k+1|k+1 ) (Fb,k , ab,k , Λb,k ) = SLR(f (x), x ˆ b,k|k−1 , Pb,k|k−1 ). (Hb,k , bb,k , Ωb,k ) = SLR(h(x), x 10





−1

(SkT Yb,k|k−1 +Hb,k )

Journal Pre-proof

To clarify the above notations, we first define the following covariance expressions

where ˜ b,k|k−1 , x ˆ b,k|k−1 − xk , x 145

h i ˜ b,k|k x ˜T Pb,k|k , E x b,k|k

pro of

h i ˜ b,k|k−1 x ˜T Pb,k|k−1 , E x b,k|k−1 ,

˜ b,k|k , x ˆ b,k|k − xk . x

(22)

(23)

Just before the measurement at time tk , the backward state xb,k is estimated by the backwardˆ b,k|k−1 and x ˆ b,k|k are the backward filtering expectation and prediction running filter. Hence, x mean, respectively. Due to the measurement going backward in time, the measurement update and prediction roles of backward filter are reversed from forward filter. The information form of −1 backward filter is the backward information filter and they are related by Yb,k|k = Pb,k|k , Yb,k|k−1 =

re-

−1 ˆ b,k|k−1 . ˆ b,k|k , and yb,k|k−1 = Yb,k|k−1 x Pb,k|k−1 , yb,k|k = Yb,k|k x

Proof. We only prove the propagations of Yb,k|k and yb,k|k . The proofs of the remaining equations are easily derived and omitted for convenience. Based on Theorem 1, the covariance Pb,k|k can also be denoted as

lP

 −1 T T T Pb,k|k = Pb,k|k−1 −(Pb,k|k−1 Hb,k +Sk ) Hb,k Pb,k|k−1 Hb,k +Ωb,k +Rk +Hb,k Sk +SkT Hb,k (Hb,k Pb,k|k−1 +SkT )

By utilizing the matrix inversion lemma, an alternative form of Pk|k is written as  −1  −1 −1 −1 −1 −1 T Pb,k|k = Pb,k|k−1 + (Hb,k + Pb,k|k−1 Sk ) Rk + Ωb,k − SkT Pb,k|k−1 Sk (SkT Pb,k|k−1 + Hb,k )

urn a

Inversing both sides of the above equation and substituting the information matrix into this resulting matrix inversion expression yields

T Yb,k|k = Yb,k|k−1 + (Hb,k + Yb,k|k−1 Sk ) Rk + Ωb,k − SkT Yb,k|k−1 Sk


−1

(SkT Yb,k|k−1 + Hb,k )

To derive the expression of yb,k|k , the backward Kalman gain Kb,k can be obtained directly based on Theorem 1, which is described as

  T T T Kb,k Rk + Ωb,k + Hb,k Sk + Hb,k Pb,k|k−1 Hb,k + SkT Hb,k = (Pb,k|k−1 Hb,k + Sk )

Jo

150

Collecting terms now gives

 T  + Sk Kb,k (Rk + Ωb,k + Hb,k Sk ) = (I − Kb,k Hb,k )Pb,k|k−1 − Kb,k SkT Hb,k T = Pb,k|k Hb,k + Sk

11

Journal Pre-proof

The above expression can be reorganized as T Kb,k = (Pb,k|k Hb,k + Sk )(Rk + Ωb,k + Hb,k Sk )−1

pro of

Then one can write the backward measurement update equation as ˆ b,k|k = x ˆ b,k|k−1 + Kb,k (zk − Hb,k x ˆ b,k|k−1 − bb,k ) x

−1 ˆ b,k|k−1 + Kb,k (zk − bb,k ) = (Pb,k|k + Kb,k SkT )Pb,k|k−1 x

−1 −1 ˆ b,k|k−1 + Kb,k (zk − bb,k ) = Pb,k|k (I + Pb,k|k Kb,k SkT )Pb,k|k−1 x −1 Left multiplying Pb,k|k on both sides of the above formula and substituting the information vector

and matrix into this resulting matrix inversion expression lead to the desired form

re-


yb,k|k = I + Lb,k SkT yb,k|k−1 + Lk (zk − bb,k )

T with Lb,k = (Hb,k + Yb,k|k Sk )(Rk + Ωb,k + Hb,k Sk )−1 .

lP

Forward filter

ˆ k|k−1 x ˆ k|k x yb,k|k yb,k|k−1 k

N

Backward information filter

urn a

0

Figure 1: Forward filter and backward information filter.

The forward filter and backward information filter are illustrated as Figure 1. The task is to ˆ k|k and yb,k|k−1 . Since the forward filter and seek a smoothing estimation that is a combination of x backward information filter are uncorrelated, based on the Millman’s theorem [32], the smoothing

Jo

estimation µk|k can be obtained directly and expressed as −1 −1 ˆ k|k + Pb,k|k−1 ˆ b,k|k−1 ) µk|k = Wk|k (Pk|k x x

(24)

with the smoothing covariance −1 −1 Wk|k = (Pk|k + Pb,k|k−1 )−1

12

(25)

Journal Pre-proof

For scalar systems, equation (25) reduces down to Wk|k =

Pk|k Pb,k|k−1 Pk|k + Pb,k|k−1

(26)

pro of

Equation (26) clearly shows that Wk|k ≤ Pk|k and Wk|k ≤ Pb,k|k−1 , which indicates that, in most cases, the smoothing accuracy is always higher than either the forward filter or the backward filter. This analysis and conclusion can easily be extended to the multidimensional nonlinear systems. Since the smoother and the forward filter must have the same estimation at terminal time tk = tN , ˆ N |N and WN |N = PN |N . According to equation (25) then, this obviously requires that µN |N = x

−1 Pb,N |N −1 ≡ Yb,N |N −1 = 0. However, for the following backward measurement update equation (27),

ˆ b,N |N −1 is yet unknown the terminal state boundary x

(27)

re-

ˆ b,k|k = x ˆ b,k|k−1 + Kb,k (zk − Hb,k x ˆ b,k|k−1 − bb,k ) x

To solve this difficulty, the information form of state update is considered, which is given in Theˆ b,k|k−1 we have orem 3. Since Yb,N |N −1 = 0, then from the relation equation yb,k|k−1 = Yb,k|k−1 x ˆ b,N |N −1 . This is why we propagate yb,k|k−1 instead yb,N |N −1 = 0, which is valid for any value of x

lP

ˆ b,k|k−1 in backward information filter since we do not know the boundary condition x ˆ b,N |N −1 . of x After some algebraic manipulations, the equations (24) and (25) are respectively rewritten as ˆ k|k + Wk|k yb,k|k−1 uk|k = (I − Gk )x

(29)

urn a

Wk|k = (I − Gk )Pk|k

(28)

with the smoothing gain Gk = Pk|k Yb,k|k−1 (I + Pk|k Yb,k|k−1 )−1 . Based on the aforementioned discussions, the forward-backward iterated posterior linearization smoother (FB-IPLS) algorithm can be derived as Algorithm 5. The correlation parameters Sk are not taken into account in the iterated 155

operations of Algorithm 5 since the effect of the noise cross-correlation has been incorporated into the anterior smoothing manipulations.

It is noteworthy that, in the iterated operations of Algorithms 2 and 3, the SLRs of nonlin-

Jo

ear functions are performed w.r.t the latest eatimation which only considers previous and current measurements. In Algorithm 5, the future measurements are also taken into account by the s160

moothing operations to linearize nonlinear systems and the estimated performances are able to be further improved in theory since the future measurements can also provide useful information. It also should be noted that each iteration (except the first iteration) of Algorithm 5 only relinearizes 13

Journal Pre-proof

Algorithm 5 The forward-backward iterated posterior linearization smoother ˆ 0|0 , P0|0 and the number of iterations J. Input: Initial moments x Output: Posterior moment estimations uk|k , Wk|k (k = 0, . . . , N ). ˆ k|k , Pk|k using Theorem 1. 1: Compute x

pro of

. Forward filtering

ˆ N |N , WN |N = PN |N , yb,N |N −1 = 0, Yb,N |N −1 = 0. 2: Smoothing initialisation: uN |N = x 3: for k = N − 1 to 0 do 4:

Calculate the information vector yb,k|k and matrix Yb,k|k by Theorem 3. )−1

5:

Compute the smoothing gain Gk = Pk|k Yb,k|k−1 (I + Pk|k Yb,k|k−1

6:

Evaluate the estimation covariance Wk|k = (I − Gk )Pk|k

7:

ˆ k|k + Wk|k yb,k|k−1 Calculate the estimation state uk|k = (I − Gk )x

8: end for

1 =W 9: Iteration initialisation: u1k|k = uk|k , Wk|k k|k (k = 0, . . . , N ).

10: for j = 2 → J do 11:

for k = 0 → N do

j−1 (Hkj , bjk , Ωjk ) = SLR(h(x), uj−1 , Wk|k ) k|k

13:

− bjk ) + Kkj (zk − Hkj uj−1 ujk|k = uj−1 k|k k|k h i−1 j−1 j−1 Kkj = Wk|k (Hkj )T Hkj Wk|k (Hkj )T + Ωjk + Rk i h j−1 j−1 j (Hkj )T + Ωjk + Rk (Kkj )T − Kkj Hkj Wk|k = Wk|k Wk|k

14: 15: 16:

re-

12:

end for

. Smoothing

. Iteration

. Using Algorithm 1

lP

17: end for

. Backward filtering

J (k = 0, . . . , N ). 18: Set uk|k = uJ , Wk|k = Wk|k k|k

the measurement functions rather than both the dynamic and measurement functions [24]. The

165

urn a

reasons for this are that the subsequent iterations depend mainly on the measurements and the computational burden can be reduced significantly by this simplified linearizing operation. Notice that the backward SLR parameters (Fb,k , Hb,k and so forth) need to be calculated in Algorithm 5, they are generally approximated and substituted by the corresponding forward SLR parameters to save computing overhead when the approximation error is small. Although RTS smoother no longer holds in the strict mathematical sense when the system 170

noises are one-step cross-correlated, compared with any loss in estimation quality, the savings

Jo

in computational load make this smoothing approach attractive in estimating the state of many nonlinear systems, which results in a suboptimal but simplified smoother. When the cross-correlated parameters Sk are small, the suboptimal RTS iterated posterior linearization smoother (RTSIPLS) can be obtained by substituting the RTS smoothing manipulations into Algorithm 5, which 175

is described as Algorithm 6. Compared with Algorithm 5, the calculation error can be further 14

Journal Pre-proof

decreased in Algorithm 6 without calculating the backward information filter. Algorithm 6 The Rauch-Tung-Striebel iterated posterior linearization smoother ˆ 0|0 , P0|0 and the number of iterations J. Input: Initial moments x ˆ k|k , Pk|k by Theorem 1. 1: Compute posterior moments x

pro of

Output: Posterior moment estimations uk|k , Wk|k (k = 0, . . . , N ). ˆ N |N , WN |N = PN |N . 2: Smoothing initialisation: uN |N = x 3: for k = N − 1 to 0 do 4:

−1 Ck = Pk|k FkT Pk+1|k

5:

ˆ k|k + Ck (uk+1|k+1 − x ˆ k+1|k ) uk|k = x

6:

Wk|k = Pk|k + Ck (Wk+1|k+1 − Pk+1|k )CkT

7: end for

8: The iteration steps are the same as those of Algorithm 5 counterparts.

. Filtering . Smoothing

. Iteration

re-

The iteration processes of Algorithms 2–6 will be stopped only when certain termination conditions are satisfied. Termination conditions may be different in various situations, and the number of iterations is a common criterion for terminating the iteration process. Since a good estimation 180

performance can generally be obtained with an acceptable computational cost in many practical

lP

applications only after 2–3 iterations, the value of iterations J is typically set to value 2–3. Another ˆ jk|k − x ˆ j−1 criterion in common use for terminating the iteration is that the inequality kx k|k k ≤ Vth can be met. The predefined threshold Vth plays an important role in successfully use of the iterated method, but it is not easy to select an appropriate Vth . Furthermore, a modified inequality for 185

terminating the iteration process is given in [22]. Although after some iterations, the modified

urn a

inequality does not hold automatically, the number of iterations is usually a considerable value. What’s worse, the modified inequality may not work well in some cases because it is sensitive to numerical errors.

Remark 1. Algorithm 2 is similar to the existing filters with cross-correlated noises mentioned 190

in the introduction and it can be reduced to them under certain conditions [23]. More precisely, if Λ = 0, Ω = 0 and the linearization parameters F , H, a and b are obtained based on Taylor

Jo

series expansions at MAP estimation, Algorithm 2 is reduced to IEKF [25] for nonlinear models possessing simultaneous cross-correlated noises. If J = 1 and the SLR parameters F , H, a, b, Λ, and Ω are selected w.r.t the prior, Algorithm 2 is reduced to prior linearization filter (PrLF) [23]. If 195

the sigma-points that are drawn from the prior estimation are applied to approximating the moments of PrLF , Algorithm 2 becomes the common derivative-free KF such as UKF [5] or CKF [6]. 15

Journal Pre-proof

3.3. Computational complexity analysis The computation overheads of Algorithms 2–6 and their corresponding comparison approaches are analyzed and compared in this subsection. The de-correlating algorithms IPLF and RTSIPLS are compared with the existing method CGAF [16] for nonlinear models possessing simul-

pro of

200

taneous cross-correlated noises. For nonlinear models possessing one-step cross-correlated noises, the modified IPLF, FB-IPLS, and RTS-IPLS are compared with the GA filtering and smoothing methods [21], which are called as the Gaussian approximate standard filter (GASF) and Gaussian approximate standard smoother (GASS) respectively in this paper. 205

For a fair comparison, the Gaussian-weighted integrals encountered in all algorithms are computed by the cubature rule [6]. To assess the computational complexity, the floating-point operations (FLOPs) required by all the considered algorithms are analyzed in detail based on the technical

re-

report [33]. In the evaluation process, the FLOPs of the arithmetic operations such as the multiplication, the matrix inversion, the computations of system functions, and the Cholesky decomposition 210

are needed to be calculated. It is noteworthy that some steps of the proposed Algorithms 2–6 are not executed since they are not required or they have been implemented in the previous operations.

lP

For example, the SLR equations (14) and (15) are only implemented in Algorithm 5 (FB-IPLS), which result the substituted SLR parameters for the backward information filter. Furthermore, the computations of process and measurement functions are not taken into account since they depend on the different functions. The FLOPs of all considered algorithms are summarized in Table 1. FLOPs

urn a

Algorithms

1. The de-correlating IPLF

19 3 33 2 1 3 2 2 2 3 nx + 2nz + 6nx nz + 8nx nz + 2 nx + 4nz + 11nx nz + 6 nx + 3nz +

5 2 2 (J −1)( 73 n3x +n3z +6n2x nz +8nx n2z + 21 2 nx +nz +11nx nz − 6 nx +nz )

2. The de-correlating RTS-IPLS

47 2 13 52 3 2 2 3 2 3 nx +2nz +6nx nz +8nx nz + 2 nx +4nz +11nx nz + 6 nx +3nz +

5 2 2 (J −1)( 73 n3x +n3z +6n2x nz +8nx n2z + 21 2 nx +nz +11nx nz − 6 nx +nz )

3. CGAF [16]

4n4x + 31 6 nx

4. IPLF

Jo

215

5. FB-IPLS

61 3 3 nx

+ 3n3z + 6n2x nz + 10nx n2z +

83 2 2 nx

+ 7n2z + 12nx nz +

+ 6nz − 1

43 3 7 3 2 2 45 2 2 3 nx +nz +10nx nz +12nx nz + 2 nx +nz +10nx nz + 6 nx +nz +(J −

15 2 7 3 3 2 2 2 1)( 10 3 nx +nz +10nx nz +12nx nz + 2 nx +nz +10nx nz + 6 nx +nz )

91 3 25 3 2 2 53 2 2 3 nx +3nz +24nx nz +20nx nz + 2 nx +5nz +10nx nz + 6 nx +4nz + 7 3 3 2 2 15 2 2 (J−1)( 10 3 nx +nz +10nx nz +8nx nz + 2 nx +nz +10nx nz + 6 nx +nz )

16

Journal Pre-proof

76 3 59 2 19 3 2 2 2 3 nx + nz + 10nx nz + 12nx nz + 2 nx + nz + 10nx nz + 6 nx + nz +

6. RTS-IPLS

7 3 3 2 2 15 2 2 (J−1)( 10 3 nx +nz +10nx nz +8nx nz + 2 nx +nz +10nx nz + 6 nx +nz ) 71 2 2 nx

+ n2z + 23nx nz − 21 nx + nz

29n3x + n3z + 10n2x nz + 12nx n2z +

8. GASS [21]

15 2 2 56n3x +2n3z +47n2x nz +23nx n2z + 105 2 nx +4nz +29nx nz + 2 nx +4nz

pro of

7. GASF [21]

Table 1: The FLOPs of all considered algorithms

Remarkably, in most cases, state dimension is not less than measurement dimension, i.e., nx ≥ nz (nz ≥ 1). It is obvious that the number of FLOPs of the filter is smaller than that of its corresponding smoother for any values of nx (nx ≥ 1) and nz since more computing operations are needed in the smoother. Moreover, the number of FLOPs of the proposed algorithm is smaller than that of its compared approach for most of values of nx and nz when J = 1.

re-

220

In order to simplify the discussion, only the de-correlating IPLF and its corresponding compared approach CGAF are considered. The other proposed algorithms and their corresponding compared approaches can also be analysed in the same way and the analogous conclusions can also be obtained.

lP

Ignoring the first-order terms which have little influence on the FLOPs, the FLOPs of the decorrelating IPLF and its compared algorithm CGAF are simplified as follows 43 3 45 n + n3z + 10n2x nz + 12nx n2z + n2x + n2z + 10nx nz 3 x 2 10 3 15 3 2 + (J − 1)( nx + nz + 10nx nz + 12nx n2z + n2x + n2z + 10nx nz ) 3 2 71 2 3 3 2 2 2 CGAF : 29nx + nz + 10nx nz + 12nx nz + nx + nz + 23nx nz 2

urn a

The de−correlating IPLF :

For the case of J ≥ 2, the comparison is discussed by setting J = 2 for convenience. In order to facilitate analysis, we assume that the state has the same dimension as the measurement, i.e., nx = nz (the simulation example belongs to this case). Collecting terms, the FLOPs of IPLF and CGAF are equal to 225

191 3 3 nx

2 + 52n2x and 52n3x + 119 2 nx , respectively. The ratio of FLOPs of IPLF and

FLOPs of CGAF is about 1.2 when the state dimension nx is large, which is acceptable for many

Jo

practical applications. If nx = 1, the ratio of FLOPs of IPLF and FLOPs of CGAF reduces to about 1, which means that the computational cost of IPLF is approximate to that of CGAF. Then we assume the state dimension is twice as large as that of the measurement, i.e., nz = 12 nx . Collecting terms, the FLOPs of IPLF and CGAF are equal to 230

407 3 12 nx

+

81 2 2 nx

and

297 3 8 nx

+

189 2 4 nx ,

respectively. It is obvious that the FLOPs of IPLF is smaller than that of CGAF for any values of 17

Journal Pre-proof

nx (nx ≥ 2) and nz = 12 nx (nz ≥ 1). The ratio of FLOPs of IPLF and FLOPs of CGAF is about 0.91 when the state dimension nx is large, which means that the computational cost of IPLF is less than that of CGAF. When the number of iterations J ≥ 3, the analogous analyses can also be 235

the subsequent corresponding simulation results.

4. Numerical simulation

pro of

made in the same way. It is remarkable that the aforementioned conclusions are consistent with

The efficiencies of the proposed Algorithms 2–6 are shown for two numerical examples in this section. The de-correlating IPLF and RTS-IPLS are compared with the existing method CGAF [16] for nonlinear models possessing simultaneous cross-correlated noises. Furthermore, when the number of iterations J = 1, the de-correlating IPLF and RTS-IPLS reduce to the de-correlating PLF

re-

240

and RTS-PLS, respectively, which are also taken into account for comparison. Notice that GARF [15] is omitted for convenience since the performance of CGAF is superior to that of GARF [16]. For nonlinear models possessing one-step cross-correlated noises, the modified IPLF, FB-IPLS, and

4.1. A model of trigonometric and exponential functions with simultaneous cross-correlated noises In this subsection, a numerical simulation example of trigonometric and exponential functions frequently encountered in tracking problems [5, 14] is given, whose system model is described as

urn a

follows [15, 16]

  1             xk = x2,k  =  x1,k−1 + e−0.05x3,k−1 + 10  + 1 wk−1       1 0.2x1,k−1 (x2,k−1 + x3,k−1 ) x3,k 

x1,k





3 sin2 (5x2,k−1 )

zk = 2x1,k + cos(x2,k x3,k ) + vk ,



(30)

(31)

where the Gaussian white noises wk and vk are correlated simultaneously with covariance Sk and their variances are Qk = 1 and Rk = 4, respectively. The initial state x0 = [−0.7 1 1]T , and prior

Jo

245

lP

RTS-IPLS algorithms are compared with the algorithms GASF and GASS [21].

ˆ 0|0 = [0 0 0]T and P0|0 = I3 , where I3 represents an identity moment estimations are selected as x matrix of dimension 3.

18

Journal Pre-proof

6

1.8 CGAF

5

De-Correlating RTS-IPLS

1.4 1.2

1 0

4

CGAF De-Correlating PLF De-Correlating IPLF De-Correlating RTS-PLS De-Correlating RTS-IPLS

pro of

RMSE of x

RMSE of x

3

De-Correlating RTS-PLS

De-Correlating IPLF

2

1.6

De-Correlating PLF

3 2

20

40

60

80

Time step k

100

(a)

1 0

20

40

60

80

Time step k

100

(b)

Figure 2: Performances of the considered algorithms for time step k with Sk = 0.5 and J = 2. 1.6

5.7

De-Correlating IPLF

1.3

1.1 1

2

3

Number of iterations J

(a)

1600 1400 1200 1000 800 600 1

CGAF

De-Correlating PLF

De-Correlating RTS-PLS

4

De-Correlating RTS-PLS

5.1

De-Correlating IPLF

De-Correlating RTS-IPLS

4.8

2

De-Correlating RTS-IPLS

3

Number of iterations J

4

4.2 1

5

De-Correlating IPLF

urn a

Number of Divergences

1800

De-Correlating PLF

4.5

lP

1.2

3

De-Correlating RTS-IPLS

ARMSE of x

De-Correlating RTS-PLS

1.4

2000

CGAF

5.4

5

(c)

2

3

Number of iterations J

4

5

(b) Average Computational Time (ms)

ARMSE of x

2

1.5

De-Correlating PLF

re-

CGAF

60 CGAF 50 40

De-Correlating PLF

De-Correlating IPLF

De-Correlating RTS-PLS De-Correlating RTS-IPLS

30 20 10 1

2

3

Number of iterations J

4

5

(d)

Jo

Figure 3: Performances of the considered algorithms for different iterations J with Sk = 0.5. (a) ARMSE in x2 . (b) ARMSE in x3 . (c) Number of Divergences. (d) Average computational time.

19

Journal Pre-proof

1.3

De-Correlating RTS-PLS De-Correlating RTS-IPLS

1 0.9 0.1

0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

0.8

0.9

1

(a)

1.2

1 0.1

Number of Divergences

De-Correlating PLF De-Correlating IPLF

0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

5.2

0.8

0.9

1

De-Correlating RTS-PLS De-Correlating RTS-IPLS 0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

(c)

0.8

0.9

1

De-Correlating PLF De-Correlating IPLF

1600 1300

De-Correlating RTS-PLS

re-

4.8

CGAF

1900

1000

700

400 0.1

lP

ARMSE of x3

1.4

2200

CGAF

4 0.1

De-Correlating IPLF De-Correlating RTS-IPLS

(b)

5.6

4.4

De-Correlating PLF

De-Correlating RTS-PLS

1.6

pro of

1.1

CGAF

De-Correlating IPLF

ARMSE of x2

ARMSE of x1

1.2

1.8

De-Correlating PLF

CGAF

De-Correlating RTS-IPLS

0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

0.8

0.9

1

(d)

Figure 4: Performances of the considered algorithms for different correlation parameters Sk with J = 2. (a) ARMSE in x1 . (b) ARMSE in x2 . (c) ARMSE in x3 . (d) Number of Divergences.

urn a

The root MSE (RMSE) and accumulative RMSE (ARMSE) are selected as performance metrics and can be expressed as follows based on the state component xi,k (i = 1, 2, 3) v u Nm u 1 X RMSE(xi,k ) = t (xn − x ˆni,k|k )2 Nm n=1 i,k v u Nm X N u 1 X ARMSE(xi,k ) = t (xni,k − x ˆni,k|k )2 , Nm N n=1

(33)

k=1

where x ˆni,k|k and xni,k are the estimated and true state component respectively in the n-th experiment.

Jo

250

(32)

To let the comparison be fair, Nm = 5000 Monte Carlo runs are executed with simulation steps q N = 100. Divergence is claimed when estimation error (xn3,k − x ˆn3,k|k )2 exceeds 20 at arbitrarily discrete time k ∈ [0, N ].

Figure 2 shows a representative RMSE results of the existing methods and the proposed de20

Journal Pre-proof

255

correlating approaches with Sk = 0.5 and J = 2. The RMSE results of state component x1,k are similar to those of state component x2,k and omitted for convenience. The estimation performances w.r.t different iteration numbers J (J = 1, 2, . . . , 5) and different correlation parameters Sk

pro of

(Sk =0.1,0.2,. . . ,1) are illustrated as Figure 3 and Figure 4, respectively.

From Figures 2–4, we can see that performing iterations within a certain range (including the 260

iterations J and correlation term Sk ) can reduce the errors of the de-correlating IPLF and RTSIPLS. The de-correlating IPLF generally obtain a better performance than the other algorithms after 2–3 iterations with an acceptable average computational time, which can achieve an efficient balance between the estimated precision and calculating cost. Furthermore, it is much more robust than the others to the varying correlation parameters Sk . The iterated algorithms including the

265

de-correlating IPLF and RTS-IPLS exhibit varying degrees of performance loss with the increase

re-

number of iterations when the value of iterations J is large than 2 and the main reason is the numerical error. In other words, when the estimation error is significantly reduced, more iterations have little effect on improving the estimation accuracy and more numerical errors will be introduced. It is remarkable that the performance of the de-correlating PLS (RTS-IPLS) is not always more accurate than that of its corresponding de-correlating PLF (IPLF) approach for some applications

lP

270

due to the cross-correlated noises.

4.2. Univariate nonstationary growth model with one-step cross-correlated noises Because of the highly nonlinearities, the univariate nonstationary growth model [24, 34] is

urn a

frequently used in validating the performances of nonlinear estimation approaches, whose system equations are denoted as

xk+1 = 0.9xk + zk+1 =

10xk + 8 cos(1.2 ∗ k) + wk 1 + x2k

x3k + vk+1 . 20

(34) (35)

ˆ 0|0 = 1, and P0|0 = 4. System noises wk−1 and vk are The initial simulation parameters x0 = 5, x

275

Jo

correlated possessing cross-covariance Sk and their covariances are Q = 1 and R = 4, respectively. The RMSE and ARMSE averaged on all Monte Carlos runs are adopted as performance metrics to assess the estimation properties. To let the comparison be fair, Nm = 5000 Monte Carlo runs are implemented with simulation steps N = 50. Divergence is claimed when estimation error q ˆ nk|k )2 exceeds 30 at random discrete time k ∈ [0, N ]. (xnk − x 21

Journal Pre-proof

2.5 2 1.5 1 0.5 0 0

IPLF GASF GASS FB-IPLS RTS-IPLS

3

RMSE of x

3

RMSE of x

3.5

IPLF GASF GASS FB-IPLS RTS-IPLS

2.5 2 1.5

pro of

3.5

1

0.5

10

20

30

40

Time step k

50

(a)

0 0

10

20

30

40

Time step k

50

(b)

Figure 5: Performances of the considered algorithms for time step k with Sk = 0.5 and different iterations J. (a) RMSE in x with J = 2. (b) RMSE in x with J = 3. GASF

GASS

FB-IPLS

RTS-IPLS

1

0.6 1

2

3

lP

ARMSE of x

1.2

0.8

Number of iterations J

24

4

20 16

50

IPLF GASF GASS FB-IPLS RTS-IPLS

40 30 20

10 0 1

5

2

3

Number of iterations J

4

5

(b)

IPLF GASF GASS FB-IPLS RTS-IPLS

urn a

Average Computational Time (ms)

(a)

60

re-

IPLF

Number of Divergences

1.4

12

8

2

3

Number of iterations J

4

5

(c)

Jo

4 1

Figure 6: Performances of the considered algorithms for different iterations J with Sk = 0.5. (a) ARMSE in x. (b) Number of Divergences. (c) Average computational time.

22

Journal Pre-proof

1.5

120 GASS

FB-IPLS

RTS-IPLS

1.3 1.1 0.9 0.7 0.1

0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

0.8

0.9

1

(a)

100 80 60

IPLF GASF GASS FB-IPLS RTS-IPLS

pro of

GASF

Number of Divergences

ARMSE of x

IPLF

40 20

0 0.1

0.2

0.3

0.4

0.5

0.6

0.7

Correlation parameters Sk

0.8

0.9

1

(b)

Figure 7: Performances of the considered algorithms for different correlation parameters Sk with

re-

J = 2. (a) ARMSE in x. (b) Number of Divergences.

Figure 5 demonstrates two representative RMSE results of all considered algorithms with Sk = 280

0.5 and different iterations. The estimation performances w.r.t different iteration numbers J and different correlation parameters Sk are displayed as Figure 6 and Figure 7, respectively.

lP

Figures 5–7 show that the errors of the proposed iterated methods can be reduced by performing iterations and these approaches can obtain better performances than the compared algorithms after 2–3 iterations with acceptable computational cost. More precisely, except for a reasonable 285

computational complexity, the proposed IPLF (FB-IPLS or RTS-IPLS) method has advantages in estimation accuracy and robustness when compared with the existing approach GASF (GASS). An

urn a

effective balance between estimation accuracy and computational complexity can be obtained by the proposed iterated algorithms. It is worthy of note that the approximated RTS-IPLS algorithm has similar performances as the FB-IPLS approach except for the less computational cost. The reason 290

for this is that the effect of the noise correlation can be diminished by the iteration operations and then these two methods are equivalent with each other approximatively.

5. Conclusion

Jo

In the paper, several novel iterated posterior linearization filtering and smoothing approaches are introduced to improve the estimation performance for nonlinear models possessing cross-correlated 295

noises. In each iteration step of the proposed algorithms, SLRs of the system functions are performed w.r.t current posterior distribution and the multidimensional integrals of these SLRs are 23

Journal Pre-proof

approximated by some efficient sigma-point approaches. Simulation results demonstrate that the presented iterated methods not only have acceptable computational cost but also are more accurate

300

6. Future recommendation

pro of

and robust than the conventional approaches when the posterior PDF is unimodal.

Note that this paper only demonstrates the estimated performance of the presented techniques and their stabilities still need to be further proved, which is very important for the choice in practical applications. Furthermore, similar estimation approaches are able to extended for the continuous-discrete models with cross-correlated noises where the dynamic equations are expressed by stochastic differential equations (SDEs) [35, 36].

re-

305

7. Acknowledgment

The work was supported in part by the National Natural Science Foundation of China (Grant

8. References 310

lP

No. 61971100).

[1] B. Cui, X. Chen, X. Tang, H. Huang, X. Liu, Robust cubature kalman filter for GNSS/INS with missing observations and colored measurement noise, ISA Trans. 72 (2018) 138–146.

urn a

doi:doi.org/10.1016/j.isatra.2017.09.019. [2] M. Xiao, Y. Zhang, Z. Wang, H. Fu, An adaptive three-stage extended kalman filter for nonlinear discrete time system in presence of unknown inputs, ISA Trans. 75 (2018) 101–117. 315

doi:doi.org/10.1016/j.isatra.2018.02.007. [3] Y. Ho, R. Lee, A bayesian approach to problems in stochastic estimation and control, IEEE Trans. Autom. Control 9 (4) (1964) 333–339. doi:10.1109/TAC.1964.1105763.

Jo

[4] B. D. Anderson, J. B. Moore, Optimal filtering, Courier Corporation, Chelmsford, America, 2012. 320

[5] S. Julier, J. Uhlmann, Unscented filtering and nonlinear estimation, Proc. IEEE 92 (3) (2004) 401–422. doi:10.1109/JPROC.2003.823141. 24

Journal Pre-proof

[6] I. Arasaratnam, S. Haykin, Cubature Kalman filters, IEEE Trans. Autom. Control 54 (6) (2009) 1254–1269. doi:10.1109/TAC.2009.2019800.

325

pro of

[7] K. Ito, K. Xiong, Gaussian filters for nonlinear filtering problems, IEEE Trans. Autom. Control 45 (5) (2000) 910–927. doi:10.1109/9.855552.

[8] I. Arasaratnam, S. Haykin, Cubature Kalman smoothers, Automatica 47 (10) (2011) 2245– 2250. doi:10.1016/j.automatica.2011.08.005.

¨ ¨ Unscented Rauch–Tung–Striebel smoother, IEEE Trans. Autom. Control 53 (3) [9] S. SArkk A, (2008) 845–849. doi:10.1109/TAC.2008.919531. 330

[10] H. Lin, S. Sun, Distributed fusion estimator for multisensor multirate systems with correlated

re-

noises, IEEE Trans. Syst., Man, Cybern: Syst. 48 (7) (2018) 1131–1139. doi:10.1109/TSMC. 2016.2645599.

[11] C. Yang, Z. Yang, Z. Deng, Robust weighted state fusion kalman estimators for networked

335

2018.01.014.

lP

systems with mixed uncertainties, Inf. Fusion 45 (2019) 246–265. doi:10.1016/j.inffus.

´ [12] R. Caballero-Aguila, A. Hermoso-Carazo, J. Linares-P´erez, Z. Wang, A new approach to distributed fusion filtering for networked systems with random parameter matrices and correlated

urn a

noises, Inf. Fusion 45 (2019) 324–332. doi:10.1016/j.inffus.2018.02.006. [13] T. Tian, S. Sun, H. Lin, Distributed fusion filter for multi-sensor systems with finite-step 340

correlated noises, Inf. Fusion 46 (2019) 128–140. doi:10.1016/j.inffus.2018.05.002. [14] Y. Bar-Shalom, X. R. Li, T. Kirubarajan, Estimation with applications to tracking and navigation: Theory algorithms and software, John Wiley & Sons, New York, America, 2004. [15] X. Wang, Y. Liang, Q. Pan, F. Yang, A Gaussian approximation recursive filter for nonlinear systems with correlated noises, Automatica 48 (9) (2012) 2290–2297. doi:10.1016/j. automatica.2012.06.035.

Jo

345

[16] Y. Huang, Y. Zhang, N. Li, Z. Shi, Gaussian filter for nonlinear systems with correlated noises at the same epoch, Automatica 60 (2015) 122–126. doi:10.1016/j.automatica.2015.06. 035.

25

Journal Pre-proof

[17] G. Chang, Comment on “A Gaussian approximation recursive filter for nonlinear systems with 350

correlated noises”, Automatica 50 (2) (2014) 655–656. doi:10.1016/j.automatica.2013.12.

pro of

018. [18] X. Wang, Y. Liang, Q. Pan, Z. Wang, General equivalence between two kinds of noisecorrelation filters, Automatica 50 (12) (2014) 3316–3318. doi:10.1016/j.automatica.2014. 10.040. 355

[19] R. G. Brown, P. Y. C. Hwang, Introduction to random signals and applied Kalman filtering: with MATLAB exercises, Wiley, New York, America, 1997.

[20] D. Simon, Optimal state estimation: Kalman, H∞, and nonlinear approaches, Wiley, New

re-

York, America, 2006.

[21] Y. Huang, Y. Zhang, N. Li, Z. Shi, Design of Gaussian approximate filter and smoother for 360

nonlinear systems with correlated noises at one epoch apart, Circuits Syst. Signal Process. 35 (11) (2016) 3981–4008. doi:10.1007/s00034-016-0256-0.

lP

[22] R. Zhan, J. Wan, Iterated unscented Kalman filter for passive target tracking, IEEE Trans. Aerosp. Electron. Syst. 43 (3) (2007) 1155–1163. doi:10.1109/TAES.2007.4383605. ´ F. Garc´ıa-Fern´andez, L. Svensson, M. R. Morelande, S. S¨ [23] A. arkk¨ a, Posterior linearization filter: 365

Principles and implementation using sigma points, IEEE Trans. Signal Process. 63 (20) (2015)

urn a

5561–5573. doi:10.1109/TSP.2015.2454485. ´ F. Garc´ıa-Fern´andez, L. Svensson, S. S¨ [24] A. arkk¨ a, Iterated posterior linearization smoother, IEEE Trans. Autom. Control 62 (4) (2017) 2056–2063. doi:10.1109/TAC.2016.2592681. [25] B. M. Bell, F. W. Cathey, The iterated Kalman filter update as a Gauss-Newton method, 370

IEEE Trans. Autom. Control 38 (2) (1993) 294–297. doi:10.1109/9.250476. [26] B. M. Bell, The iterated Kalman smoother as a Gauss-Newton method, SIAM J. Optim. 4 (3)

Jo

(1993) 626–636. doi:10.1137/0804035. [27] X. Hu, M. Bao, X.-P. Zhang, L. Guan, Y.-H. Hu, Generalized iterated Kalman filter and its performance evaluation, IEEE Trans. Signal Process. 63 (12) (2015) 3204–3217. doi: 375

10.1109/TSP.2015.2423266. 26

Journal Pre-proof

[28] A. Gelb, Applied optimal estimation, MIT Press, Cambridge, America, 1974. [29] I. Arasaratnam, S. Haykin, R. J. Elliott, Discrete-time nonlinear filtering algorithms using

pro of

Gauss-Hermite quadrature, IEEE Trans. Signal Process. 95 (5) (2007) 953–977. doi:10.1109/ JPROC.2007.894705. 380

[30] H. E. Rauch, F. Tung, C. T. Striebel, Maximum likelihood estimates of linear dynamic systems, AIAA J. 3 (8) (1965) 1445–1450. doi:doi.org/10.2514/3.3166.

[31] D. C. Frmer, J. E. Potter, The optimum linear smoother as a combination of two optimum linear filters, IEEE Trans. Autom. Control 14 (4) (1969) 387–390. doi:10.1109/TAC.1969. 1099196.

[32] F. Lewis, Optimal estimation with an introduction to stochastic control, John Wiley & Sons, New York, America, 1986.

re-

385

[33] R. Hunger, Floating point operations in matrix-vector calculus, Tech. Rep., Munich University

lP

of Technology, 2007.

[34] Y. Wu, D. Hu, M. Wu, X. Hu, A numerical-integration perspective on Gaussian filters, IEEE 390

Trans. Signal Process. 54 (8) (2006) 2910–2921. doi:10.1109/TSP.2006.875389. [35] Y. Wang, H. Zhang, X. Mao, Y. Li, Accurate smoothing methods for state estimation of

urn a

continuous-discrete nonlinear dynamic systems, IEEE Trans. Autom. Control 64 (10) (2019) 4284–4291. doi:10.1109/TAC.2019.2893876. [36] Y. Wang, H. Zhang, Accurate smoothing for continuous-discrete nonlinear systems with nongaussian noise, IEEE Signal Process. Lett. 26 (3) (2019) 465–469. doi:10.1109/LSP.2018. 2890313.

Jo

395

27

*Conflict of Interest

Journal Pre-proof

Conflict of interest statement We declare that we have no financial and personal relationships with other people or organizations that can inappropriately influence our work,

pro of

there is no professional or other personal interest of any nature or kind in any product, service and/or company that could be construed as

influencing the position presented in, or the review of, the manuscript

Jo

urn a

lP

re-

entitled.