An Iterative Nonlinear Filter Using Variational Bayesian Optimization

Abstract

We propose an iterative nonlinear estimator based on the technique of variational Bayesian optimization. The posterior distribution of the underlying system state is approximated by a solvable variational distribution approached iteratively using evidence lower bound optimization subject to a minimal weighted Kullback-Leibler divergence, where a penalty factor is considered to adjust the step size of the iteration. Based on linearization, the iterative nonlinear filter is derived in a closed-form. The performance of the proposed algorithm is compared with several nonlinear filters in the literature using simulated target tracking examples.

1. Introduction

Bayesian estimation is widely applied across many areas of engineering such as target tracking, aerial surveillance, intelligent vehicles, and machine learning [1]. In linear Gaussian systems, the state estimation can be optimally achieved using a Kalman filter as a closed form solution. However, many real-world estimation problems are nonlinear, resulting in analytically intractable posterior probability density function (PDF) for the state. In consequence, suboptimal approximation methods are sought to solve the nonlinear estimation problems [2].

Many suboptimal techniques have been developed to solve nonlinear estimation problems. These techniques may be divided into the following three categories. The first category, includes the extended Kalman filter (EKF) [3], the iterated extended Kalman filter (IEKF) [4,5], and their variants [6,7], solves the state estimation problem through replacing the nonlinear functions by their linear approximations via the Taylor expansion. The second category involves stochastic sampling methods. In the filtering process, a set of randomly sampled points with weights are adopted to approximate the PDF of the underlying state. For example, the particle filter (PF) [8,9,10] is a sequential Monte Carlo (SMC) stochastic sampling method, which approximates the PDF by the sampled particles from a proposal distribution. PF can be applied to nonlinear non-Gaussian systems. Markov Chain Monte Carlo (MCMC) is another popular stochastic method since it is able to achieve arbitrarily high accuracy using a large number of particles, sometimes resulting in prohibitive computational expenditure. The techniques in the third category use deterministic sampling methods. Nonlinear state PDFs are approximated by a set of fixed points and weights that represent the location and spread of the distribution. This category includes the unscented Kalman filter (UKF) [9,11,12], the cubature Kalman filter (CKF) [13,14], and the central difference Kalman filter (CDKF) [15]. The UKF and the CDKF approximate the nonlinear state transition function and the measurement function by the unscented transform (UT) and the Stirling interpolation, respectively. The state and the corresponding error covariance are then calculated based on the sampling points and the weights. The CKF uses the third order spherical-radial cubature rule to approximate integrals numerically.

Where nonlinear filters involve optimisation, quasi-Newton approximations are used, along with Kullback-Leibler (KL) divergence and $\alpha$-divergence as objective functions. For example, the EKF update step may use a Hessian correction term, resulting in improved performance [7,16]. Another approach for estimation, in some ways can be regarded as the fourth category, uses the variational Bayes (VB) technique. VB has a unified and principled architecture and can be used, as with other methods mentioned earlier, for problems where analytic solutions cannot be found. The objective of the VB approximation is to find the variational distribution best able, in the sense of the KL divergence, to approximate the true PDF [17,18,19]. Various variational inference methods for parametric distributions are discussed in [20], where the true PDF of hidden variables is assumed to be given by a parametric model. Since VB usually runs faster than MCMC [20,21,22], it is widely applied in areas such as statistics, finite element analysis, machine learning, etc. An important concern with the VB inference is the accuracy of the approximation [23]. Paul [24] and Stephane et al. [25] introduced a proximal point algorithm in the framework of the expectation-maximization (EM) and studied its convergence. Khan et al. [18,26] adopted KL divergence as a measure of divergence to improve the accuracy of VB inference by considering the geometry of the true posterior PDF. They applied their algorithm to parameter estimation, data set classification and regression.

In this paper, we adopt KL divergence as a metric of posterior PDF approximation, and derive an alternative nonlinear estimation algorithm based on the VB technique. The posterior PDF of the underlying system state is approximated by a parameterized variational distribution and the difference between the two distributions is minimized iteratively using a weighted KL divergence criterion. This is carried out through optimization of the evidence lower bound (ELBO); the intractable integration of the posterior PDF is converted to a solvable optimization problem. A penalty factor is applied to ensure that the filter algorithm obtains a good trade-off between accuracy and computational overhead. Numerical simulations of two typical target tracking scenarios and a benchmark nonlinear filtering problem are presented. The simulation results show that the approximation of nonlinear stochastic system state by the proposed algorithm is tighter than EKF and UKF. The computational cost and the effect of different penalty factors on estimation accuracy are analyzed.

The rest of the paper is organized as follows. In Section 2, we introduce the general nonlinear Bayesian filtering problem. The formulation of the ELBO is described in Section 3. In Section 4, we propose the proximal iterative nonlinear filter. In Section 5, we present simulations of two target tracking scenarios, where the performance of the proposed algorithm is compared with those of EKF and UKF in terms of estimation accuracy and computational overhead. Lastly, conclusions are drawn in Section 6.

2. Problem Formulation

Consider a general dynamic system with measurement as follows.

$$x_k=f_k\left(x_{k-1}\right)+{\omega }_k,$$

