Iterative Truncated Unscented Particle Filter

Abstract

The particle filter method is a basic tool for inference on nonlinear partially observed Markov process models. Recently, it has been applied to solve constrained nonlinear filtering problems. Incorporating constraints could improve the state estimation performance compared to unconstrained state estimation. This paper introduces an iterative truncated unscented particle filter, which provides a state estimation method with inequality constraints. In this method, the proposal distribution is generated by an iterative unscented Kalman filter that is supplemented with a designed truncation method to satisfy the constraints. The detailed iterative unscented Kalman filter and truncation method is provided and incorporated into the particle filter framework. Experimental results show that the proposed algorithm is superior to other similar algorithms.

1. Introduction

State estimation of a nonlinear dynamic system is of great importance in a variety of fields including computer vision, target tracking, machine learning, geophysics, bioengineering, control systems and econometrics [1,2,3]. The most popular approach for state estimation is the Bayesian filtering method, within which the posterior probability density function (PDF) is recursively calculated. However, it is difficult to obtain a closed-form solution for Bayesian filtering problems. Thus, researchers must search approximate solutions for these state estimation problems.

The particle filter (PF), or sequential Monte Carlo method, is an efficient solution to nonlinear filtering problems which has been applied in various fields including virtual reality [4], target tracking [5], robotics [6], econometrics [7], computer vision [8,9], etc. The basic idea of the PF is to calculate the posterior PDF using a finite set of particles and corresponding weights. The particle set defines the shape of the PDF of the state. A crucial issue of the PF is to design an appropriate proposal distribution for drawing particles. Much work has been done for designing proposal distributions. Doucet [10] used the extended Kalman filter (EKF) to build a Gaussian distribution for drawing particles. The mean and covariance estimates are obtained using the EKF prediction and update equations. As EKF is based on the linearization of nonlinear models, the estimation accuracy cannot be assured. In [10], the unscented Kalman filter (UKF) is used to build the proposal distribution, which avoids the linearization operation through propagating a set of deterministically chosen sample points. The resulting algorithm is named an unscented particle filter (UPF) which is successfully applied to visual tracking [9], and image Jacobian matrix estimation [6]. The iterated EKF is used for designing the proposal distribution in [11] providing better estimation accuracy compared to UPF. Following the idea of [11], the authors in [12] proposed to generate the proposal distribution using an iterative version of UKF (IUKF) which makes use of both statistical and analytical linearization techniques in different stages of the filtering process. In order to obtain better estimation accuracy, it is of great importance to make full use of the current observations which provide valuable information on the states. In [13], the authors combine the iterative EKF (IEKF) and the UKF to build the proposal distribution in the particle filtering framework. The proposed two-stage particle filter updates each particle sequentially using the IEKF and UKF, which makes full use of current observations.

Most of the above mentioned particle filters deal with unconstrained state estimation. The systems are described by the state and measurement equations accompanied by the PDFs of the uncertainties. However, in many practical applications, the system nonlinearity is always limited by the sub-regions of the state space when the system is subject to some constraints. Constrained state estimation arise from physical restrictions, natural phenomena, or elicited qualitative knowledge about the system of interest [14]. For example, in visual object tracking, when an object moves with moderate speed in a video sequence, its posotion is limited in the frame space. It is difficult to incorporate such constraints into the system models. In order to solve the constrained nonlinear state estimation problems, Fernandez et al. [15] devise a truncated unscented Kalman filter (TUKF) to approximate the first two moments of the posterior PDF. In [16], the authors proposed a mixture truncated unscented Kalman filter (MTUKF) which approximates the posterior PDF as a Gaussian mixture rather than a single Gaussian. However, KF-type filters may not be applicable when the state distribution becomes highly non-Gaussian due to truncated marginal probability density function.

Constrained particle filters [17] are known to provide an effective solution to constrained state estimation. Straka et al. [18] proposed to design the proposal distribution using the TUKF in the particle filter framework for generally nonlinear inequality-constrained filtering problems. Li et al. [19] proposed a novel auxiliary truncated particle filter (ATPF) for bearing-only tracking. In ATPF, the proposal distribution combines the prior PDF and a modified PDF which consider not only current observations but also spatio-temporal information. Zhang et al. [20] extends the ATPF to a multiple model particle filter for bearings-only target tracking thus exhibiting better performance. In [21], the authors present a truncated unscented particle filter which uses the TUKF to generate the proposal distribution in order to incorporate both observations and constraint information. Zhao et al. [22] proposed to impose constraints on prior particles, posterior particles and state estimates. The proposed constrained particle filter provides better physical interpretation and involves no restrictive assumptions on the distributions. In [23], the authors introduced a controlled particle filter in which the proposal distribution is generated using optimal control. The proposed method can achieve good estimation performance with fewer particles. Huang et al. [24] proposed a Rao-Blackwellized particle filter with optimal proposal distribution. This method could enforces the inequality constraints for all outcomes of the filtering distribution. It exploits the Gaussian linear sub-structure for analytic integration and a marginalization method to reduce the dimension of the state variables which ensure its computational efficiency.

