Design of Sigma-Point Kalman Filter with Recursive Updated Measurement

Abstract

In this study, the authors focus on improving measurement update of existing nonlinear Kalman approximation filter and propose a new sigma-point Kalman filter with recursive measurement update. Statistical linearization technique based on sigma transformation is utilized in the proposed filter to linearize the nonlinear measurement function, and linear measurement update is applied gradually and repeatedly based on the statistically linearized measurement equation. The total measurement update of the proposed filter is nonlinear, and the proposed filter can extract state information from nonlinear measurement better than existing nonlinear filters. Simulation results show that the proposed method has higher estimation accuracy than existing methods.

1 Introduction

State estimation and filtering have been widely used in abundant practical applications such as navigation, tracking, positioning, signal processing and control [23]. Nonlinear filtering has been gaining more attention because there are always inherent nonlinearities in many practical problems. Generally, in the sense of minimum mean square error (MMSE), there is a lack of optimal estimator for nonlinear dynamic state estimation problem because the closed form solution of its posterior probability density function (PDF) is unavailable, and linear MMSE (LMMSE) estimators are usually obtained based on Gaussian approximation to such PDFs in most applications [2, 9, 22]. However, LMMSE estimator may fail in some applications with large prior uncertainty and high measurement accuracy [5, 10, 18], which motivates the research on iterated Kalman-type filter (IKTF) [13] and progressive Gaussian filtering (PGF) [7, 8, 11, 14].

The first proposed IKTF is iterated extended Kalman filter (IEKF). In the IEKF, the first-order linearization of measurement function is implemented repeatedly at the most recent estimate, which avoids possible filtering divergence due to the first-order Taylor series truncation of measurement function [13]. Bell and Cathey interpret the IEKF as maximum-likelihood estimator based on Gauss–Newton method, which justifies from another aspect that the IEKF’s benefits over EKF [3]. However, linearization based on the first-order Taylor series truncation is used in the IEKF, which results in limited filtering accuracy. Besides, IEKF needs cumbersome evaluation of Jacobian matrix, which results in inconvenient implementation. In order to solve these problems, an iterated sigma-point Kalman filter (ISPKF) is proposed based on statistical linearization and Gauss–Newton iteration method [12, 15]. Different algorithms have also been proposed to further improve the performance of IKTF, including iterated unscented Kalman filter (IUKF) with step length-varying strategy [19], iterated divided difference filter (IDDF) [16], marginalized IUKF (MIUKF) [4] and iterated posterior linearization filter (IPLF) [6]. However, Kalman gains of IUKF, IDDF, MIUKF and IPLF are almost equal to zero after the first iteration, which implies that their most iterations are redundant and their total measurement updates are almost linear.

One way to avoid such problems in state estimation is the use of PGF, which gradually includes the measurement information instead of using all the information in one step [8]. In [8] and [14], PGFs based on deterministic Dirac mixture approximation method are proposed, which have higher estimation accuracy than standard Gaussian-approximated filter. On the other hand, by means of the idea of progressive update, an EKF with recursive measurement update is proposed, which utilizes linear measurement update gradually, i.e., measurement information is extracted gradually [18]. However, as discussed in [18], similar to classical EKF algorithm, it suffers the following drawbacks. (i) It needs to assume that Jacobian matrixes of nonlinear process and measurement functions are available. (ii) Its estimated statistics may be inconsistent with true distribution of error when its Kalman gain varies significantly across all possible estimates. (iii) It fails when Jacobian matrixes of measurement functions are zeros. Furthermore, it is based on the EKF method, and its accuracy can be further improved.

In this paper, a new sigma-point Kalman filter (SPKF) with recursive measurement update is developed. Statistical linearization technique based on sigma transformation is utilized in the proposed method to linearize the nonlinear measurement function, and linear measurement update is applied gradually and repeatedly based on the statistically linearized measurement equation. The proposed method does not need Jacobian matrixes, and its Kalman gain is calculated based on state prior distribution represented by deterministically choosing sigma points around distribution instead of state estimate. The superior performance of the proposed method as compared with existing methods is illustrated in numerical examples concerning univariate nonstationary growth model and bearing-only tracking.

2 SPKF with Recursive Measurement Update

Consider the following discrete-time nonlinear stochastic dynamic system as shown by the state-space model [21]

$$\begin{aligned} \varvec{x}_k= & {} \varvec{f}_{k-1}(\varvec{x}_{k-1})+\varvec{w}_{k-1} \qquad {\text {(process equation)}} \end{aligned}$$

(1)

$$\begin{aligned} \varvec{z}_k= & {} \varvec{h}_{k}(\varvec{x}_{k})+\varvec{v}_{k} \qquad \quad {\text {(measurement equation)}} \end{aligned}$$

(2)

where k is the discrete-time index, ${\varvec{x}_k\in {\mathbb {R}}^n }$ is the state vector, ${\varvec{z}_k\in {\mathbb {R}}^m }$ is the measurement vector, ${\varvec{w}_k\in {\mathbb {R}}^n }$ and ${\varvec{v}_k\in {\mathbb {R}}^m }$ are uncorrelated zero-mean Gaussian white noise vectors satisfying $E[\varvec{w}_{k}\varvec{w}_{l}^{{T}}]=\varvec{Q}_k \delta _{kl}$ and $E[\varvec{v}_{k}\varvec{v}_{l}^{{T}}]=\varvec{R}_k \delta _{kl}$, respectively, where $\delta _{kl}$ is the Kronecker delta function, the initial state $\varvec{x}_{0}$ is a Gaussian random vector with mean $\hat{\varvec{x}}_{0|0}$ and covariance matrix $\varvec{P}_{0|0}$, and it is uncorrelated with $\varvec{w}_{k}$ and $\varvec{v}_{k}$.

2.1 Kalman Filter with Recursive Measurement Update

In this subsection, KF with recursive measurement update for linear measurement case is reviewed. For linear measurement case, we can reformulate measurement equation in (2) as follows.