(1)

$$z_k=h_k\left(x_k\right)+v_k,$$

(2)

where $f_k\left(·\right)$ denotes the state transition function, and $h_k\left(·\right)$ denotes the mapping from the system state to the measurement; ${\omega }_k$ and $v_k$ are the process noise and the measurement noise, respectively. We assume that ${\omega }_k$ and $v_k$ are Gaussian and mutually independent, ${\omega }_k\sim \mathcal{N}\left(0,Q_k\right)$ and $v_k\sim \mathcal{N}\left(0,R_k\right)$.

Assuming that the posterior PDF $p\left(x_{k-1}|z_{k-1}\right)$ at time $k-1$ is available, the PDF of the predicted state is obtained by the Chapman-Kolmogorov equation.

$$p\left(x_k|z_{k-1}\right)=\int p\left(x_k|x_{k-1}\right)p\left(x_{k-1}|z_{k-1}\right)d{x}_{k-1}.$$

(3)

Then, at time k, the posterior PDF is obtained using the measurement $z_k$ by an application of the Bayes rule:

$$p\left(x_k|z_k\right)=\frac{p\left(z_k|x_k\right)p\left(x_k|z_{k-1}\right)}{\int p\left(z_k|x_k\right)p\left(x_k|z_{k-1}\right)d{x}_k}.$$

(4)

For linear Gaussian systems, it is well known that the optimal state estimation $x_{k|k}$ and the corresponding error covariance $P_{k|k}$ under the criterion of minimum variance estimation are obtained by:

$$x_{k|k}=\int x_{k}p\left(x_k|z_k\right)d{x}_k,$$

(5)

$$P_{k|k}=\int \left(x_k-x_{k|k}\right){\left(x_k-x_{k|k}\right)}^{\mathrm{T}}p\left(x_k|z_k\right)d{x}_k.$$

(6)

For nonlinear systems, the integral in Equation (4) is often intractable. Suboptimal approximations for the posterior PDF are needed. Most existing suboptimal algorithms adopt linearization or sampling techniques to approximate the posterior PDF $p\left(x_k|z_k\right)$. We consider an iterative VB approach, in which the true PDF is approximated by a variational distribution and is approached by iterative optimization of the ELBO. The proposed algorithm converts the nontrivial integration to a closed-form optimization and therefore improves estimation accuracy.

3. Evidence Lower Bound Maximization

The above nonlinear estimation problem can be solved using a VB framework. Express the marginal PDF $p\left(z_k\right)$ using a variational distribution $q\left(x_k|{\psi }_k\right)$ as follows:

$$\begin{array}{cc}\hfill logp\left(z_k\right)& =log\int q\left(x_k|{\psi }_k\right)\frac{p\left(x_k,z_k\right)}{q\left(x_k|{\psi }_k\right)}d{x}_k\hfill \\ & =\mathcal{L}\left({\psi }_k\right)+{\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right|z_k)\parallel q\left(x_k\right|{\psi }_k)\right],\hfill \end{array}$$

(7)

where ${\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right|z_k)\parallel q\left(x_k\right|{\psi }_k)\right]$ is the KL divergence between the true posterior PDF $p\left(x_k\right|z_k)$ and the variational distribution $q\left(x_k\right|{\psi }_k)$; that is,

$${\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right|z_k)\parallel q\left(x_k\right|{\psi }_k)\right]=-\int q\left(x_k|{\psi }_k\right)log\frac{p\left(x_k|z_k\right)}{q\left(x_k|{\psi }_k\right)}d{x}_k.$$

(8)

$\mathcal{L}\left({\psi }_k\right)$ is the variational ELBO:

$$\begin{array}{cc}\hfill \mathcal{L}\left({\psi }_k\right)& =\int q\left(x_k|{\psi }_k\right)log\frac{p\left(x_k,z_k\right)}{q\left(x_k|{\psi }_k\right)}d{x}_k\hfill \\ & ={\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]-{\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right)\parallel q\left(x_k\right|{\psi }_k)\right].\hfill \end{array}$$

(9)

The variational distribution $q\left(x_k|{\psi }_k\right)$ is assumed Gaussian with unknown parameter ${\psi }_k=\left(x_{k|k},P_{k|k}\right)$ (to be estimated), where $x_{k|k}$ is the mean and $P_{k|k}$ is the covariance.

Please note that the poterior PDF $p\left(x_k\right|z_k)$ needs to be closely approximated by a known distribution in nonlinear filtering. From Equation (7), evidently, the variational distribution $q\left(x_k\right|{\varphi }_k)$ would be equal to the true posterior PDF $p\left(x_k\right|z_k)$ if the KL divergence were zero. Minimizing the KL divergence, and thereby approximating the posterior PDF by a variational distribution, is equivalent to maximizing the ELBO, i.e.,

$$\begin{array}{cc}\hfill {\psi }_k^{*}& =\underset{{\psi }_k}{argmax}\mathcal{L}\left({\psi }_k\right).\hfill \end{array}$$

(10)

According to Equations (9) and (10), the nontrivial integration of Equation (7) is converted to the problem of maximizing ELBO, which can be solved by the VB method.