In this paper, an iterative truncated particle filter is developed by using the iterative unscented Kalman filter (IUKF) to generate a proposal distribution for efficient sampling for state estimation with general inequality constraints. A truncation step is inserted into the iterated unscented Kalman filter in order to make the approximated PDF accommodate to the constraints. The proposed method is named as iterative truncated unscented particle filter (ITUPF).

The paper is organized as follows: Preliminary knowledge about constrained state estimation and particle filtering is presented in Section 2. In Section 3, the ITUPF is introduced after the details of the IUKF and the truncation method. Section 4 shows simulation results and the last section draws conclusion.

2. Preliminary Knowledge

2.1. State Estimation with Inequality Constraints

Let us consider a state-space model defined by the following equations:

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

(1)

$$y_k=h_k\left(x_k\right)+v_k$$

(2)

where $x_k\in R^{n_x}$ represents the system state at time k, $y_k$ denotes observation, $f_k$ and $h_k$ represent the system and measurement functions, respectively. The process noise is $u_k$ and measurement noise is $v_k$. Both of them are described by known probability density functions $p\left(u_k\right)$ and $p\left(v_k\right)$. The initial state $x_0$ has the prior density $p\left(x_0\right)$.

The aim of Bayesian filtering is to find the state $x_k$ based on the history of observations $Y^k=\{y_1,y_2,\dots ,y_k\}$ up to time k. According to the Bayesian approach, the posterior density $p\left(x_k\right|Y^k)$ provides the solution of the filtering problem.

In order to consider the information besides the system description given by (1) and (2), we use the following general constraint:

$$a_k\le {\mathsf{\Psi }}_k\left(x_k\right)\le b_k$$

(3)

where ${\mathsf{\Psi }}_k$ is the constraint function at time k, and the inequality holds for all the elements of the vectors. The aim of the constrained state estimation is to find the PDF $p\left(x_k\right|Y^k)$ considering the system Equations (1) and (2), and constraint (3). We use the notation ${\mathsf{\Psi }}_k$ to represent the states that satisfy the inequality of (3), that is:

$${\mathsf{\Psi }}_k=\{x_k:a_k\le {\mathsf{\Psi }}_k\left(x_k\right)\le b_k\}$$

(4)

Then, constrained state estimation is given by the constrained PDF $p\left(x_k\right|Y^k,x_k\in {\mathsf{\Psi }}_k)$.

2.2. Particle Filter

In the Bayesian context, the PDF $p\left(x_k\right|Y^k)$ can be obtained through two steps, i.e., prediction and update steps. The aim of prediction step is calculating the predictive density $p\left(x_k\right|Y^{k-1})$ as follows:

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

(5)

The update step computes the required $p\left(x_k\right|Y^k)$ based on the predictive density and new observation $y_k$ by:

$$p\left(x_k\right|Y^k)=\frac{p\left(y_k\right|x_k)p\left(x_k\right|Y^{k-1})}{\int p\left(y_k\right|x_k)p\left(x_k\right|Y^{k-1})d{x}_k}$$

(6)

The above two equations constitute the solution of the Bayesian estimation problem, but the integrals are intractable for most nonlinear non-Gaussian models. We have to resort to approximation method to obtain a solution.

The PF method can approximate the posterior PDF using a set of particles ${\left\{x_k^{\left(i\right)}\right\}}_{i=1}^N$ with corresponding weights ${\left\{w_k^{\left(i\right)}\right\}}_{i=1}^N$ as

$$p\left(x_k\right|Y^k)=\sum _{i=1}^N{w}_k^{\left(i\right)}\delta (x_k-x_k^{\left(i\right)}).$$

(7)

In this equation, $\delta$ denotes the Dirac delta function and N the number of particles. The particles are intuitively drawn from the posterior PDF in order to better approximate it. However, it is impossible to directly draw particles from $p\left(x_k\right|Y^k)$. So, importance sampling is usually adopted to draw particles from a so-called importance or proposal distribution $q\left(x_k\right|x_{k-1},y_k)$. The weight of each particle $x_k^{\left(i\right)}$ is given by:

$$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}\frac{p(y_k|x_k^{\left(i\right)})·p(x_k^{\left(i\right)}|x_{k-1}^{\left(i\right)})}{q(x_k^{\left(i\right)}|x_{k-1}^{\left(i\right)},y_k)}$$

(8)

3. Iterative Truncated Particle Filter

3.1. Iterative Truncated UKF

3.1.1. Iterative Unscented Kalman Filter

The iterative UKF is an improved UKF where the Newton-Raphson iteration is used at the update step. It is also rooted in the unscented transformation. A set of deterministically chosen sigma-points are used to compute the first two moments of a $n_x$-dimensional random variable $x_k$. The sigma-points ${\chi }_i$ with corresponding weights ${\varpi }_i$ are selected based on the first two moments ${\widehat{x}}_k$ and $P_k$ as follows:

$${\chi }_0={\widehat{x}}_k,{\varpi }_0=\frac{\lambda }{n_x+\lambda }$$

(9)

$${\chi }_i={\widehat{x}}_k+{(\sqrt{(n_x+\lambda )P_k})}_i,{\varpi }_i=\frac{1}{2(n_x+\lambda )}$$

(10)

$${\chi }_{n_x+i}={\widehat{x}}_k-{(\sqrt{(n_x+\lambda )P_k})}_i,{\varpi }_{n_x+i}=\frac{1}{2(n_x+\lambda )}$$

(11)

where $i=1,2,\dots ,n_x$, $\lambda$ is the scaling parameter, ${\left(\sqrt{(n_x+\lambda )P_k}\right)}_i$ represents the i-th column of the matrix square root of $(n_x+\lambda )P_k$.

At time $k-1$, suppose the PDF $p\left(x_{k-1}\right|Y^{k-1})$ is determined by the first two moments ${\widehat{x}}_{k-1}$ and $P_{k-1}$. In order to obtain the moments estimation of time k, we first transfer the sigma-points through the system transition model.

$${\chi }_{k|k-1}=f_{k-1}\left({\chi }_{k-1}\right)$$

(12)

Then, the predicted mean is obtained based on the transferred sigma-points as follows:

$${\widehat{x}}_{k|k-1}=\sum _{i=0}^{2n_x}{\varpi }_{k-1}^{\left(i\right)}{\chi }_{k|k-1}^{\left(i\right)}$$

(13)

and the predicted covariance matrix is

$$P_{k|k-1}=\sum _{i=0}^{2n_x}{\varpi }_{k-1}^{\left(i\right)}[{\chi }_{k|k-1}^{\left(i\right)}-{\widehat{x}}_{k|k-1}]{[{\chi }_{k|k-1}^{\left(i\right)}-{\widehat{x}}_{k|k-1}]}^T$$

(14)

The next step is update step in which the predicted estimates will be updated using the latest observation $y_k$. According to the Bayesian theorem, the PDF $p\left(x_k\right|Y^k)$ can be written as:

$$\begin{array}{cc}\hfill p\left(x_k\right|Y^k)& =p\left(x_k\right|y_k,Y^{k-1})\hfill \\ \hfill & =\frac{p\left(y_k\right|x_k,Y^{k-1})·p\left(x_k\right|Y^{k-1})}{p\left(y_k\right|Y^{k-1})}\hfill \\ \hfill & =\frac{p\left(y_k\right|x_k)·p\left(x_k\right|Y^{k-1})}{p\left(y_k\right|Y^{k-1})},\hfill \end{array}$$

(15)

where the denominator $p(y_k|Y^{k-1})$ is a normalizing constant. We assume that all the PDFs are Gaussian, consequently, $p(x_k|Y^k)$ can be rewritten as:

$$\begin{array}{cc}\hfill p\left(x_k\right|Y^k)& \approx \frac{1}{c}N(y_k;h_k\left(x_k\right),v_k)·N(x_k;{\widehat{x}}_{k|k-1},P_{k|k-1})\hfill \\ \hfill & \propto exp\{-\frac{1}{2}({[y_k-h_k\left(x_k\right)]}^T·{\left(M_k^v\right)}^{-1}·\hfill \\ \hfill & [y_k-h_k\left(x_k\right)]+{[{\widehat{x}}_{k|k-1}-x_k]}^T·P_{k|k-1}^{-1}·\hfill \\ \hfill & [{\widehat{x}}_{k|k-1}-x_k]\left)\right\}\hfill \end{array}$$