$$\begin{aligned} \varvec{z}_k=\varvec{H}_{k}\varvec{x}_{k}+\varvec{v}_{k} \end{aligned}$$

(3)

Based on the linear measurement function formulated in (3), the recursive measurement update equations of KF can be formulated as follows [18].

$$\begin{aligned} \hat{\varvec{x}}_{k|k}^{(0)}=\hat{\varvec{x}}_{k|k-1} \quad \varvec{P}_{k|k}^{(0)}=\varvec{P}_{k|k-1} \quad \varvec{P}_{xv, k|k}^{(0)}=\varvec{0}_{n\times {m}} \end{aligned}$$

(4)

for $i=1:N$

$$\begin{aligned} r_{k}^{(i)}= & {} 1/(N+1-i) \end{aligned}$$

(5)

$$\begin{aligned} \varvec{P}_{zz, k|k}^{(i)}= & {} \varvec{H}_{k}\varvec{P}_{k|k}^{(i-1)}(\varvec{H}_{k})^{T}+\varvec{H}_{k}\varvec{P}_{xv, k|k}^{(i-1)}+\left( \varvec{P}_{xv, k|k}^{(i-1)}\right) ^{T} (\varvec{H}_{k})^{T}+\varvec{R}_{k} \end{aligned}$$

(6)

$$\begin{aligned} \varvec{K}_{k}^{(i)}= & {} r_{k}^{(i)}\left[ \varvec{P}_{k|k}^{(i-1)}(\varvec{H}_{k})^{T}+\varvec{P}_{xv, k|k}^{(i-1)}\right] \left( \varvec{P}_{zz, k|k}^{(i)}\right) ^{-1} \end{aligned}$$

(7)

$$\begin{aligned} \hat{\varvec{x}}_{k|k}^{(i)}= & {} \hat{\varvec{x}}_{k|k}^{(i-1)}+\varvec{K}_{k}^{(i)}\left[ \varvec{z}_k-\hat{\varvec{z}}_{k|k}^{(i)}\right] =\hat{\varvec{x}}_{k|k}^{(i-1)}+\varvec{K}_{k}^{(i)}\left[ \varvec{z}_k-\varvec{H}_{k}\hat{\varvec{x}}_{k|k}^{(i-1)}\right] \end{aligned}$$

(8)

$$\begin{aligned} \varvec{P}_{k|k}^{(i)}= & {} \left[ \varvec{I}_{n\times {n}}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}\right] \varvec{P}_{k|k}^{(i-1)}\left[ \varvec{I}_{n\times {n}}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}\right] ^{T}+\varvec{K}_{k}^{(i)}\varvec{R}_{k}\left( \varvec{K}_{k}^{(i)}\right) ^{T}\nonumber \\&-\left[ \varvec{I}_{n\times {n}}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}\right] \varvec{P}_{xv, k|k}^{(i-1)}\left( \varvec{K}_{k}^{(i)}\right) ^{T}\nonumber \\&- \varvec{K}_{k}^{(i)}\left( \varvec{P}_{xv, k|k}^{(i-1)}\right) ^{T}\left[ \varvec{I}_{n\times {n}}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}\right] ^{T} \end{aligned}$$

(9)

$$\begin{aligned} \varvec{P}_{xv,k|k}^{(i)}= & {} \left[ \varvec{I}_{n\times {n}}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}\right] \varvec{P}_{xv, k|k}^{(i-1)}-\varvec{K}_{k}^{(i)}\varvec{R}_{k} \end{aligned}$$

(10)

end for

where $\hat{\varvec{x}}_{k|k-1}$ is the predicted state at time k, $\varvec{P}_{k|k-1}$ is the predicted error covariance matrix at time k, N denotes the number of recursion steps, $\varvec{I}_{n\times {n}}$ denotes the $n\times {n}$ identity matrix, $\varvec{0}_{n\times {m}}$ denotes the $n\times {m}$ zero matrix, (i) denotes the ith recursion, $r_{k}^{(i)}$ denotes the recursion coefficient at recursion i, $\varvec{P}_{zz, k|k}^{(i)}$ denotes the innovation covariance matrix at recursion i, $\varvec{K}_{k}^{(i)}$ denotes the Kalman gain at recursion i, $\hat{\varvec{x}}_{k|k}^{(i)}$ and $\varvec{P}_{k|k}^{(i)}$ denote the state estimation and corresponding estimation error covariance matrix at recursion i, respectively, $\hat{\varvec{z}}_{k|k}^{(i)}$ denotes the measurement estimation at recursion i, and $\varvec{P}_{xv,k|k}^{(i)}$ denotes the cross-covariance matrix of state and measurement noise at recursion i.

After recursion N, the filtering estimation $\hat{\varvec{x}}_{k|k}$ and corresponding estimation error covariance matrix $\varvec{P}_{k|k}$ at time k can be computed as

$$\begin{aligned} \hat{\varvec{x}}_{k|k}=\hat{\varvec{x}}_{k|k}^{(N)} \qquad \varvec{P}_{k|k}=\varvec{P}_{k|k}^{(N)} \end{aligned}$$

(11)

The KF with recursive measurement update formulated above is identical to standard KF in linear measurement case, which implies that these recursions are redundant for linear measurement. The idea of such recursive measurement update approach is superior to nonlinear measurement, but it cannot be straightforwardly used in nonlinear measurement case. Next, a new SPKF with recursive measurement update will be developed.

2.2 SPKF with Recursive Measurement Update

The derivation of existing KF with recursive measurement update is based on the linear measurement equation. The usual SPKF cannot be straightforwardly embedded into the recursive measurement update equations of KF because its measurement update equations are not explicit functions of the linear measurement matrix $\varvec{H}_{k}$. Considering that a statistical linearization can be deemed as a sigma-point transform, a statistical linearization approach will be utilized to develop a new SPKF with recursive measurement update. The statistical linearization takes into account the probabilistic spread of the prior random vector when linearizing nonlinear function, which results in more accurate linearized function in statistical sense than the first-order truncated Taylor series expansion of nonlinear function around prior mean [1, 6].