4. Proximal Iterative Nonlinear Filter

In this section, we derive an iterative procedure in a closed-form to iteratively maximize the ELBO so as to minimize the KL divergence between the true posterior PDF $p\left(x_k\right|z_k)$ and the variational distribution $q\left(x_k\right|{\psi }_k)$.

4.1. Penalty Function Based on KL Divergence

Notice that the KL divergence ${\mathsf{D}}_{\mathrm{KL}}\left[q\left(x_k|{\psi }_k\right)\parallel q\left(x_k|{\psi }_k^i\right)\right]$ is nonnegative for all $q\left(x_k|{\psi }_k\right)$. Following [24], we adopt the proximal point algorithm to generate a sequence ${\psi }_k^{i+1}$ via the following iterative scheme,

$${\psi }_k^{i+1}=\underset{{\psi }_k}{argmax}\mathcal{L}\left({\psi }_k\right)-\frac{1}{{\beta }_i}{\mathsf{D}}_{\mathrm{KL}}\left[q\left(x_k|{\psi }_k\right)\parallel q\left(x_k|{\psi }_k^i\right)\right],$$

(11)

where i denotes the iteration index and the penalty factor ${\beta }_i$ is used to adjust the optimization step length. Roughly speaking, the ELBO is maximized when the KL divergence between the two variational distributions $q\left(x_k|{\psi }_k\right)$ and $q\left(x_k|{\psi }_k^i\right)$ approaches zero. Equation (11) can be rewritten as

$${\psi }_k^{i+1}=\underset{{\psi }_k}{argmax}\left\{\mathcal{L}\left({\psi }_k\right)-\frac{1}{{\beta }_i}{\mathsf{D}}_{\mathrm{KL}}\left[q\left(x_k|{\psi }_k\right)\parallel q\left(x_k|{\psi }_k^i\right)\right]\right\}.$$

(12)

Please note that one iteration of this proximal method is equivalent to moving a step in the direction of the natural gradient [18]. The influence of the KL divergence on ${\psi }_k^{i+1}$ can be adjusted by ${\beta }_i$. The larger the ${\beta }_i$, the weaker the influence of the KL divergence on ${\psi }_k^{i+1}$, and vice versa. In [18], it is assumed that ${\beta }_i=1$.

4.2. The Proximal Iterative Nonlinear Filter

The proximal iterative method is implemented via an iterative minimization of the KL divergence, where the initial state is assigned with an estimation from a core-filter, e.g., Bayesian filter. Here, EKF is adopted as the core-filter to predict and update the system state before the iterative optimization process. We propose a proximal iterative nonlinear filter combined with VB, called PEKF-VB, which is described and derived in the following.

Firstly, by substituting Equation (10) into Equation (12), we can rewrite the iterative optimization as

$$\begin{array}{c}\hfill {\psi }_k^{i+1}\underset{{\psi }_k}{=argmax}\left\{{\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]-{\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right)\parallel q\left(x_k\right|{\psi }_k)\right]-\frac{1}{{\beta }_i}{\mathsf{D}}_{\mathrm{KL}}\left[q\left(x_k|{\psi }_k\right)\parallel q\left(x_k|{\psi }_k^i\right)\right]\right\}.\end{array}$$

(13)

Under the Gaussian assumptions for the process noise and the measurement noise, the variational distribution is of the form $q\left(x_k|{\psi }_k\right)\sim \mathcal{N}(x_k;x_{k|k},P_{k|k})$. Given the prior of the state $x_{k-1|k-1}$ at time $k-1$, we can assume $p\left(x_k\right)\sim \mathcal{N}\left(x_k;x_{k|k-1},P_{k|k-1}\right)$. Accordingly, the ${\psi }_k^{i+1}$ is

$$\begin{array}{cc}\hfill {\psi }_k^{i+1}=& \underset{{\psi }_k}{argmax}\left\{{\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]-{\mathsf{D}}_{\mathrm{KL}}\left[\mathcal{N}(x_k;x_{k|k-1},P_{k|k-1})\parallel \mathcal{N}\left(x_k;x_{k|k},P_{k|k}\right)\right]\right.\hfill \\ & \left.-\frac{1}{{\beta }_i}{\mathsf{D}}_{\mathrm{KL}}\left[q\left(x_k|{\psi }_k\right)\parallel q\left(x_k|{\psi }_k^i\right)\right]\right\}.\hfill \end{array}$$

(14)

For the first term in Equation (14), the expectation ${\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]$ related to $x_k$ and $P_k$ can be approximated linearly using the gradients with respect to (w. r. t.) $x_k$ and $P_k$. Defining $g\left(x_{k|k},P_{k|k}\right)\triangleq {\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]$, the gradients of g w. r. t. $x_{k|k}$ and $P_{k|k}$ are

$${\nabla }_{x}g\left(x_{k|k},P_{k|k}\right)=-{\alpha }_k,$$

(15)

$${\nabla }_{P}g\left(x_{k|k},P_{k|k}\right)=-{\gamma }_k.$$

(16)

The expectation ${\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]$ at time k is then maximized by gradient ascent in the variables $x_{k|k}$ and $P_{k|k}$; that is,