(16)

where $M_k^v$ is the covariance matrix of the measurement noise. The minimum mean square error (MMSE) estimate is equivalent to the maximum a posteriori probability (MAP) estimate which is obtained through maximizing $p\left(x_k\right|Y^k)$. Taking logarithms on both sides of Equation (16), the following formula can be obtained:

$$\begin{array}{cc}\hfill log\left(p\left(x_k\right|Y^k)\right)& =-\frac{1}{2}({[y_k-h_k\left(x_k\right)]}^T·{\left(M_k^v\right)}^{-1}·\hfill \\ \hfill & [y_k-h_k\left(x_k\right)]+{[{\widehat{x}}_{k|k-1}-x_k]}^T·P_{k|k-1}^{-1}·\hfill \\ \hfill & [{\widehat{x}}_{k|k-1}-x_k])\hfill \end{array}$$

(17)

Let $\ell \left(x\right)=-log\left(p\left(x_k\right|Y^k)\right)$, consequently, maximizing $p\left(x_k\right|Y^k)$ is equivalent to minimizing $\ell \left(x\right)$. The Newton-Raphson iteration method can be used to find the minimum of $\ell \left(x\right)$ starting from ${\overline{x}}_0={\widehat{x}}_{k|k-1}$. At iteration j, $\ell \left(x\right)$ can be expanded at ${\overline{x}}_{j-1}$ to a second order Taylor series approximation:

$$\begin{array}{cc}\hfill \ell \left(x\right)& =\ell \left({\overline{x}}_{j-1}\right)+{(x-{\overline{x}}_{j-1})}^T\frac{\partial \ell \left({\overline{x}}_{j-1}\right)}{\partial x}\hfill \\ \hfill & +\frac{1}{2}{(x-{\overline{x}}_{j-1})}^T\frac{{\partial }^2\ell \left({\overline{x}}_{j-1}\right)}{\partial x^2}(x-{\overline{x}}_{j-1})\hfill \end{array}$$

(18)

In this equation, $\frac{\partial \ell }{\partial x}$ is the Jacobian while $\frac{{\partial }^2\ell }{\partial x^2}$ is the Hessian of $\ell \left(x\right)$. Differentiating Equation (18), and equating the differentiation to 0, we can obtain,

$$\frac{\partial \ell \left({\overline{x}}_{j-1}\right)}{\partial x}+\frac{{\partial }^2\ell \left({\overline{x}}_{j-1}\right)}{\partial x^2}(x-{\overline{x}}_{j-1})=0$$

(19)

Then, the estimate ${\overline{x}}_j$ can be computed by:

$${\overline{x}}_j={\overline{x}}_{j-1}-{\left(\frac{{\partial }^2\ell \left({\overline{x}}_{j-1}\right)}{\partial x^2}\right)}^{-1}·\frac{\partial \ell \left({\overline{x}}_{j-1}\right)}{\partial x}$$

(20)

where the Jacobian and Hessian in this equation can be directly computed according to Equation (17) as:

$$\begin{array}{cc}\hfill \frac{\partial \ell \left({\overline{x}}_{j-1}\right)}{\partial x}& =P_{k|k-1}^{-1}·({\overline{x}}_{j-1}-{\widehat{x}}_{k|k-1})-\hfill \\ \hfill & {\left(\frac{\partial h\left({\overline{x}}_{j-1}\right)}{\partial x}\right)}^T·{\left(M_k^v\right)}^{-1}·(y_k-h_k\left({\overline{x}}_{j-1}\right))\hfill \end{array}$$

(21)

$$\frac{{\partial }^2\ell \left({\overline{x}}_{j-1}\right)}{\partial x^2}=P_{k|k-1}^{-1}+{\left(\frac{\partial h\left({\overline{x}}_{j-1}\right)}{\partial x}\right)}^T·{\left(M_k^v\right)}^{-1}·\frac{\partial h\left({\overline{x}}_{j-1}\right)}{\partial x},$$

(22)

where $J\left({\overline{x}}_{j-1}\right)=\frac{\partial h\left({\overline{x}}_{j-1}\right)}{\partial x}$ is the Jacobian of $h_k\left(x\right)$ computed at ${\overline{x}}_{j-1}$. Here, we use a simplified form $J_j$. By substituting (21) and (22) into (20), the iterative formula can be obtained as follows:

$$\begin{array}{cc}\hfill {\overline{x}}_j& ={\overline{x}}_{j-1}-{\left(P_{k|k-1}^{-1}+J_j^T{\left(M_k^v\right)}^{-1}{J}_j\right)}^{-1}·\hfill \\ \hfill & \left(P_{k|k-1}^{-1}({\overline{x}}_{j-1}-{\widehat{x}}_{k|k-1})-J_j^T{\left(M_k^v\right)}^{-1}(y_k-h_k\left({\overline{x}}_{j-1}\right))\right)\hfill \end{array}$$

(23)

The iteration number is usually empirically set as a fixed value L, i.e., $j=1,2,\dots ,L$. Thus, after all the iterations are finished, the final estimate is the value of the last iteration, i.e., ${\widehat{x}}_k={\overline{x}}_L$, and the covariance estimate is:

$$P_k={(P_{k|k-1}^{-1}+J_j^T{(M_k^v)}^{-1}{J}_j)}^{-1}$$

(24)

3.1.2. Truncation of the PDF

Let us use $p_t(x_k|Y^k)$ to represent the truncated PDF which can be written as

$$p_t\left(x_k\right|Y^k)=\left\{\begin{array}{c}\hfill \frac{p\left(x_k\right|Y^k)}{{\int }_{{\mathsf{\Psi }}_k}p\left(x_k\right|Y^k)d{x}_k},if{x}_k\in {\mathsf{\Psi }}_k\\ \hfill 0,if{x}_k\notin {\mathsf{\Psi }}_k\end{array}\right.$$

(25)

This equation shows a closed-form description of $x_k$ using the form of a PDF under the constraint ${\mathsf{\Psi }}_k$. This truncated PDF will be used as the proposal distribution for drawing samples. As it is hard to find an easy-to-sample PDF, we will approximate this truncated PDF by a Gaussian PDF which is determined by its first two moments: ${\widehat{x}}_k^t$ and $P_k^t$.

$$p_t(x_k|Y^k)\approx N(x_k;{\widehat{x}}_k^t,P_k^t),$$

(26)

where the mean and covariance are obtained using the following formulas:

$${\widehat{x}}_k^t=\int x_k{p}_t\left(x_k\right|Y^k)d{x}_k$$

(27)

$$P_k^t=\int (x_k-{\widehat{x}}_k^t){(x_k-{\widehat{x}}_k^t)}^T{p}_t\left(x_k\right|Y^k)d{x}_k$$

(28)

As the integrals in the above two equations are difficult to calculate, we can use the importance sampling based approximation method to find approximate values for ${\widehat{x}}_k^t$ and $P_k^t$. Suppose a set of $N_o$ samples $x_k^{\left(i\right)}$, $i=1,2,\dots ,N_o$, are drawn from an importance distribution $\rho \left(x_k\right)$. $\rho \left(x_k\right)$ is affected by only one condition, that is, $\rho \left(x_k\right)\ne 0$ for any $x_k\in {\mathsf{\Psi }}_k$. It is intuitive that the the probability mass of $\rho \left(x_k\right)$ within the constrained region should be as close as possible to one in order to obtain higher accuracy. Among the $N_o$ samples, $N_o^t$ samples fall in the constrained region ${\mathsf{\Psi }}_k$. This subset of samples is denoted as $x_k^{t,\left(j\right)}$, $j=1,2,\dots ,N_o^t$, which are used to approximate the mean and covariance of the truncated PDF as

$${\widehat{x}}_k^t=\frac{1}{{\sum }_{j=1}^{N_o^t}{w}_k^{\left(j\right)}}\sum _{j=1}^{N_o^t}{x}_k^{t,\left(j\right)}{w}_k^{\left(j\right)}$$

(29)

$$P_k^t=\frac{1}{{\sum }_{j=1}^{N_o^t}{w}_k^{\left(j\right)}}\sum _{j=1}^{N_o^t}(x_k^{t,\left(j\right)}-{\widehat{x}}_k^t){(x_k^{t,\left(j\right)}-{\widehat{x}}_k^t)}^T{w}_k^{\left(j\right)}$$

(30)

The sample weight is $w_k^{\left(j\right)}=\frac{p\left(x_k^{t,\left(j\right)}\right)}{\rho \left(x_k^{t,\left(j\right)}\right)}$. The truncated PDF will be denoted as:

$$\{{\widehat{x}}_k^t,P_k^t\}=Tr({\widehat{x}}_k,P_k,{\mathsf{\Psi }}_k)$$

(31)

This truncated PDF will be used as proposal distribution in PF.

3.2. Iterative Truncated UPF

In this section, we will detail the iterative truncated unscented particle filter (ITUPF). The truncated iterative unscented Kalman filter presented in the previous section is used to generate the proposal distribution. Suppose we have obtained the particle set, at time $k-1$, ${\{x_{k-1}^{\left(i\right)},w_{k-1}^{\left(i\right)}\}}_{i=1}^N$. The mean and covariance estimates can be computed by

$${\widehat{x}}_{k-1}=\frac{1}{N}\sum _{i=1}^N{w}_{k-1}^{\left(i\right)}{x}_{k-1}^{\left(i\right)}$$

(32)

$$P_{k-1}=\sum _{i=1}^N\left[x_{k-1}^{\left(i\right)}-{\widehat{x}}_{k-1}\right]{\left[x_{k-1}^{\left(i\right)}-{\widehat{x}}_{k-1}\right]}^T{w}_{k-1}^{\left(i\right)}$$

(33)

Then, the IUKF equations are used to calculate the mean and covariance estimates through the prediction and update steps (23) and (24). Afterwards, these two estimate are used to compute the constrained mean and covariance according to (31). These two estimates provide the proposal distribution for drawing particles, i.e., $N(x_k;{\widehat{x}}_k^t,P_k^t)$. The ITUPF is summarized as in Algorithm 1. The prediction step of IUKF is denoted as $IUK{F}_{pred}$ and the update step is denoted as $IUK{F}_{upd}$.

Algorithm 1: Iterative Truncated Unscented Particle Filter

Step 1: Calculate mean and covariance: ${\widehat{x}}_{k-1}$ and $P_{k-1}$ according to Equations (32) and (33); Step 2: Calculate the first two moment estimations using the IUKF as: $$\{{\widehat{x}}_{k|k-1},P_{k|k-1}\}=IUK{F}_{pred}({\widehat{x}}_{k-1},P_{k-1})$$ (34) $$\{{\widehat{x}}_k,P_k\}=IUK{F}_{upd}({\widehat{x}}_{k|k-1},P_{k|k-1},y_k)$$ (35)   Step 3: Compute the constrained mean ${\widehat{x}}_k^t$ and covariance $P_k^t$ of the truncated PDF according to (31);   Step 4: Draw particles from the truncated proposal distribution: $N(x_k;{\widehat{x}}_k^t,P_k^t)$ until for N particles the condition $x_k\in {\mathsf{\Psi }}_k$ holds;   Step 5: Compute the particle weights: $$w_k^{\left(i\right)}=\frac{p(y_k|x_k^{\left(i\right)})N(x_k^{\left(i\right)};x_{k|k-1},P_{k|k-1}))}{N(x_k^{\left(i\right)};{\widehat{x}}_k^t,P_k^t)}$$ (36)     Step 6: Normalize the particle weights to ensure ${\sum }_{i=1}^N{w}_k^{\left(i\right)}=1$;     Step 7: Using the particle set ${\{x_k^{\left(i\right)},w_k^{\left(i\right)}\}}_{i=1}^N$ to calculate the state estimates, then continue with step 1;