Define an auxiliary vector $\varvec{u}_{k}=\varvec{h}_{k}(\varvec{x}_{k})$. Similar to the idea of progressive Gaussian filtering, the intermediate state posterior PDFs are all approximated as Gaussian [14], i.e., $p^{i-1}(\varvec{x}_{k}|\varvec{Z}_{k})=N(\varvec{x}_{k};\hat{\varvec{x}}_{k|k}^{(i-1)}, \varvec{P}_{k|k}^{(i-1)})$, where $\varvec{Z}_{k}=\{\varvec{z}_{j}\}_{j=1}^{k}$, $p^{i-1}(\varvec{x}_{k}|\varvec{Z}_{k})$ denotes the posterior PDF at recursion $i-1$, state estimate $\hat{\varvec{x}}_{k|k}^{(i-1)}$ and corresponding estimation error covariance matrix $\varvec{P}_{k|k}^{(i-1)}$ at recursion $i-1$ denote the first two moments of $p^{i-1}(\varvec{x}_{k}|\varvec{Z}_{k})$, respectively. The statistical linearization of nonlinear measurement function $\varvec{h}_{k}(\varvec{x}_{k})$ with respect to $p^{i-1}(\varvec{x}_{k}|\varvec{Z}_{k})$ can be written as [1]

$$\begin{aligned} \varvec{h}_{k}(\varvec{x}_{k})=\varvec{H}_{k}^{(i-1)}\varvec{x}_{k}+\varvec{c}_{k}^{(i-1)}+\varvec{e}_{k}^{(i-1)} \end{aligned}$$

(12)

where $\varvec{H}_{k}^{(i-1)}$ denotes the sensitivity matrix of statistical linearization at recursion $i-1$

$$\begin{aligned} \varvec{H}_{k}^{(i-1)}= \left( \varvec{P}_{xu,k|k}^{(i-1)}\right) ^{T} \left( \varvec{P}_{k|k}^{(i-1)}\right) ^{-1} \end{aligned}$$

(13)

and $\varvec{c}_{k}^{(i-1)}$ denotes the constant vector of statistical linearization at recursion $i-1$,

$$\begin{aligned} \varvec{c}_{k}^{(i-1)}=\bar{\varvec{u}}_{k}^{(i-1)}-\varvec{H}_{k}^{(i-1)}\hat{\varvec{x}}_{k|k}^{(i-1)} \end{aligned}$$

(14)

and $\varvec{e}_{k}^{(i-1)}$ denotes the deviation of statistical linearization at recursion $i-1$, which can be deemed as random white noise and is uncorrelated with state $\varvec{x}_{k}$ and measurement noise $\varvec{v}_{k}$, and its mean and covariance matrix can be formulated as follows [1]

$$\begin{aligned} \bar{\varvec{e}}_{k}^{(i-1)}= & {} \varvec{0}_{m\times {1}} \end{aligned}$$

(15)

$$\begin{aligned} \varvec{P}_{ee,k|k}^{(i-1)}= & {} \varvec{P}_{uu,k|k}^{(i-1)}- \varvec{H}_{k}^{(i-1)}\varvec{P}_{k|k}^{(i-1)}\left( \varvec{H}_{k}^{(i-1)}\right) ^{T} \end{aligned}$$

(16)

where $\bar{\varvec{u}}_{k}^{(i-1)}$ and $\varvec{P}_{uu,k|k}^{(i-1)}$ denote the mean and covariance matrix of auxiliary vector $\varvec{u}_{k}$ at recursion $i-1$, respectively, $\varvec{P}_{xu,k|k}^{(i-1)}$ denotes cross-covariance matrix of state and auxiliary vector at recursion $i-1$, and they can be approximately computed as

$$\begin{aligned} \bar{\varvec{u}}_{k}^{(i-1)}= & {} {\mathrm {E}}\left[ \varvec{h}_{k}\left( \varvec{x}_{k}\right) \right] = \int _{{\mathbb {R}}^{n}}\varvec{h}_{k}\left( \varvec{x}_{k}\right) N\left( \varvec{x}_{k}; \hat{\varvec{x}}_{k|k}^{(i-1)}, \varvec{P}_{k|k}^{(i-1)}\right) d\varvec{x}_{k} \end{aligned}$$

(17)

$$\begin{aligned} \varvec{P}_{xu,k|k}^{(i-1)}= & {} {\mathrm {E}}\left[ \left( \varvec{x}_{k}- \hat{\varvec{x}}_{k|k}^{(i-1)}\right) \left( \varvec{h}_{k}\left( \varvec{x}_{k}\right) - \bar{\varvec{u}}_{k}^{(i-1)}\right) ^{T}\right] =\int _{{\mathbb {R}}^{n}} \varvec{x}_{k} \varvec{h}_{k}^{T}\left( \varvec{x}_{k}\right) N\nonumber \\&\quad \left( \varvec{x}_{k}; \hat{\varvec{x}}_{k|k}^{(i-1)}, \varvec{P}_{k|k}^{(i-1)}\right) d\varvec{x}_{k}- \hat{\varvec{x}}_{k|k}^{(i-1)}\left( \bar{\varvec{u}}_{k}^{(i-1)}\right) ^{T} \end{aligned}$$

(18)

$$\begin{aligned} \varvec{P}_{uu,k|k}^{(i-1)}= & {} {\mathrm {E}} \left[ \left( \varvec{h}_{k}\left( \varvec{x}_{k}\right) -\bar{\varvec{u}}_{k}^{(i-1)}\right) \left( \varvec{h}_{k}\left( \varvec{x}_{k}\right) -\bar{\varvec{u}}_{k}^{(i-1)}\right) ^{T}\right] = \int _{{\mathbb {R}}^{n}} \varvec{h}_{k}\left( \varvec{x}_{k}\right) \varvec{h}_{k}^{T} \left( \varvec{x}_{k}\right) N\nonumber \\&\quad (\varvec{x}_{k};\hat{\varvec{x}}_{k|k}^{(i-1)}, \varvec{P}_{k|k}^{(i-1)})d\varvec{x}_{k}- \bar{\varvec{u}}_{k}^{(i-1)}\left( \bar{\varvec{u}}_{k}^{(i-1)}\right) ^{T} \end{aligned}$$