$$\begin{array}{c}\hfill {\mathsf{E}}_q\left[logp\left(z_k|x_k\right)\right]\approx {\alpha }_k{x}_{k|k}+\frac{1}{2}{\gamma }_k{P}_{k|k},\end{array}$$

(17)

where ${\alpha }_k^i$ and ${\gamma }_k^i$ at iteration i are given by

$${\alpha }_k^i=-{\left(H_k^i\right)}^{\mathrm{T}}{R}_k^{-1}\left(z_k-h_k\left(x_{k|k}^i\right)\right),$$

(18)

$${\gamma }_k^i=\frac{1}{2}{\left(H_k^i\right)}^{\mathrm{T}}{R}_k^{-1}{H}_k^i,$$

(19)

and $H_k^i$ is the Jacobian matrix:

$$H_k^i=\frac{\partial h_k\left(x\right)}{\partial x^{\mathrm{T}}}{|}_{x=x_{k|k}^i}.$$

(20)

In other words, the coefficients ${\alpha }_k^i$ and ${\gamma }_k^i$ are updated by $H_k^i$ with $x_{k|k}^i$ in each iterative step. The detailed calculations of ${\alpha }_k^i$ and ${\gamma }_k^i$ are given in Appendix A.

For Gaussian distributions $\mathcal{N}\left({\xi }_1;{\mu }_1,C_1\right)$ and $\mathcal{N}\left({\xi }_2;{\mu }_2,C_2\right)$ with the same dimension d, the KL divergence is

$$\begin{array}{c}\hfill {\mathsf{D}}_{\mathrm{KL}}\left[\mathcal{N}\left({\xi }_1;{\mu }_1,C_1\right)\parallel \mathcal{N}\left({\xi }_2;{\mu }_2,C_2\right)\right]=-\frac{1}{2}\left[log\left|C_1{C}_2^{-1}\right|-\mathrm{tr}\left(C_1{C}_2^{-1}\right)-{\left({\mu }_1-{\mu }_2\right)}^{\mathrm{T}}{C}_2^{-1}\left({\mu }_1-{\mu }_2\right)+d\right].\end{array}$$

(21)

where operators $\mathrm{tr}(·)$ and $\left|·\right|$ denote the trace and the determinant of a matrix, respectively. By Equations (17)–(21), Equation (13) can be rewritten as,

$$\begin{array}{cc}\hfill {\psi }_k^{i+1}=& \underset{{\psi }_k}{argmax}\left\{{\alpha }_k^i{x}_{k|k}+\frac{1}{2}{\gamma }_k^i{P}_{k|k}-\frac{1}{2}\left[log\left|P_{k|k-1}{\left(P_{k|k}^i\right)}^{-1}\right|-\mathrm{tr}\left(P_{k|k-1}{P}_{k|k}^{-1}\right)\right.\right.\hfill \\ & -\mathrm{tr}\left(P_{k|k}{\left(P_{k|k}^i\right)}^{-1}\right)-{\left(x_{k|k-1}-x_{k|k}\right)}^{\mathrm{T}}{P}_{k|k}^{-1}\left(x_{k|k-1}-x_{k|k}\right)\hfill \\ & \left.\left.-{\left(x_{k|k}-x_{k|k}^i\right)}^{\mathrm{T}}{\left(P_{k|k}^i\right)}^{-1}\left(x_{k|k}-x_{k|k}^i\right)\right]-d\right\}.\hfill \end{array}$$

(22)

Then ${\psi }_k^{i+1}$ is maximized at a point $(x,P)$ which can be explicitly calculated. To find them, set the partial derivatives of ${\psi }_k^{i+1}$ w.r.t. $\{x,P\}$ to be zero, i.e.,

$$\frac{\partial {\psi }_k^{i+1}}{\partial x}{{|}_{x=x_{k|k}^{i+1}}=0,\frac{\partial {\psi }_k^{i+1}}{\partial P}|}_{P=P_{k|k}^{i+1}}=0.$$

(23)

Accordingly, $x_{k|k}^{i+1}$ and $p_{k|k}^{i+1}$ are seen to be

$$\begin{array}{cc}\hfill x_{k|k}^{i+1}& ={\left[\frac{{\beta }_i}{1+{\beta }_i}{P}_{k|k-1}^{-1}+\frac{1}{1+{\beta }_i}{\left(P_{k|k}^i\right)}^{-1}\right]}^{-1}\left[\frac{{\beta }_i}{1+{\beta }_i}\left(P_{k|k-1}^{-1}{x}_{k|k-1}-{\alpha }_k^i\right)+\frac{1}{1+{\beta }_i}{\left(P_{k|k}^i\right)}^{-1}{x}_{k|k}^i\right]\hfill \\ & ={\left[\left(1-b_i\right)P_{k|k-1}^{-1}+b_i{\left(P_{k|k}^i\right)}^{-1}\right]}^{-1}\left[\left(1-b_i\right)\left(P_{k|k-1}^{-1}{x}_{k|k-1}-{\alpha }_k^i\right)+b_i{\left(P_{k|k}^i\right)}^{-1}{x}_{k|k}^i\right],\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill P_{k|k}^{i+1}& ={\left[\frac{1}{1+{\beta }_i}{\left(P_{k|k}^i\right)}^{-1}+\frac{{\beta }_i}{1+{\beta }_i}\left(P_{k|k-1}^{-1}+{\gamma }_k^i\right)\right]}^{-1}={\left[b_i{\left(P_{k|k}^i\right)}^{-1}+\left(1-b_i\right)\left(P_{k|k-1}^{-1}+{\gamma }_k^i\right)\right]}^{-1},\hfill \end{array}$$