4. Simulation Results

In this section, in order to demonstrate the improved performance of the proposed algorithm, we conduct three simulation experiments.

4.1. Univariate Nonstationary Growth Model I

For the first simulation, we considered a univariate nonstationary growth model (UNGM) [15]. The nonlinear system and measurement models are as follows:

$$x_k=\frac{x_{k-1}}{2}+25\frac{x_{k-1}}{1+x_{k-1}^2}+8cos\left(1.2k\right)+u_k$$

(37)

$$y_k=\frac{x_k^3}{25}+v_k$$

(38)

where the system noise $u_k$ is non-Gaussian with Gamma distribution $\gamma (3,2)$, while the measurement noise is Gaussian with zero mean and variance 0.01. The particle number was set to $N=200$ for all the algorithms, the total time step was $T=60$, and 100 Monte Carlo runs were performed to calculate the root mean square error (RMSE) defined as follows:

$$RMSE={\left(\frac{1}{T}\sum _{k=1}^T({\widehat{x}}_k-x_k{)}^2\right)}^{1/2}$$

(39)

The constraint was defined as: $-25\le x_k\le 25$. The proposed method was compared with the generic PF, UPF, IUPF, MCMC-UPF and TUPF. Figure 1 shows the estimated state of different particle filters in a single Monte Carlo run and the data generated in this simulation are shown in Figure 2.

Table 1. RSME and computational time of different particle filters.