(19)

Based on the statistical linearization of $\varvec{h}_{k}(\cdot )$ at recursion $i-1$ in (12)–(19), the nonlinear measurement equation in (2) can be rewritten as

$$\begin{aligned} \varvec{z}_k=\varvec{H}_{k}^{(i-1)}\varvec{x}_{k}+\varvec{c}_{k}^{(i-1)}+\varvec{e}_{k}^{(i-1)}+\varvec{v}_{k}=\varvec{H}_{k}^{(i-1)}\varvec{x}_{k}+\check{\varvec{v}}_{k} \end{aligned}$$

(20)

where $\check{\varvec{v}}_{k}$ can be deemed as pseudo-measurement noise because it consists of real measurement noise $\varvec{v}_{k}$, constant vector $\varvec{c}_{k}^{(i-1)}$ and random deviation $\varvec{e}_{k}^{(i-1)}$ of statistical linearization, i.e.,

$$\begin{aligned} \check{\varvec{v}}_{k}=\varvec{v}_{k}+\varvec{c}_{k}^{(i-1)}+\varvec{e}_{k}^{(i-1)} \end{aligned}$$

(21)

It can be seen from (3) and (20) that pseudo-measurement noise $\check{\varvec{v}}_{k}$ is used instead of real measurement noise $\varvec{v}_{k}$ to develop SPKF with recursive measurement update. Therefore, the statistics of pseudo-measurement noise $\check{\varvec{v}}_{k}$ need to be firstly computed as follows.

Considering that $\varvec{v}_{k}$ has zero-mean and using (14)–(15), the mean of pseudo-measurement noise $\check{\varvec{v}}_{k}$ can be computed as

$$\begin{aligned} \bar{\check{\varvec{v}}}_{k}^{(i-1)}=\varvec{c}_{k}^{(i-1)}=\bar{\varvec{u}}_{k}^{(i-1)}-\varvec{H}_{k}^{(i-1)}\hat{\varvec{x}}_{k|k}^{(i-1)} \end{aligned}$$

(22)

Considering that $\varvec{e}_{k}^{(i-1)}$ is uncorrelated with state $\varvec{x}_{k}$ and real measurement noise $\varvec{v}_{k}$ and using (16) and (21), then the cross-covariance matrix of state and pseudo-measurement noise $\varvec{P}_{x\check{v}, k|k}^{(i-1)}$ and the covariance matrix of pseudo-measurement noise $\varvec{P}_{\check{v}\check{v}, k|k}^{(i-1)}$ at recursion $i-1$ can be computed as follows.

$$\begin{aligned} \varvec{P}_{x\check{v}, k|k}^{(i-1)}= & {} \varvec{P}_{xv, k|k}^{(i-1)} \end{aligned}$$

(23)

$$\begin{aligned} \varvec{P}_{\check{v}\check{v}, k|k}^{(i-1)}= & {} \varvec{R}_{k}+\varvec{P}_{ee,k|k}^{(i-1)}=\varvec{R}_{k}+\varvec{P}_{uu,k|k}^{(i-1)}-\varvec{H}_{k}^{(i-1)}\varvec{P}_{k|k}^{(i-1)} \left( \varvec{H}_{k}^{(i-1)}\right) ^{T} \end{aligned}$$

(24)

According to the definition of $\varvec{P}_{zz, k|k}^{(i)}$ and using (20), we can obtain

$$\begin{aligned} \varvec{P}_{zz, k|k}^{(i)}= & {} \varvec{H}_{k}^{(i-1)}\varvec{P}_{k|k}^{(i-1)} \left( \varvec{H}_{k}^{(i-1)}\right) ^{T}+\varvec{H}_{k}^{(i-1)} \varvec{P}_{x\check{v}, k|k}^{(i-1)}+ \left( \varvec{P}_{x\check{v}, k|k}^{(i-1)}\right) ^{T}\left( \varvec{H}_{k}^{(i-1)}\right) ^{T}\nonumber \\&+\varvec{P}_{\check{v}\check{v}, k|k}^{(i-1)} \end{aligned}$$

(25)

Using (13) and (23)–(24) in (25), we can obtain

$$\begin{aligned} \varvec{P}_{zz, k|k}^{(i)}= & {} \varvec{P}_{uu,k|k}^{(i-1)}+\varvec{R}_{k} +\left( \varvec{P}_{xu,k|k}^{(i-1)}\right) ^{T}\left( \varvec{P}_{k|k}^{(i-1)}\right) ^{-1}\varvec{P}_{xv, k|k}^{(i-1)}\nonumber \\&+\left( \varvec{P}_{xv, k|k}^{(i-1)}\right) ^{T}\left( \varvec{P}_{k|k}^{(i-1)}\right) ^{-1}\varvec{P}_{xu,k|k}^{(i-1)} \end{aligned}$$

(26)

According to the definition of $\varvec{P}_{xz, k|k}^{(i)}$ and using (20), we can obtain

$$\begin{aligned} \varvec{P}_{xz, k|k}^{(i)}=\varvec{P}_{k|k}^{(i-1)}(\varvec{H}_{k})^{T}+\varvec{P}_{x\check{v}, k|k}^{(i-1)} \end{aligned}$$

(27)

Substituting (13) and (23) into (27), we can obtain

$$\begin{aligned} \varvec{P}_{xz, k|k}^{(i)}=\varvec{P}_{xu,k|k}^{(i-1)}+\varvec{P}_{xv, k|k}^{(i-1)} \end{aligned}$$