(25)

where $b_i=\frac{1}{1+{\beta }_i}$. Equations (24) and (25) show that the state estimate and the associated covariance in the iteration are updated by ${\alpha }_k^i$ and ${\gamma }_k^i$, respectively. As shown in Figure 1, the complete iteration procedure consists of Equations (18), (19), (24) and (25).

We note that, in principle, the computational cost in Equations (24) and (25) can be slightly reduced by using the Matrix Inversion Lemma [27]. As a result, Equations (24) and (25) are derived as Equations (26) and (27), respectively.

$$x_{k|k}^{i+1}=\left\{\frac{1}{b_i}{P}_{k|k}^i-\frac{1}{b_i}{P}_{k|k}^i\left[\frac{1}{1-b_i}{P}_{k|k-1}+\frac{1}{b_i}{P}_{k|k}^i\right]\frac{1}{b_i}{P}_{k|k}^i\right\}\left[\left(1-b_i\right)\left(P_{k|k-1}^{-1}{x}_{k|k-1}-{\alpha }_k^i\right)+b_i{\left(P_{k|k}^i\right)}^{-1}{x}_{k|k}^i\right]$$

(26)

$$P_{k|k}^{i+1}=\frac{1}{1-b_i}{\mathcal{K}}_k^{-1}-\frac{b_i}{1-b_i}{\mathcal{K}}_k^{-1}{\left(P_{k|k}^i\right)}^{-1}{\left(I_{4\times 4}+\frac{b_i}{1-b_i}{\mathcal{K}}_k^{-1}{\left(P_{k|k}^i\right)}^{-1}\right)}^{-1}\frac{1}{1-b_i}{\mathcal{K}}_k^{-1},$$

(27)

where ${\mathcal{K}}_k^{-1}={\left(P_{k|k-1}^{-1}+{\gamma }_k^i\right)}^{-1}=P_{k|k-1}^{-1}-P_{k|k-1}^{-1}{\gamma }_k^i\left(I_{4\times 4}+P_{k|k-1}{\gamma }_k^i\right)P_{k|k-1}$.

For special case when ${\beta }_i=1$, Equations (24) and (25) can be rewritten as

$$\begin{array}{cc}\hfill x_{k|k}^{i+1}& =c_1{x}_{k|k-1}+c_2{x}_{k|k}^i-c_g{\alpha }_k^i,\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill P_{k|k}^{i+1}& =2{\left(c_g^{-1}+{\gamma }_k^i\right)}^{-1},\hfill \end{array}$$

(29)

where $c_g={\left(P_{k|k-1}^{-1}+{\left(P_{k|k}^i\right)}^{-1}\right)}^{-1}$, $c_1=c_g{P}_{k|k-1}^{-1}$ and $c_2=c_g{\left(P_{k|k-1}^i\right)}^{-1}$.

The flow diagram of the proposed PEKF-VB algorithm is shown in Figure 1 and the detailed implementation of PEKF-VB is given in Algorithm 1.

Algorithm 1 The implementation of the PEKF-VB algorithm

1: Initialization ($k=0$): state estimation $x_0$ and associated error covariance $P_0$, the number of iterations. 2: Compute the predicted state $x_{k|k-1}$ and the associated error covariance $P_{k|k-1}$ $$\begin{array}{c}\hfill x_{k|k-1}=f_k\left(x_{k-1|k-1}\right),P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^{\mathrm{T}}+Q_{k-1},\end{array}$$ where $F_k=\frac{\partial f_k\left(x\right)}{\partial x}{|}_{x=x_{k-1|k-1}}$. 3: Compute the filtering grain $K_k$ $$S_k=H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}+R_k,K_k=P_{k|k-1}{H}_k^{\mathrm{T}}{\left(S_k\right)}^{-1},$$ where $H_k=\frac{\partial h_k\left(x\right)}{\partial x}{|}_{x=x_{k|k-1}}$. 4: Update the state estimation $x_{k|k}$ and the associated error covariance $P_{k|k}$ $$\begin{array}{c}\hfill x_{k|k}^{*}=x_{k|k-1}-K_k\left(z_k-h_k\left(x_{k|k-1}\right)\right),P_{k|k}^{*}=P_{k|k-1}-K_k{H}_k{P}_{k|k-1}.\end{array}$$ 5: Let $x_{k|k}^1=x_{k|k}^{*}$, $P_{k|k}^1=P_{k|k}^{*}$, and $i=1$. 6: while not converge do 7:  Compute parameters ${\alpha }_k^{i+1}$ and ${\gamma }_k^{i+1}$ by Equations (18) and (19). 8:  Compute iterated state estimation $x_{k|k}^{i+1}$ and its error covariance $P_{k|k}^{i+1}$ by Equations (26) and (27). 9:  Let $i=i+1$. 10: end while 11: Let $k=k+1$, go back to Step 2.