Filters	RMSE Mean	RMSE Variance	Average Computational Time
PF	1.2907	0.5968	0.3768
UPF	2.389	13.6974	0.9644
IUPF	2.3624	0.3168	1.6903
UPF-MCMC	2.4064	12.8904	2.2442
TUPF	0.9382	0.01955	0.9364
ITUPF	0.9272	0.01938	1.5063

gives the means and variances of the RMSE generated by different particle filters after 100 Monte Carlo runs. It shows that UPF, IUPF and UPF-MCMC generated higher RMSE than PF and TUPF, while the ITUPF gave the lowest RMSE equal to 0.9272. Figure 3 shows the RMSEs of different algorithms after 100 Monte Carlo runs at different time steps, which clearly demonstrates that the ITUPF gave the best and most stable estimation results compared to the other methods. The processing times of different methods are also shown in Table 1. As the iterative TUPF needed more iterations to achieve better estimation results, it needed more computational time than TUPF.

4.2. Univariate Nonstationary Growth Model II

For the second simulation, we used another dynamical system described by the following model:

$$x_k=1+sin\left(\frac{1}{25}\pi (k-1)\right)+\frac{1}{2}{x}_{k-1}+u_k$$

(40)

$$y_k=\frac{1}{20}{x}_k^3+v_k$$

(41)

where the system noise had the same distribution as in the previous experiment. The measurement noise was also Gaussian with zero mean and variance 0.0001. The total time step and Monte Carlo runs were the same as in the previous experiment. We used 100 particles for all the filters in this experiment. The RMSE was calculated to compare the performance of different particle filters. The constraint was defined as: $0\le x_k\le 10$. Figure 4 shows the estimated and true states in a single Monte Carlo run with system data shown in Figure 5. The red curve denotes the state estimation results generated by the proposed ITUPF. Compared to the other estimation algorithms, ITUPF provided state estimates much closer to the true states.

The RMSE means and variances of different filters are shown in

Table 2. RMSE and computational time of different particle filters.

Filters	RMSE Mean	RMSE Variance	Average Computational Time
PF	0.6151	0.0454	0.1969
UPF	0.2957	0.0999	0.5026
IUPF	0.2884	0.0976	0.7978
UPF-MCMC	0.3014	0.1107	1.1462
TUPF	0.1240	0.00173	0.4964
ITUPF	0.1178	0.00102	0.7866

. PF gave the highest RMSE followed by UPF-MCMC, UPF, IUPF and TUPF, and ITUPF gave the lowest RMSE as expected. As for the computational time, we could draw the same conclusion as in the previous experiment.

We also include the averaged RMSEs of each time step over the 100 Monte Carlo runs in Figure 6. The RMSEs of PF, UPF, IUPF and UPF-MCMC were obviously strongly fluctuating over time while TUPF and ITUPF were much more stable. The overall performance of our proposed method was better than the other filters.

4.3. Tracking an Vehicle Moving along a Circular Road

In order to further demonstrate the performance of the proposed ITUPF, we considered a problem of tracking a vehicle. The road was defined by two arcs with the radii $r_1=100$ and $r_2=96$ meters (m) while the center was at the origin of the Cartesian coordinate system. We supposed the vehicle kept angular velocity $\omega$ within $\omega \in [2.85,5.7]$ degrees per second. In Figure 7, we show the outline of the the road and an example trajectory of the vehicle.

The initial position of the vehicle was ${[98,0]}^T$ determining the position in the x and y directions. The vehicle was supposed to follow the continuous white noise acceleration motion model. We modeled the state of the vehicle as $x_k={[x_{k,1},x_{k,2},x_{k,3},x_{k,4}]}^T$, where $x_{k,1}$ and $x_{k,2}$ are the positions in the x and y directions, while $x_{k,3}$ and $x_{k,4}$ represent the velocities in the x and y directions. The state evolved according to the following transition model:

$$x_k=\left(\begin{array}{cccc}1& T& 0& 0\\ 0& 0& 1& T\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right)x_{k-1}+\left(\begin{array}{cc}0.5T^2& 0\\ 0& 0.5T^2\\ T& 0\\ 0& T\end{array}\right)+u_k$$

(42)

where $T=1$ is the sampling period and $u_k$ is a Gaussian zero-mean system noise with covariance matrix $Q=[10;01]$. The vehicle traveled for $K=20$ time steps, that is $k=0,1,2,\dots ,20$. A sensor was used to track the vehicle by measuring the range and bearings with sampling interval T. The measurement function was therefore defined as follows:

$$y_k=\left(\begin{array}{c}\sqrt{x_{k,1}^2+x_{k,2}^2}\\ arctan\left(\frac{x_{k,2}}{x_{k,1}}\right)\end{array}\right)+v_k$$

(43)

where $v_k$ is a Gaussian zero-mean measurement noise with covariance $R=diag\left([8,{10}^{-3}]\right)$. When $k=0$, the initial condition for the filters was:

$$p\left(x_0\right)=N\left(x_0:\left(\begin{array}{c}98\\ 0\\ 0\\ 10\end{array}\right),\left(\begin{array}{cccc}10& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 10& 0\\ 0& 0& 0& 1\end{array}\right)\right)$$

(44)

The constraint with respect to (3) is defined as $r_2\le \sqrt{x_{k,1}^2+x_{k,2}^2}\le r_1$. In this simulation experiment, we compare the TUPF and ITUPF. The particle number is set to $N=1000$ for the two filters. The number of Monte Carlo simulations is set to $MC=1000$. The performance of the filters is measured using the mean square error (MSE) which is defined as follows:

$$MSE=\frac{{\sum }_{m=1}^{MC}{\sum }_{k=1}^K{\sum }_{i=1}^{n_x}(x_{k,i}^{\left(m\right)}-{\widehat{x}}_{k,i}^{\left(m\right)}{)}^2}{MC(K+1)}$$

(45)

where $n_x$ is the dimension of the state variable $x_k$, $x_{k,i}^{\left(m\right)}$ is the i-th component of the true state in the m-th Monte Carlo run at time step k, and ${\widehat{x}}_{k,i}^{\left(m\right)}$ is the estimated state component. In this simulation, we only considered the position of the object when computing the mean square error (MSE).

Figure 8 shows the true trajectory and the estimated trajectories of both ITUPF and TUPF. It is clear that the trajectory estimated by ITUPF was much closer to the true trajectory than that of TUPF. The MSEs are summarized in

Table 3. Mean square error (MSE) and computational time of the ITUPF and TUPF.

Filters	MSE	Average Computational Time
TUPF	3.6721	4.84
ITUPF	3.2655	7.47

. We can see that the MSE of TUPF was 3.6721 while that of ITUPF was 3.2655. This is caused by the fact that the proposal distribution generated by the iterative truncated UKF could accommodate the constrained region better than TUPF. The iteration operation could reduce the estimation error at the expense of higher computational load. The average computational cost of ITUPF was 7.47 which was much higher than that of TUPF.

4.4. Particle Number Influence

In this section, we discuss the influence of particle numbers on the estimation performance. Intuitively, the performance of a particle filter directly depends on the particle number. The more particles, the better the estimation accuracy. We used Model II in Section 4.2 with different particle numbers to do this experiment. Each particle number corresponds to 100 Monte Carlo runs. The average RMSEs are calculated and shown in Figure 9. The generic PF showed an evident RMSE decrease as the particle number grew from 100 to 3000, which demonstrates that its performance highly denpended on the particle number. UPF, IUPF, and UPF-MCMC also showed a similar trend to the generic PF. However, such a trend was less evident than in the generic PF. When it comes to TUPF and proposed ITUPF, we can see from this figure that the RMSEs of the two methods decreased evidently when the particle number grew from 100 to 500. However, this downward trend became less apparent as the particle number continued to rise. The RMSEs of ITUPF was consistently lower than the other filters.

4.5. Discussion

In this section, we have assessed the performance of the proposed particle filter through simulation experiments. As shown in the figures and tables, the proposed ITUPF shows improved performance compared to other similar particle filters. The iteration operation in the update step can make use of the current observation $y_k$ at each iteration step (see Equation (23)). Meanwhile, the injection of the truncation step makes the particle filter well accommodate the inequality constraints imposed to the nonlinear systems, thus improving the performance of the truncated particle filter in constrained state estimation. In addition, simulation results also demonstrate that ITUPF is less sensitive to the variation of particle number. However, the computational cost of the ITUPF increases as the number of iteration steps L grows.

5. Conclusions

We proposed an iterative truncated unscented particle filter for constrained nonlinear state estimation. It uses a modified iterated UKF to construct the proposal distribution. A truncation step is injected into the IUKF in order to make the obtained PDF satisfy the constraints. This leads to an efficient sampling distribution which exploits the current observation and take into account inequality constraints. We use two nonlinear system models and a vehicle tracking model to carry out the simulation experiments showing that the proposed particle filter is superior to several other methods. In our future work, we will apply the ITUPF to bearing-only maneuvering target tracking and consider to reduce the computational cost of ITUPF.