(28)

Similar to (7), by using (28), the Kalman gain of SPKF with recursive measurement update can be calculated as follows.

$$\begin{aligned} \varvec{K}_{k}^{(i)}=r_{k}^{(i)}\varvec{P}_{xz, k|k}^{(i)}\left( \varvec{P}_{zz, k|k}^{(i)}\right) ^{-1} =r_{k}^{(i)}\left[ \varvec{P}_{xu,k|k}^{(i-1)}+\varvec{P}_{xv, k|k}^{(i-1)}\right] \left( \varvec{P}_{zz, k|k}^{(i)}\right) ^{-1} \end{aligned}$$

(29)

It is clear from (8) that $\hat{\varvec{z}}_{k|k}^{(i)}=\varvec{H}_{k}\hat{\varvec{x}}_{k|k}^{(i-1)}$ for linear measurement equation. However, this equation does not hold any more for nonlinear measurement equation because the statistical linearization error needs to be included in $\hat{\varvec{z}}_{k|k}^{(i)}$, i.e.,

$$\begin{aligned} \hat{\varvec{z}}_{k|k}^{(i)}=\varvec{H}_{k}^{(i-1)}\hat{\varvec{x}}_{k|k}^{(i-1)}+\bar{\check{\varvec{v}}}_{k}^{(i-1)} \end{aligned}$$

(30)

Using (22), (30) can be rewritten as

$$\begin{aligned} \hat{\varvec{z}}_{k|k}^{(i)}=\bar{\varvec{u}}_{k}^{(i-1)} \end{aligned}$$

(31)

Substituting (31) into (8), the recursive state update equation can be formulated as

$$\begin{aligned} \hat{\varvec{x}}_{k|k}^{(i)}=\hat{\varvec{x}}_{k|k}^{(i-1)}+\varvec{K}_{k}^{(i)}\left[ \varvec{z}_k-\bar{\varvec{u}}_{k}^{(i-1)}\right] \end{aligned}$$

(32)

By using (6)–(7), $\varvec{P}_{k|k}^{(i)}$ in (9) can be rewritten as

$$\begin{aligned} \varvec{P}_{k|k}^{(i)}=\varvec{P}_{k|k}^{(i-1)}+\left( 1-2/{r_{k}^{(i)}}\right) \varvec{K}_{k}^{(i)}\varvec{P}_{zz, k|k}^{(i)}\left( \varvec{K}_{k}^{(i)}\right) ^{T} \end{aligned}$$

(33)

In the following, $\varvec{P}_{xv,k|k}^{(i)}$ will be computed. Firstly, the state estimation error $\tilde{\varvec{x}}_{k|k}^{(i)}$ at recursion $i-1$ can be formulated as follows

$$\begin{aligned} \tilde{\varvec{x}}_{k|k}^{(i)}=\varvec{x}_{k}-\hat{\varvec{x}}_{k|k}^{(i)}= \tilde{\varvec{x}}_{k|k}^{(i-1)}-\varvec{K}_{k}^{(i)} \left( \varvec{z}_k-\bar{\varvec{u}}_{k}^{(i-1)}\right) \end{aligned}$$

(34)

According to the definition of $\varvec{P}_{xv,k|k}^{(i)}$ and using (20), we have

$$\begin{aligned} \varvec{P}_{xv,k|k}^{(i)}=E\left[ \tilde{\varvec{x}}_{k|k}^{(i)} \varvec{v}_{k}^{T}\right]= & {} E\left[ \left( \tilde{\varvec{x}}_{k|k}^{(i-1)}- \varvec{K}_{k}^{(i)}\left( \varvec{z}_k-\bar{\varvec{u}}_{k}^{(i-1)}\right) \right) \varvec{v}_{k}^{T}\right] \nonumber \\= & {} \varvec{P}_{xv,k|k}^{(i-1)}-\varvec{K}_{k}^{(i)}E \left[ \varvec{z}_k\varvec{v}_{k}^{T}\right] \nonumber \\= & {} \varvec{P}_{xv,k|k}^{(i-1)}- \varvec{K}_{k}^{(i)}\varvec{H}_{k}^{(i-1)}E \left[ \varvec{x}_{k}\varvec{v}_{k}^{T}\right] -\varvec{K}_{k}^{(i)}E \left[ \check{\varvec{v}}_{k}\varvec{v}_{k}^{T}\right] \nonumber \\= & {} \varvec{P}_{xv,k|k}^{(i-1)}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}^{(i-1)}E \left[ \tilde{\varvec{x}}_{k|k}^{(i-1)}\varvec{v}_{k}^{T}\right] - \varvec{K}_{k}^{(i)}E\left[ \tilde{\check{\varvec{v}}}_{k}\varvec{v}_{k}^{T}\right] \nonumber \\= & {} \varvec{P}_{xv,k|k}^{(i-1)}-\varvec{K}_{k}^{(i)}\varvec{H}_{k}^{(i-1)} \varvec{P}_{xv,k|k}^{(i-1)}-\varvec{K}_{k}^{(i)}E \left[ \tilde{\check{\varvec{v}}}_{k}\varvec{v}_{k}^{T}\right] \end{aligned}$$

(35)

Considering that $\varvec{e}_{k}^{(i-1)}$ is uncorrelated with real measurement noise $\varvec{v}_{k}$, we can obtain

$$\begin{aligned} E\left[ \tilde{\check{\varvec{v}}}_{k}\varvec{v}_{k}^{T} \right] =E\left[ \varvec{v}_{k}\varvec{v}_{k}^{T}\right] =\varvec{R}_{k} \end{aligned}$$

(36)

Substituting (36) into (35) and using (13), we can obtain

$$\begin{aligned} \varvec{P}_{xv,k|k}^{(i)}=\varvec{P}_{xv,k|k}^{(i-1)}-\varvec{K}_{k}^{(i)} \left[ \varvec{R}_{k}+\left( \varvec{P}_{xu,k|k}^{(i-1)}\right) ^{T}\left( \varvec{P}_{k|k}^{(i-1)}\right) ^{-1} \varvec{P}_{xv, k|k}^{(i-1)}\right] \end{aligned}$$