4.3. Remarks

• The VB method approximates the true posterior PDF by choosing from a parameterized variational distribution. In each iteration of the PEKF-VB, the ELBO (9) increases. It follows that the ELBO is a proper criterion for measuring the performance of variational optimization. The ELBO of the proposed nonlinear filter is

  $$\begin{array}{cc}\hfill \mathcal{L}\left({\psi }_k\right)=& -\frac{1}{2}log\frac{{\left(2\pi \right)}^{D_z}|R_k\left|\right|P_{k|k-1}|}{|P_{k|k}|}-\frac{1}{2}\mathrm{tr}\left(P_{k|k-1}^{-1}\left[\left(x_{k|k}-x_{k|k-1}\right){\left(x_{k|k}-x_{k|k-1}\right)}^{\mathrm{T}}\right]\right)\hfill \\ & -\frac{1}{2}\mathrm{tr}\left(P_{k|k-1}^{-1}{P}_{k|k}\right)-\frac{1}{2}\mathrm{tr}\left(R_k^{-1}\left(\left(z_k-H_k{x}_{k|k}\right){\left(z_k-H_k{x}_{k|k}\right)}^{\mathrm{T}}+H_k{P}_{k|k}{H}_k^{\mathrm{T}}\right)\right)+\frac{D_x}{2},\hfill \end{array}$$

  (30)

  where $D_x$ and $D_z$ denote the dimension of the state and the dimension of the measurement, respectively. The derivation of the ELBO is given in Appendix B.
• Apart from the KL divergence, we can use Calvo and Oller’s distance (COD) as the penalty function in Equation (13); the corresponding filter is denoted by CODEKF. The COD of two distributions $f\left({\xi }_1\right)=p\left({\xi }_1|{\mu }_1,C_1\right)$ and $f\left({\xi }_2\right)=p\left({\xi }_2|{\mu }_2,C_2\right)$ is [28],

  $$d({\xi }_1,{\xi }_2)={\left[1/2\sum _{i=1}^{n+1}{ln}^2{\lambda }_i\right]}^{1/2},$$

  (31)

  where n is the dimension of ${\xi }_i$, $i\in \{1,2\}$, ${\lambda }_i$ are the eigenvalues of $f(\Delta \overline{\mu },D)$ with $\overline{\mu }=T^{\mathrm{T}}{C}_1^{-1/2}\Delta \mu$, $D=T^{\mathrm{T}}{C}_1^{-1/2}{C}_2{C}_1^{-1/2}T$ which is diagonal, and $\Delta \mu ={\mu }_2-{\mu }_1$, $T{T}^{\mathrm{T}}=I$. We replace the KLD in Equation (13) with Equation (31) as follows.

  $$\begin{array}{cc}\hfill {\psi }_k^{i+1}=& \underset{{\psi }_k}{argmax}\left\{({\alpha }_k^i{x}_{k|k}+\frac{1}{2}{\gamma }_k^i{P}_{k|k})-{\mathsf{D}}_{\mathrm{KL}}\left[p\left(x_k\right)\parallel q\left(x_k\right|{\psi }_k)\right]-\frac{1}{{\beta }_i}d({\xi }_1,{\xi }_2)\right\}.\hfill \end{array}$$

  (32)
• Since both PEKF-VB and CODEKF involve iterations within the VB framework to minimize the divergence between the posterior PDF and variational distribution, their complexity is increased by the calculation of the Jacobian in each iteration.
• In PEKF-VB, we use KL divergence to measure the similarity between two distributions. Under Gaussian assumptions for the distributions, a closed-form solution of the variational distribution has been derived. However, the VB framework with the KL divergence can also apply to non-Gaussian distributions. If no closed-form exists, a Monte Carlo method can be used to approximate the divergence. Other measures of dissimilarity between probability distributions, such as the alpha-divergence, the Rényi-divergence and the alpha-beta divergence, can also be used in the VB framework. See [29] and references therein. Unfortunately, in general, no computationally tractable form of the variational distribution can be derived and a Monte Carlo method has to be employed.

5. Numerical Simulations

In this section, we present two nonlinear estimation examples of 2D target tracking and a benchmark nonlinear filtering problem to illustrate the performance of PEKF-VB and CODEKF. We compare them with EKF and UKF. The performance is measured by root-mean-squared error (RMSE) of the estimates and the computational overhead.

Example 1: Range-bearing tracking. In this scenario, the underlying target motion is described by a constant turn (CT) model, with the state vector consists of 2D position and velocity components. As shown in Figure 2a, the target moves with initial state $x_0={(565.7,29.99,1166,-0.62)}^{\mathrm{T}}$ along a circular trajectory, and is observed by a range-bearing sensor. The state transition matrix $F_k$ in Equation (1) and the measurement function $h_k\left(x_k\right)$ in Equation (2) are:

$$F_k=\left[\begin{array}{cccc}1& sin\left(\dot{\theta }\mathrm{T}\right)/\dot{\theta }& 0& \left(cos\left(\dot{\theta }\mathrm{T}\right)-1\right)/\dot{\theta }\\ 0& cos\left(\dot{\theta }\mathrm{T}\right)& 0& -sin\left(\dot{\theta }\mathrm{T}\right)\\ 0& \left(1-cos\left(\dot{\theta }\mathrm{T}\right)\right)/\dot{\theta }& 1& sin\left(\dot{\theta }\mathrm{T}\right)/\dot{\theta }\\ 0& sin\left(v\dot{\theta }\mathrm{T}\right)& 0& cos\left(\dot{\theta }\mathrm{T}\right)\end{array}\right],$$

(33)

$$h_{k|k}\left(x_k\right)=\left[\begin{array}{c}\sqrt{\left(x^2+y^2\right)}\\ \mathrm{artan}\left(y/x\right)\end{array}\right],$$

(34)

where the turn rate $\dot{\theta }=-0.0333\mathrm{rad}/\mathrm{s}$, the covariances of the zero mean Gaussian white noises ${\omega }_k$ and ${\upsilon }_k$ are $Q_k=1.5Gq{G}^{\mathrm{T}}$ and $R_k=\mathrm{diag}\left(r^2,{\varphi }^2\right)$, respectively, where $q=I_{2\times 2}$, and $G={\left[\begin{array}{cccc}{\mathrm{T}}^2/2& \mathrm{T}& 0& 0\\ 0& 0& {\mathrm{T}}^2/2& \mathrm{T}\end{array}\right]}^{\mathrm{T}}$, $r=35$, and $\varphi =0.5\pi /180$. At each run, the track is initialised using the two-point method [1] with initial error covariance $P_0=\mathrm{diag}\left([600,100,600,100]\right)$. We let ${\beta }_i=1$, and $T=1\mathrm{s}$. The target moves for 80 scans (periods). 1000 Monte Carlo runs are carried out.

The RMSE plots of EKF, UKF, CODEKF and PEKF-VB are showed in Figure 2b. The black, red, green and blue curves are obtained by the PEKF-VB CODEKF, UKF and EKF, respectively. It is seen that in terms of RMSE performance, PEKF-VB is slightly better than CODEF; both PEKF-VB and CODEF are better than EKF and UKF.

Table 1. The quantitative comparison of the mean RMSE (from 20 to 80 scan) and the running time.

Algorithm	EKF	UKF	CODEKF	PEKF-VB
Time ratio	1	3.64	3.96	6.76
Mean RMSE	17.8129	17.8455	16.7758	16.3958

provides the quantitative comparison of RMSE and execution time of EKE, UKF and CODEKF, PEKF-VB. Figure 3b gives the relationship between iteration index and the running time of PEKF-VB.

In Figure 3a we compare the RMSE performance of PEKF-VB by varying the numbers of iterations. Notice that the RMSE decreases with the increasing of the number of iterations. Figure 3b gives the computational overhead of PEKF-VB w.r.t the number of iterations. Both results agree with our intuition.

To illustrate the convergence of PEKF-VB, we present the ELBO for different numbers of iterations in Figure 4. Figure 4a illustrates the ELBO at the second scan, and Figure 4b shows the ELBO for all scans. Clearly, the ELBO increases with the number of iterations increases, showing that the iteration procedure in PEKF-VB converges.

Example 2: Bearing-only tracking. In this scenario, a single target tracking using measurements from a single bearing-only sensor is considered. While the sensor (ownership) measurement model is nonlinear to the target state, the sensor has to maneuver relative to the target in order to observe it [30,31]. Let $x_k=\left[x_k,y_k,{\dot{x}}_k,{\dot{y}}_k\right]$ be the state of the target at time k, where $\left(x_k,y_k\right)$ and $\left({\dot{x}}_k,{\dot{y}}_k\right)$ are the position and velocity, respectively. The target, with an initial range of 5 km (relative to the sensor) and initial course of 220° in a clockwise direction (Set the positive axis of y is 0°), is modeled by a constant velocity model in Equation (35).