(37)

Generally, in the framework of SPKF or Gaussian-approximated filter, the predicted state $\hat{\varvec{x}}_{k|k-1}$ and predicted error covariance $\varvec{P}_{k|k-1}$ at time k can be approximately computed as

$$\begin{aligned} \hat{\varvec{x}}_{k|k-1}= & {} \int _{{\mathbb {R}}^{n}}\varvec{f}_{k-1} \left( \varvec{x}_{k-1}\right) N\left( \varvec{x}_{k-1};\hat{\varvec{x}}_{k-1|k-1}, \varvec{P}_{k-1|k-1}\right) d\varvec{x}_{k-1} \end{aligned}$$

(38)

$$\begin{aligned} \varvec{P}_{k|k-1}= & {} \int _{{\mathbb {R}}^{n}}\varvec{f}_{k-1} \left( \varvec{x}_{k-1}\right) \varvec{f}_{k-1}^{T} \left( \varvec{x}_{k-1}\right) N \left( \varvec{x}_{k-1};\hat{\varvec{x}}_{k-1|k-1}, \varvec{P}_{k-1|k-1} \right) d\varvec{x}_{k-1}\nonumber \\&-\hat{\varvec{x}}_{k|k-1}\hat{\varvec{x}}_{k|k-1}^{T}+\varvec{Q}_{k-1} \end{aligned}$$

(39)

where $\hat{\varvec{x}}_{k-1|k-1}$ and $\varvec{P}_{k-1|k-1}$ are given in (11).

The proposed SPKF with recursive measurement update operates by combining the analytical computations in (26), (29), (32)–(33) and (37) with Gaussian-weighted integrals in (17)–(19) and (38)–(39). Different SPKFs with recursive measurement update can be obtained when different numerical techniques or sigma transformation methods are used to compute Gaussian-weighted integrals or mathematical expectations in (17)–(19) and (38)–(39). The algorithm for SPKF with recursive measurement update is summarized in Table 1.

Table 1 SPKF with recursive measurement update

Firstly, the proposed SPKF is different from the classic SPKF in the measurement update, though they have same formulas in the time update. Secondly, we can see from Table 1 that the proposed SPKF with recursive measurement update is identical to the classic SPKF when $N=1$, which will be confirmed as follows. If $N=1$, we can obtain following equations from Table 1

$$\begin{aligned} \varvec{P}_{zz, k|k}^{(1)}= & {} \varvec{P}_{uu,k|k}^{(0)}+\varvec{R}_{k} \end{aligned}$$

(40)

$$\begin{aligned} \varvec{K}_{k}^{(1)}= & {} \varvec{P}_{xu,k|k}^{(0)}\left( \varvec{P}_{zz, k|k}^{(1)}\right) ^{-1} \end{aligned}$$

(41)

$$\begin{aligned} \hat{\varvec{x}}_{k|k}^{(1)}= & {} \hat{\varvec{x}}_{k|k-1}+\varvec{K}_{k}^{(1)}\left[ \varvec{z}_k-\bar{\varvec{u}}_{k}^{(0)}\right] \end{aligned}$$

(42)

$$\begin{aligned} \varvec{P}_{k|k}^{(1)}= & {} \varvec{P}_{k|k-1}-\varvec{K}_{k}^{(1)}\varvec{P}_{zz, k|k}^{(1)}\left( \varvec{K}_{k}^{(1)}\right) ^{T} \end{aligned}$$

(43)

Substituting $\hat{\varvec{x}}_{k|k}^{(0)}=\hat{\varvec{x}}_{k|k-1}$ and $\varvec{P}_{k|k}^{(0)}=\varvec{P}_{k|k-1}$ into (17)–(19), we can obtain

$$\begin{aligned} \bar{\varvec{u}}_{k}^{(0)}= & {} \int _{{\mathbb {R}}^{n}}\varvec{h}_{k}(\varvec{x}_{k})N(\varvec{x}_{k};\hat{\varvec{x}}_{k|k-1}, \varvec{P}_{k|k-1})d\varvec{x}_{k}=\hat{\varvec{z}}_{k|k-1} \end{aligned}$$

(44)

$$\begin{aligned} \varvec{P}_{xu,k|k}^{(0)}= & {} \int _{{\mathbb {R}}^{n}}\varvec{x}_{k} \varvec{h}_{k}^{T}(\varvec{x}_{k}) N(\varvec{x}_{k};\hat{\varvec{x}}_{k|k-1}, \varvec{P}_{k|k-1})d\varvec{x}_{k}-\hat{\varvec{x}}_{k|k-1}\hat{\varvec{z}}_{k|k-1}^{T}=\varvec{P}_{xz,k|k-1} \nonumber \\ \end{aligned}$$

(45)

$$\begin{aligned} \varvec{P}_{uu,k|k}^{(0)}= & {} \int _{{\mathbb {R}}^{n}} \varvec{h}_{k}(\varvec{x}_{k})\varvec{h}_{k}^{T}(\varvec{x}_{k})N(\varvec{x}_{k}; \hat{\varvec{x}}_{k|k-1}, \varvec{P}_{k|k-1})d\varvec{x}_{k}- \hat{\varvec{z}}_{k|k-1}\hat{\varvec{z}}_{k|k-1}^{T} \end{aligned}$$

(46)

where $\hat{\varvec{z}}_{k|k-1}$ is the predicted measurement at time k, $\varvec{P}_{xz,k|k-1}$ is the cross-covariance matrix at time k. Substituting (46) into (40), we can obtain

$$\begin{aligned} \varvec{P}_{zz, k|k}^{(1)}=\varvec{P}_{zz,k|k-1} \end{aligned}$$

(47)

where $\varvec{P}_{zz,k|k-1}$ is the innovation covariance matrix at time k. Substituting (44)–(45) and (47) into (41)–(43), we can obtain

$$\begin{aligned} \varvec{K}_{k}^{(1)}=\varvec{K}_{k} \quad \hat{\varvec{x}}_{k|k}^{(1)}=\hat{\varvec{x}}_{k|k} \quad \varvec{P}_{k|k}^{(1)}=\varvec{P}_{k|k} \end{aligned}$$

(48)

Thus, when $N=1$, the proposed SPKF is identical to the classic SPKF.

Finally, we can also see from Table 1 that, when $N>1$, the proposed SPKF is not identical to the classic SPKF. Moreover, when $N>1$, the total measurement update of the proposed SPKF is nonlinear because the linear measurement update is applied gradually and the statistical linearization of nonlinear measurement function is implemented repeatedly; however, the measurement update of the classic SPKF is linear. Besides, the classic SPKF may fail in some applications with large prior uncertainty and high measurement accuracy, but the proposed SPKF with recursive measurement update can avoid such problem by applying linear measurement update gradually.

3 Simulation

In this section, the superior performance of the proposed method as compared with existing methods is shown by univariate nonstationary growth model and bearing-only tracking. In these simulations, we choose standard UT with tuning parameter $\kappa =0$ to implement the proposed method and existing methods, which leads to UKF with recursive measurement update, PUKF [14], ISPKF [12] and IUKF [19] (Note that an automatic step size determination technique is used in the PUKF). MIUKF [4] was often found to halt its operation, and thus, its simulation results are not shown in the following simulation.

3.1 Univariate Nonstationary Growth Model

Univariate nonstationary growth model is very popular in econometrics, and it has been widely used as a benchmark problem to validate the performance of nonlinear filters due to its high nonlinearity. Its dynamic state-space model can be written as follows [5]