$$F_k=\left[\begin{array}{cccc}1& 0& \mathrm{T}& 0\\ 0& 1& 0& \mathrm{T}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

(35)

where $\mathrm{T}=1\min$. The speed of the target is $0.1235$ km/min. The sensor starts moving with a fixed speed of $0.1543$ km/min and an initial course of 140° in a clockwise direction (Set the positive axis of y is 0°). Please note that for the bearing-only tracking problem, to be able to estimate the range of the target, the sensor has to maneuver. Here we assume the sensor maneuvers from scan 14 to scan 17, and then moves with constant velocity from scan 18 to scan 40. The motion model of the sensor is given by Equation (36), where the turn rate $\dot{\theta }=30°/\min$. Both the target and the sensor move for 40 scans and their trajectories are shown in Figure 5a.

$$\left\{\begin{array}{cc}{S}_k=\left[\begin{array}{cccc}1& 0& \mathrm{T}& 0\\ 0& 1& 0& \mathrm{T}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\hfill & \hfill 0<k<14,18\le k\le 40,\\ S_k=\left[\begin{array}{cccc}1& 0& sin\left(\dot{\theta }\mathrm{T}\right)/\dot{\theta }& \left(cos\left(\dot{\theta }\mathrm{T}\right)-1\right)/\dot{\theta }\\ 0& 1& \left(1-cos\left(\dot{\theta }\mathrm{T}\right)\right)/\dot{\theta }& sin\left(\dot{\theta }\mathrm{T}\right)/\dot{\theta }\\ 0& 0& cos\left(\dot{\theta }\mathrm{T}\right)& -sin\left(\dot{\theta }\mathrm{T}\right)\\ 0& 0& sin\left(\dot{\theta }\mathrm{T}\right)& cos\left(\dot{\theta }\mathrm{T}\right)\end{array}\right],\hfill & \hfill 14\le k<18.\end{array}\right.$$

(36)

The measurement function $h_k\left(x_k\right)$ of the bearing-only sensor is,

$$h_k\left(x_k\right)=\mathrm{atan}\left(\left(x_k^{\prime }-x_s\right)/\left(y_k^{\prime }-y_s\right)\right),$$

(37)

where $x_k^{\prime }$ and $y_k^{\prime }$ are the position of the target in Cartesian coordinate system, $x_s$ and $y_s$ are the position of the sensor. The standard deviation of measurement noise is 1°. The initial position of the target is randomly sampled at range $r=13\mathrm{km}$ with covariance $P_0$

$$P_0=\mathcal{A}\mathcal{B}{\mathcal{A}}^{\prime },$$

(38)

where

$$\mathcal{A}=\left[\begin{array}{cccc}cos\dot{\theta }& sin\dot{\theta }& 0& 0\\ -sin\dot{\theta }& cos\dot{\theta }& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\mathcal{B}=\left[\begin{array}{cccc}{r}^2{\sigma }_m^2& 0& 0& 0\\ 0& \Delta r^2& 0& o\\ 0& 0& \Delta v^2& 0\\ 0& 0& 0& \Delta v^2\end{array}\right].$$

(39)

where ${\sigma }_m=\pi /180\mathrm{rad}$, $\Delta r=2\mathrm{km}$ and $\Delta v=61.7\mathrm{m}/\min$. We let ${\beta }_i=1$. Figure 5b shows the estimated target trajectories obtained by the EKF, UKF, CODEKF and PEKF-VB for a single run.

Based on 1000 Monte Carlo simulations, the RMSE comparison of EKF, UKF, CODEKF and PEKF-VB is illustrated in Figure 6, where the number of iterations for PEKF-VB is 5.

• As we expected, with a fixed sensor trajectory, PEKF-VB has the best target observability from sensor measurements and leads to a better RMSE performance than EKF and UKF. Under the VB framework, the variational distribution approaches the real posterior PDF through the iteration of the proximal filter.
• The RMSE performance of CODEKF is also better than those of EKF and UKF because, for CODEKF, the Jacobian matrix of Equation (37) in each iteration is updated to minimize the COD. However, the RMSE performance of CODEKF is slightly worse than that of PEKF-VB.
• In the first few scans, the performance of the four filters are comparable. This is because, in this bearing-only tracking problem, the accumulative measurements in these scans do not provide enough information to the four filters. The performance of CODEKF and of PEKF-VB suffers when measurement data is very limited. As more measurements accumulate both CODEKF and PEFK-VB extract more information via the iteration process, resulting in superior performance.

Figure 7 shows the metric values versus the number of iterations for PEKF-VB and CODEKF. The KLD curve is close to zero after the fifth iteration. The enlarged plots marked from the sixth to the tenth iteration shows that PEKF-VB converge faster than CODEKF.

Example 3: A Strongly nonlinear filtering problem. For further verifying our proposed method, we run EKF, UKF, CODEDK and PEKF-VB on the benchmark nonlinear problem in [32,33,34]

$$\begin{array}{cc}\hfill x_k=& \frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^2}+8\cos \left(1.2k\right)+{\nu }_{k-1},\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill z_k=& \frac{{x_k}^2}{20}+{\omega }_k,\hfill \end{array}$$

(41)

where ${\nu }_{k-1}$ and ${\omega }_k$ are zero mean Gaussian noise with variances $Q_{k-1}$ and $R_k$, respectively. We let $Q_k=0.0001$, $R_k=1$ and scan period $T=1$. The simulation results are given in Figure 8, from which we can see that CODEKF and PEKF-VB have very similar results and outperform EKF and UKF. This is because that local linearization adopted by EKF-based filter are not a sufficient description of the nonlinear nature of this example [34], while the VB iteration can make use of measurement information as much as possible.

6. Conclusions

We have developed a proximal iterative nonlinear filter, in which the expectation of the posterior PDF is approximated by a parameterised variational distribution that is iteratively optimized in the VB framework. A weighted KL divergence is adopted as a proximal term in the iteration to ensure the convergence can be achieved with a tight bound. The simulation results show that the proposed algorithm is better than several existing algorithms in terms of estimation accuracy at the cost of increased computational burden.