$$\begin{aligned} \left\{ \begin{array}{l} \varvec{x}_k=0.5\varvec{x}_{k-1}+10\frac{\varvec{x}_{k-1}}{1+\varvec{x}_{k-1}^2}+8\cos (1.2k)+\varvec{w}_{k-1}\\ \varvec{z}_k=\frac{\varvec{x}_{k}^3}{20}+\varvec{v}_{k} \end{array}\right. \end{aligned}$$

(49)

where the process noise $\varvec{w}_{k}$ and measurement noise $\varvec{v}_{k}$ are uncorrelated zero-mean Gaussian white processes with $\varvec{Q}_{k}=2$ and $\varvec{R}_{k}=1$; the initial true state $\varvec{x}_{0}=0.1$, the initial state estimate $\hat{\varvec{x}}_{0|0}=0$ and its corresponding estimate error variance $\varvec{P}_{0|0}=1$. Simulation time is $T=2000\mathrm{s}$, and the time-averaged square error (TSE) and time-averaged mean square error (TMSE) are chosen as performance metrics in this simulation, which are formulated as

$$\begin{aligned} {\mathrm {TSE}}= & {} \frac{1}{T }\sum \limits _{k =1}^{T } \left( \varvec{x}_{k}^{s}-\hat{\varvec{x}}_{k}^{s}\right) ^2 \end{aligned}$$

(50)

$$\begin{aligned} {\mathrm {TMSE}}= & {} \frac{1}{MT }\sum \limits _{s =1}^{M } \sum \limits _{k =1}^{T }\left( \varvec{x}_{k}^{s}-\hat{\varvec{x}}_{k}^{s}\right) ^2 \end{aligned}$$

(51)

where $M=50$ denotes the number of Monte Carlo run, and $\varvec{x}_{k}^{s}$ and $\hat{\varvec{x}}_{k}^{s}$ are the true and estimated state at time k of the s-th Monte Carlo run. (Note that we only use 50 Monte Carlo runs in this simulation so that we can clearly see the TSE of each run in Fig. 1.)

Fig. 1

TSEs of the proposed method and existing methods when $N=10$ in 50 Monte Carlo runs

In situation I, the number of recursion steps is set as $N=10$, and the TSEs of existing PUKF [14], existing ISPKF [12], existing IUKF [19], existing EKF with recursive measurement update [18] and the proposed method are shown in Fig. 1.

It is clear to see from Fig. 1 that TSE of the proposed method is smaller than that of existing methods. Thus, the proposed method has higher estimation accuracy than existing methods.

In situation II, $N=1:20$ and Fig. 2 shows the TMSEs of existing ISPKF [12], existing IUKF [19], existing EKF with recursive measurement update [18] and the proposed method.

Fig. 2

TMSEs of the proposed method and existing methods when $N=1:20$

It is clear to see from Fig. 2 that the proposed method has higher estimation accuracy than existing methods when $N=1:20$. We also can see from Fig. 2 that TMSE of the proposed method is convergent while TMSEs of existing ISPKF are oscillating, which implies that the proposed method has better convergence with respect to the number of recursion N. Although existing EKF with recursive measurement update is convergent with respect to the number of recursion N, it has worse filtering performance than the proposed method. Besides, the TMSE of existing IUKF keeps constant, which implies that recursions are redundant for existing IUKF.

3.2 Bearing-only Tracking

The bearing-only tracking example has been studied extensively. The target moves within the $s-t$ plane according to the following dynamic model [17, 20]

$$\begin{aligned} \varvec{x}_{k}=\left[ \begin{array}{cccc} 1 \quad 1 \quad 0 \quad 0 \\ 0 \quad 1 \quad 0 \quad 0 \\ 0 \quad 0 \quad 1 \quad 1 \\ 0 \quad 0 \quad 0 \quad 1 \end{array}\right] \varvec{x}_{k-1}+ \left[ \begin{array}{cccc} 0.5 &{} 0 \\ 1 &{} 0 \\ 0 &{} 0.5 \\ 0 &{} 1 \end{array}\right] \varvec{w}_{k-1} \end{aligned}$$

(52)

where $\varvec{x}_{k}=[s,\dot{s},t,\dot{t}]_{k}^{T}$, $\varvec{w}_{k}=[w_s,w_t]_{k}^{T}$, s and t are Cartesian coordinates of the moving target, $\dot{s}$ and $\dot{t}$ are corresponding velocities in s and t direction, and the process noise $\varvec{w}_{k}\sim {N}(0,0.001^2\varvec{I}_{2\times 2})$. The measurement model of bearing-only observer fixed at the origin of the plane can be formulated as

$$\begin{aligned} \varvec{z}_{k}=\arctan (t_{k}/s_{k})+\varvec{v}_{k} \end{aligned}$$

(53)

where the measurement noise $\varvec{v}_{k}\sim {N}(0,2.5\times 10^{-4})$. The initial true state vector $\varvec{x}_{0}=[-0.05,0.001,0.7,-0.055]^{T}$, and the initial estimate error covariance matrix $\varvec{P}_{0|0}=10*{\mathrm {diag}}([0.1^2 \; 0.005^2 \; 0.1^2 \; 0.01^2])$; the initial state estimate $\hat{\varvec{x}}_{0|0}$ is chosen randomly from $N(\varvec{x}_{0},\varvec{P}_{0|0})$. Simulation time is $T=30s$, and the logarithmic MSEs (LMSEs) of positions and velocities and the logarithmic TMSEs (LTMSEs) of positions and velocities are chosen as performance metrics in this simulation, which are formulated as

$$\begin{aligned} \left\{ \begin{array}{l} {\mathrm {LMSE}}_{{\mathrm {pos}}}=\log 10 \left\{ \frac{1}{M }\sum \limits _{l =1}^{M } \left[ \left( \varvec{s}_{k}^{l}-\hat{\varvec{s}}_{k}^{l}\right) ^2+\left( \varvec{t}_{k}^{l}- \hat{\varvec{t}}_{k}^{l}\right) ^2\right] \right\} \\ {\mathrm {LMSE}}_{{\mathrm {vel}}}=\log 10 \left\{ \frac{1}{M }\sum \limits _{l =1}^{M } \left[ \left( \dot{\varvec{s}}_{k}^{l}-\hat{\dot{\varvec{s}}}_{k}^{l}\right) ^2+ \left( \dot{\varvec{t}}_{k}^{l}-\hat{\dot{\varvec{t}}}_{k}^{l}\right) ^2\right] \right\} \end{array}\right. \end{aligned}$$

(54)

$$\begin{aligned} \left\{ \begin{array}{l} {\mathrm {LTMSE}}_{{\mathrm {pos}}}=\log 10\left\{ \frac{1}{MT } \sum \limits _{l =1}^{M }\sum \limits _{k =1}^{T } \left[ \left( \varvec{s}_{k}^{l}-\hat{\varvec{s}}_{k}^{l}\right) ^2+ \left( \varvec{t}_{k}^{l}-\hat{\varvec{t}}_{k}^{l}\right) ^2\right] \right\} \\ {\mathrm {LTMSE}}_{{\mathrm {vel}}}=\log 10\left\{ \frac{1}{MT } \sum \limits _{l =1}^{M }\sum \limits _{k =1}^{T } \left[ \left( \dot{\varvec{s}}_{k}^{l}-\hat{\dot{\varvec{s}}}_{k}^{l} \right) ^2+\left( \dot{\varvec{t}}_{k}^{l}-\hat{\dot{\varvec{t}}}_{k}^{l}\right) ^2 \right] \right\} \end{array}\right. \end{aligned}$$

(55)

where $M=1000$ denotes the number of Monte Carlo run.

In situation I, $N=10$, and the LMSEs of existing PUKF [14], existing ISPKF [12], existing IUKF [19], existing EKF with recursive measurement update [18] and the proposed method are shown in Figs. 3 and 4.

Fig. 3

${\mathrm {LMSE}}_{{\mathrm {pos}}}$ of the proposed method and existing methods when $N=10$

Fig. 4

${\mathrm {LMSE}}_{{\mathrm {vel}}}$ of the proposed method and existing methods when $N=10$

It is clear to see from Figs. 3 and 4 that ${\mathrm {LMSE}}_{{\mathrm {pos}}}$ and ${\mathrm {LMSE}}_{{\mathrm {vel}}}$ of the proposed method are both smaller than that of existing methods. Thus, the proposed method has higher estimation accuracy than existing methods.

In situation II, $N=1:20$ and Figs. 5 and 6 show ${\mathrm {LTMSE}}_{{\mathrm {pos}}}$s and ${\mathrm {LTMSE}}_{{\mathrm {vel}}}$s of existing ISPKF [12], existing IUKF [19], existing EKF with recursive measurement update [18] and the proposed method.

Fig. 5

${\mathrm {LTMSE}}_{{\mathrm {pos}}}$ of the proposed method and existing methods when $N=1:20$

Fig. 6

${\mathrm {LTMSE}}_{{\mathrm {vel}}}$ of the proposed method and existing methods when $N=1:20$

It is seen from Figs. 5 and 6 that the proposed method has higher filtering accuracy than existing methods. We also can see from Figs. 5 and 6 that ${\mathrm {LTMSE}}_{{\mathrm {pos}}}$ and ${\mathrm {LTMSE}}_{{\mathrm {vel}}}$ of the proposed method are convergent while that of existing ISPKF and IUKF almost keep constant, which implies that recursions are redundant for existing ISPKF and IUKF. Besides, the proposed method has faster convergence speed than existing EKF with recursive measurement update with respect to number of recursion N.

4 Conclusion

In this paper, a new SPKF with recursive measurement update is proposed to improve the measurement update of existing nonlinear Kalman approximation filter. The total update of the proposed filter is nonlinear, and the proposed filter can extract state information from nonlinear measurement better than existing methods. Simulation results show that the proposed method outperforms existing methods in terms of filtering accuracy for state estimation with nonlinear measurements.