Constrained Cubature Particle Filter for Vehicle Navigation

Abstract

In vehicle navigation, it is quite common that the dynamic system is subject to various constraints, which increases the difficulty in nonlinear filtering. To address this issue, this paper presents a new constrained cubature particle filter (CCPF) for vehicle navigation. Firstly, state constraints are incorporated in the importance sampling process of the traditional cubature particle filter to enhance the accuracy of the importance density function. Subsequently, the Euclidean distance is employed to optimize the resampling process by adjusting particle weights to avoid particle degradation. Further, the convergence of the proposed CCPF is also rigorously proved, showing that the posterior probability function is converged when the particle number N → ∞. Our experimental results and the results of a comparative analysis regarding GNSS/DR (Global Navigation Satellite System/Dead Reckoning)-integrated vehicle navigation demonstrate that the proposed CCPF can effectively estimate system state under constrained conditions, leading to higher estimation accuracy than the traditional particle filter and cubature particle filter.

1. Introduction

As the “eye” of a vehicle, the navigation system provides navigation information for vehicle maneuvers to reach their target. Since navigation systems commonly involve nonlinear structural characteristics and dynamics, nonlinear filtering is an important means for navigation computations [1,2,3]. In practice, vehicle navigation systems are subject to constraints and non-Gaussian uncertainties, making nonlinear filter even more challenging [4,5,6].

Currently, the extended Kalman filter (EKF), polynomial filter, unscented Kalman filter (UKF), and cubature Kalman filter (CKF) are commonly used for nonlinear state estimation. The extended Kalman filter involves a system linearization error and requires the calculation of the Jacobian matrix [7,8]. The polynomial filter is sensitive to outliers and requires a large amount of computing for large-scale signal processing [9]. The UKF and CKF select typical sampling points according to prior state estimation to apply the unscented transformation [10,11] and cubature rules [12,13] to obtain sample points, respectively. Subsequently, the obtained sampling points are weighted to acquire system state estimations. Compared to the UKF, the CKF is superior in both estimation accuracy and stability for high-dimensional systems, since the Gaussian-weighted integrals are calculated through third-degree spherical-radial cubature [12,13,14]. In general, these improved nonlinear Kalman filters are subject to the condition that system noises are Gaussian. However, in practical applications (such as multi-sensor integrated systems for vehicle navigation), non-Gaussian noises are commonly involved in nonlinear systems, and thus the direct use of these improved Kalman filters will lead to divergent solutions.

The particle filter (PF) is a typical method used to handle nonlinear systems with non-Gaussian noises [15,16,17]. Instead of integral calculation, PF uses sample mean to implement the Bayesian estimation under nonlinear dynamics. When the particle number is sufficiently large, the posterior probability density function of particles will be sufficiently accurate to guarantee the accuracy of the state mean and variance. However, the particle filter suffers from particle degradation, and there can be difficulty in selecting an appropriate importance density function for importance sampling [18,19,20]. Research efforts have been dedicated to improving the PF. Pitt et al. used auxiliary samples to adjust the resampling order to improve the PF’s performance [21]. D. Liu et al. studied an adaptive partial resampling method to prevent particle degradation [22]. T.H. Liu et al. proposed an adaptive processing scheme to lower particle weights based on a genetic algorithm [23]. However, these improvements cannot guarantee an optimal resultant particle distribution.

The cubature rules provide an effective way to improve sampling accuracy, and thus have been used in the PF to obtain the importance density function, leading to the cubature particle filter (CPF). Xia et al. studied a CPF in the context of system state estimation, where the second-order resistor–capacitor equivalent circuit model was used to verify the filter accuracy [24]. Shi et al. proposed a robust CPF by introducing Huber’s M-estimation theory to handle poor observation data [25]. Feng et al. studied a CPF based on the artificial bee colony for target tracking via underwater wireless sensor networks [26]. Zhang et al. combined a CPF with truncation adaptation to generate recommendation distributions for strong nonlinear systems [27]. Liu et al. investigated a hybrid CPF for stable-state estimation in harsh charging or discharging schedules [28]. Xing et al. constructed an adaptive CPF to estimate navigation parameters for high-dimensional nonlinear vehicle systems [29]. In general, the existing CPFs still suffer from the problem of particle degradation in the PF.

Further, the existing CPFs do not consider constraints. In practical applications (such as multi-sensor integrated systems for vehicle navigation), it is quite common that a nonlinear system is subject to various conditions. Considering these conditions as state constraints in the CPF filter process can effectively improve the estimation accuracy. However, since the existence of state constraints changes the probability structure of a dynamic system, which increases the difficulty of filtering estimation, the related research is still limited. Gao et al. proposed a constrained unscented particle filter for SINS/GNSS/ADS-integrated airship navigation in the presence of wind field disturbances [30]. Seifzadeh et al. studied a constrained particle filter based on soft data to improve filtering performance for target tracking [31]. In complex and highly dynamic environments, Xu et al. presented a particle filter based on spatial–temporal constraints for cooperative target tracking [32]. Zhang et al. presented a constrained multiple model based on a PF for target tracking [33]. However, the above studies are not based on the advanced cubature rules. They also focus on particular applications with specific constraints rather than on general constraint problems.

This paper presents a novel constrained cubature particle filter (CCPF) to improve the CPF’s performance for vehicle navigation. This method incorporates constraints in the cubature transformation to improve the CPF’s importance sampling, resulting in an improved importance density distribution. It also improves the CPF resampling process by using the Euclidean distance of the measurement residual to adjust particle weights to ensure the diversity of particles, thus avoiding particle degradation. The convergence of the proposed CCPF has been rigorously proved. Experiments and a comparative analysis with the traditional PF and CPF were conducted to evaluate the CCPF’s efficiency for GNSS/DR-integrated vehicle navigation.

2. Cubature Particle Filter

Consider the nonlinear dynamic system

$$\begin{array}{l}{\mathit{x}}_k=f({\mathit{x}}_{k-1})+{\mathit{v}}_k\\ {\mathit{y}}_k=h({\mathit{x}}_k)+n_k\end{array}$$

(1)

where ${\mathit{x}}_k\in R^n$ is the n-dimensional system state vector at time point $k$, ${\mathit{y}}_k\in R^n$ is the m-dimensional system measurement vector, ${\mathit{v}}_k\in R^n$ is the process noise with covariance $\mathit{R}$, $n_k\in R^m$ is the measurement noise with covariance $\mathit{Q}$, and $f(\cdot )$ and $h(\cdot )$ represent the nonlinear system and measurement functions. Obviously, the system described by (1) does not involve the assumption of Gaussian noises.

Like the unscented transform, CPF uses the third-order spherical radial cubature rules to approximate the nonlinear system functions. According to the prior mean and covariance of the system state, CPF selects the cubature points via the cubature rules and further approximates the state mean and covariance by the weighting sum of the cubature points [34,35,36]. Consider the dynamic system described by (1), the CPF procedure is given as below [37,38]:

Step 1: Initialization

Set the initial state estimate together with its covariance and weights as follows:

$$\begin{array}{l}{\widehat{\mathit{x}}}_0^j=E[{\mathit{x}}_0^j]\\ {\mathit{P}}_0^j=E[({\mathit{x}}_0^j-{\widehat{\mathit{x}}}_0^j)({\mathit{x}}_0^j-{\widehat{\mathit{x}}}_0^j){]}^T\\ {\mathit{w}}_0^j=\frac{1}{N}\end{array}(j=1,2,\cdots ,N)$$

(2)

where $N$ is the number of particles, ${\widehat{\mathit{x}}}_0^j\sim p({\mathit{x}}_0)$ denotes the initial state estimate of the $j\mathrm{th}$ particle, and $p({\mathit{x}}_0)$ denotes the initial state distribution.

For $k=1,2,\cdots ,M$, execute Steps 2–3.

Step 2: Importance sampling

For $j=1,2,\cdots ,N$, execute Steps (a)–(c).

(a) Calculate the cubature points as follows:

$${\mathit{x}}_{i,k-1}^j={\mathit{S}}_{k-1}^j{\varsigma }_i+{\hat{\mathit{x}}}_{k-1}^j$$

(3)

$${\mathit{P}}_{k-1}^j={\mathit{S}}_{k-1}^j{({\mathit{S}}_{k-1}^j)}^T$$

(4)

where $i=1,2,\cdots ,2n$, ${\mathit{P}}_{k-1}^j$ is the state covariance at $k-1$, ${\mathit{S}}_{k-1}^j$ is the lower triangular matrix obtained via the Cholesky decomposition of ${\mathit{P}}_{k-1}^j$, ${\mathit{x}}_{i,k-1}^j$ is the ith cubature point of the jth particle at $k-1$, ${\widehat{\mathit{x}}}_{k-1}^j$ is the state estimation, and ${\varsigma }_i$ is defined as follows:

$${\varsigma }_i=\{\begin{array}{cc}\sqrt{n}{I}_i& i=1,2,\cdots ,n\\ -\sqrt{n}{I}_{i-n}& i=n+1,n+2,\cdots ,2n\end{array}$$

(5)

where $I_i$ denotes the ith column vector of the $n\times n$ identity matrix.

(b) Time update

Calculate the state prediction covariance and one-step measurement prediction via the cubature points

$${\mathit{x}}_{i,k|k-1}^j=\mathit{f}({\mathit{x}}_{i,k-1}^j)$$

(6)

$${\widehat{\mathit{x}}}_{i,k|k-1}^j=\frac{1}{2n}\sum _{i=1}^{2n}{\mathit{x}}_{i,k|k-1}^j$$

(7)

$${\mathit{P}}_{k|k-1}^j=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\left[\left({\mathit{x}}_{i,k|k-1}^j-{\widehat{\mathit{x}}}_{i,k|k-1}^j\right){\left({\mathit{x}}_{i,k|k-1}^j-{\widehat{\mathit{x}}}_{i,k|k-1}^j\right)}^T\right]}+\mathit{R}$$

(8)

$${\mathit{y}}_{k|k-1}^j=\mathit{h}({\widehat{\mathit{x}}}_{k|k-1}^j)$$

(9)

$${\widehat{\mathit{y}}}_{k|k-1}^j=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}{\mathit{y}}_{k|k-1}^j}$$

(10)

where ${\widehat{\mathit{x}}}_{k|k-1}^j$, ${\widehat{\mathit{y}}}_{k|k-1}^j$ and ${\mathit{P}}_{k|k-1}^j$, denote the predicted state mean, measurement mean, and measurement covariance of the jth cubature point, respectively.

(c) Measurement update

Calculate the cubature point auto-correlation covariance and cross-correlation covariance

$$\begin{gathered} {\mathit{P}}_{y_k{y}_k}^j=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\left[\left({\mathit{y}}_{i,k|k-1}^j-{\widehat{\mathit{y}}}_{k|k-1}^j\right){\left({\mathit{y}}_{i,k|k-1}^j-{\widehat{\mathit{y}}}_{k|k-1}^j\right)}^T\right]}+\mathit{Q} \\ {\mathit{P}}_{x_k{y}_k}^j=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\left[\left({\mathit{x}}_{i,k|k-1}^j-{\widehat{\mathit{x}}}_{k|k-1}^j\right){\left({\mathit{y}}_{i,k|k-1}^j-{\widehat{\mathit{y}}}_{k|k-1}^j\right)}^T\right]} \end{gathered}$$

(11)

Calculate the cubature point estimates

$$\begin{gathered} {\widehat{\mathit{x}}}_k^j={\widehat{\mathit{x}}}_{k|k-1}^i+{\mathit{K}}_k^j({\mathit{y}}_k-{\widehat{\mathit{y}}}_{k|k-1}^j) \\ {\widehat{\mathit{P}}}_k^j={\mathit{P}}_{k|k-1}^j-{\mathit{K}}_k^j{\mathit{P}}_{y_k{y}_k}^j{({\mathit{K}}_k^j)}^T \\ {\mathit{K}}_k^j={\mathit{P}}_{x_k{y}_k}^j{({\mathit{P}}_{y_k{y}_k}^j)}^{-1} \end{gathered}$$

(12)

where ${\mathit{K}}_k^j$ denotes the filter gain at k of the jth particle, and ${\widehat{\mathit{x}}}_k^j$ and ${\widehat{\mathit{P}}}_k^j$ denote the estimate and its covariance of the jth cubature point after cubature transformation.

The particle importance density function is calculated using $({\widehat{\mathit{x}}}_k^j,{\widehat{\mathit{P}}}_k^j)$, and the particles are sampled by ${\overline{\mathit{x}}}_k^j\sim q({\mathit{x}}_k^j|{\mathit{x}}_{0:k-1}^j,{\mathit{y}}_{1:k})=N({\widehat{\mathit{x}}}_k^j,{\widehat{\mathit{P}}}_k^j)$, ${\overline{\mathit{x}}}_{0:k}^j\triangleq ({\mathit{x}}_{0:k-1}^j,{\overline{\mathit{x}}}_k^j)$, and ${\widehat{\mathit{P}}}_{0:k}^j\triangleq ({\mathit{P}}_{0:k-1}^j,{\widehat{\mathit{P}}}_k^j)$.

Calculate the particle weights and normalize them as follows:

$$w_k^j=w_{k-1}^j\frac{p({\mathit{y}}_k|{\overline{\mathit{x}}}_k^j)p({\overline{\mathit{x}}}_k^j|{\mathit{x}}_{k-1}^j)}{q({\overline{\mathit{x}}}_k^j|{\mathit{x}}_{k-1}^j,{\mathit{y}}_k)}$$

(13)

$${\tilde{w}}_k^i=w_k^i/{\displaystyle \sum _{i=1}^n{w}_k^i}$$

(14)

Step 3: Resampling

According to the approximately distributed $p{\overline{\mathit{x}}}_{0:k}^j|{\mathit{y}}_{1:k}$ ($j=1,2,\cdots N$), generate $N$ new particles by duplicating the particles of high weights and further assigning the same weight $\frac{1}{N}$ to them.

Calculate the state estimate and its covariance

$$\begin{gathered} {\widehat{\mathit{x}}}_k={\displaystyle \sum _{j=1}^N}{\tilde{w}}_k^j{\overline{\mathit{x}}}_k^j \\ {\mathit{P}}_k={\displaystyle \sum _{j=1}^N{\tilde{w}}_k^j}[({\overline{\mathit{x}}}_k^j-{\mathit{x}}_k){({\overline{\mathit{x}}}_k^j-{\mathit{x}}_k)}^T] \end{gathered}$$

(15)

As mentioned previously, the existence of state constraints changes the probability structure of a dynamic system. In the importance sampling step, if the system state is subject to constraints, (7) will be biased. The bias in (7) will be propagated through (8)–(14), and eventually, the state estimate from (15) will be biased. Therefore, it is necessary to incorporate the state changes caused by constraints in the importance sampling step. In addition, in the resampling process, the duplication of the particles with high weights from (13) will lose the particle diversity. Since the useful particle samples are reduced, the filtering solution will deteriorate. A straightforward solution is to adjust the weights of useful particles to increase their contributions to the state estimation. This paper addresses the above two issues in CPF, leading to a new CCPF for state estimation under constraints.

3. Constrained Cubature Particle Filter

The proposed CCPF combines Euclidean distance-based resampling and constraints to improve the CPF’s performance. In the importance sampling process, the proposed CCPF improves the CPF importance sampling density function with state constraints. In the resampling process, the proposed CCPF optimizes the CPF weights by adopting the Euclidean distance to adjust the particle weights to ensure the diversity of particles, preventing particle degradation while enabling the easy acquisition of an importance density function which is close to the true density function.

3.1. Importance Sampling

In the importance sampling process of CCPF, we perform constrained projection calculations on the cubature point estimates obtained by (12). Suppose the system described by (1) is subject to the following constraint:

$$\mathit{D}{\mathit{x}}_k\le \mathit{d}$$

(16)

where $\mathit{D}$ represents the constrained matrix, and $\mathit{d}$ represents the constrained vector [39,40].

The estimation problem of the constrained state can be transformed into the following optimization problem:

$$\min J({\tilde{\mathit{x}}}_k^j)={({\tilde{\mathit{x}}}_k^j-{\widehat{\mathit{x}}}_k^j)}^T\mathit{W}({\tilde{\mathit{x}}}_k^j-{\widehat{\mathit{x}}}_k^j)$$

(17)

$$\mathit{D}{\tilde{\mathit{x}}}_k^j={\mathit{d}}_k^j$$

(18)

where ${\tilde{\mathit{x}}}_k^j$ represents the state estimation under the constraint, $\mathit{W}$ represents an arbitrary symmetric positive definite matrix for constructing the Lagrange function, and ${\widehat{\mathit{x}}}_k^j$ represents the state estimation without the constraint.

Using the Lagrange multiplier to solve (17) and (18), we can obtain the following:

$$J({\tilde{\mathit{x}}}_k^j,\lambda )={({\tilde{\mathit{x}}}_k^j-{\widehat{\mathit{x}}}_k^j)}^T\mathit{W}({\tilde{\mathit{x}}}_k^j-{\widehat{\mathit{x}}}_k^j)+2{\lambda }^T(\mathit{D}{\tilde{\mathit{x}}}_k^j-{\mathit{d}}_k^j)$$

(19)

where $\lambda$ represents a vector for constructing the Lagrange function.

Finding the partial derivative of (19), we obtain

$${\tilde{\mathit{x}}}_k^j={\widehat{\mathit{x}}}_k^j-{\mathit{W}}^{-1}{\mathit{D}}^T{(\mathit{D}{\mathit{W}}^{-1}{\mathit{D}}^T)}^{-1}(\mathit{D}{\widehat{\mathit{x}}}_k^j-{\mathit{d}}_k^j)$$

(20)

Using (20), the cubature point estimate described by (12) becomes

$$\begin{gathered} {\widehat{\mathit{x}}}_k^j={\widehat{\mathit{x}}}_{k|k-1}^i+{\mathit{K}}_k^j({\mathit{y}}_k-{\widehat{\mathit{y}}}_{k|k-1}^j) \\ {\widehat{\mathit{P}}}_k^j={\mathit{P}}_{k|k-1}^j-{\mathit{K}}_k^j{\mathit{P}}_{y_k{y}_k}^j{({\mathit{K}}_k^j)}^T \\ {\mathit{K}}_k^j={\mathit{P}}_{x_k{y}_k}^j{(}^{-1}{\mathit{P}}_{x_k{y}_k}^j) \\ {\tilde{\mathit{x}}}_k^j={\widehat{\mathit{x}}}_k^j-{\mathit{W}}^{-1}{\mathit{D}}^T{(\mathit{D}{\mathit{W}}^{-1}{\mathit{D}}^T)}^{-1}(\mathit{D}{\widehat{\mathit{x}}}_k^j-{\mathit{d}}_k^j) \end{gathered}$$

(21)

From (21), we can obtain the importance sampling density function under constraints, i.e., ${\overline{\mathit{x}}}_k^j\sim N({\tilde{\mathit{x}}}_k^j,{\widehat{\mathit{P}}}_k^{*j})$, where ${\widehat{\mathit{P}}}_k^{*j}$ denotes the covariance under constraints.

3.2. Resampling

Calculate the Euclidean distance of the measurement residual. Using (13), record the maximum weights ${\mathit{w}}_{\mathit{k}}^{{\mathit{j}}_{\max }}$ and minimum weights ${\mathit{w}}_{\mathit{k}}^{{\mathit{j}}_{\min }}$

$${\mathit{L}}_{\max }={({\mathit{r}}_k^{{\mathit{j}}_{\min }}-{\mathit{r}}_k^{j_{\max }})}^T\cdot ({\mathit{r}}_k^{j_{\min }}-{\mathit{r}}_k^{j_{\max }})$$

(22)

$${\mathit{L}}^j={({\mathit{r}}_k^j-{\mathit{r}}_k^{j_{\max }})}^T\cdot ({\mathit{r}}_k^j-{\mathit{r}}_k^{j_{\max }})$$

(23)

where ${\mathit{L}}_{\max }$ and ${\mathit{L}}^j$ denote the Euclidean distances with the subscripts “${\mathit{j}}_{\max }$” and “${\mathit{j}}_{\min }$”, representing the index numbers of the particles with the maximum and minimum weights, and $\mathit{r}$ denotes the measurement residual, i.e.,

$$\begin{gathered} {\mathit{r}}_k^{j_{\max }}={\mathit{y}}_k-h({\overline{\mathit{x}}}_k^{j_{\max }}) \\ {\mathit{r}}_k^{j_{\min }}={\mathit{y}}_k-h({\overline{\mathit{x}}}_k^{j_{\min }}) \end{gathered}$$

(24)

Accordingly, the weights are calculated as

$$w_k^{j^{*}}=w_k^j+(\frac{w_k^{j_{max}}}{N})·\sin (\frac{{\mathit{L}}^j}{{\mathit{L}}_{\max }}·\frac{\pi }{2})·\beta$$

(25)

where β ≥ 0 is a coefficient related to measurement characteristics [41]. The larger $\beta$ is, the larger the adjustment to the weight will be. When $\beta =0$, there will be no adjustment to the weight. The CCPF procedure is shown in Figure 1.

Figure 1. The CCPF procedure.

3.3. Convergence Analysis

In this section, we will study the convergence of the proposed CCPF when the sample size $N$ is sufficiently large. Since the posterior density of the system state corresponds to the empirical measure of particle samples, we conduct a convergence analysis based on an empirical measure which is defined by a probability measure.

Similar to the PF, assuming that the transfer kernel function $\varphi$ satisfies the Feller process and the state likelihood function $g$ is a continuous bounded positive definite function (i.e., $\Vert g\Vert <\infty$), for $k\ge 0$, when $N\to \infty$, we can have [42]

$$\underset{N\to \infty }{\lim }{\pi }_{k|k}^N=\underset{N\to \infty }{\lim }{\varphi }_{1:k}^N({\eta }^N)={\varphi }_{1:k}(\eta )={\pi }_{k|k}$$

(26)

where ${\pi }_{k|k}$ denotes the ideal distribution of the system state, $\eta$ represents the initial distribution, and ${\pi }_{k|k}^N$ is the posterior probability density, which is expressed as

$${\pi }_{k|k}^N(d{x}_k)=\frac{1}{N}{\displaystyle \sum _{i=1}^N{\delta }_{x_k^i}(d{x}_k)}$$

(27)

where $\delta$ denotes the Dirichlet function.

The time and measurement updates of CCPF can be represented as follows [43]:

$${\pi }_{k|k-1}={\displaystyle \underset{{ℝ}^{n_{\mathit{x}}}}{\int }{\pi }_{k-1|k-1}(\mathit{d}{\mathit{x}}_{k-1})\varphi (\mathit{d}{\mathit{x}}_k|{\mathit{x}}_{k-1})}$$

(28)

$${\pi }_{k|k}(\mathit{d}{\mathit{x}}_k)=\frac{g({\mathit{y}}_k|{\mathit{x}}_k){\pi }_{k|k-1}(\mathit{d}{\mathit{x}}_k)}{{\displaystyle \underset{{ℝ}^{n_{\mathit{x}}}}{\int }g({\mathit{y}}_k|{\mathit{x}}_k){\pi }_{k|k-1}(\mathit{d}{\mathit{x}}_k)}}$$

(29)

Equations (28) and (29) can be rewritten as

$$({\pi }_{k|k-1},\phi )=({\pi }_{k-1|k-1},\varphi \phi )$$

(30)

$$({\pi }_{k|k},\phi )={({\pi }_{k|k-1},g)}^{-1}({\pi }_{k|k-1},\phi g)$$

(31)

where $\phi \subset B({ℝ}^{n_x})$ can be any bounded function, and $({\pi }_{k|k-1},g)>0$.

As shown in Section 3.1, the CCPF involves three key steps: the initialization, importance sampling, and resampling. Therefore, the convergence analysis of CCPF will be applied to each step. Denote the posterior probability density in the importance sampling process using ${\pi }_{k|k}^N$ and that in the resampling process using ${\tilde{\pi }}_{k|k}^N$.

Theorem 1.

For any bounded function $\phi \subset B({ℝ}^{n_x})$; if $\Vert g\Vert$ is bounded, then 

$$E[(({\pi }_{k-1|k-1}^N,\phi )-({\pi }_{k-1|k-1},\phi ){)}^2]\le c_{k-1|k-1}^N\frac{{\Vert \phi \Vert }^2}{N}$$

(32)

Proof. 

Using (31), we obtain

$$\begin{array}{ll}({\pi }_{k-1|k-1}^N,\phi )-({\pi }_{k-1|k-1},\phi )& =\frac{({\pi }_{k-1|k-2}^N,g\phi )}{({\tilde{\pi }}_{k-1|k-2}^N,g)}-\frac{({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2},g)}\\ & =\frac{({\pi }_{k-1|k-1}^N,g\phi )}{({\pi }_{k-1|k-1}^N,g)}-\frac{({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}+\frac{({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}--\frac{({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}\\ & =\frac{({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}\\ & +\frac{({\pi }_{k-1|k-2},g\phi )(({\pi }_{k-1|k-2},g\phi )-({\pi }_{k-1|k-2}^N,g))}{({\pi }_{k-1|k-2}^N,g)({\pi }_{k-1|k-2},g)}\end{array}$$

(33)

According to the Minkowski inequality, we have

$$\begin{array}{l}\mathrm{E}\left[\right(\frac{({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}+\frac{({\pi }_{k-1|k-2},g\phi )(({\pi }_{k-1|k-2},g\phi )-({\pi }_{k-1|k-2}^N,g))}{({\pi }_{k-1|k-2}^N,g)({\pi }_{k-1|k-2},g)}{)}^2{]}^{\frac{1}{2}}\\ \le \mathrm{E}\left[\right(\frac{({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2},g\phi )}{({\pi }_{k-1|k-2}^N,g)}{)}^2{]}^{\frac{1}{2}}+\mathrm{E}\left[\right(\frac{({\pi }_{k-1|k-2}^{ },g\phi )(({\pi }_{k-1|k-2}^{ },g\phi )-({\pi }_{k-1|k-2}^N,g))}{({\pi }_{k-1|k-2}^N,g)({\pi }_{k-1|k-2}^{ },g)}{)}^2{]}^{\frac{1}{2}}\\ \le \frac{\mathrm{E}[(({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2}^{ },g\phi ){)}^2{]}^{\frac{1}{2}}}{({\pi }_{k-1|k-2}^N,g)}+\frac{({\pi }_{k-1|k-2}^{ },g\phi )\mathrm{E}[(({\pi }_{k-1|k-2}^{ },g\phi )-({\pi }_{k-1|k-2}^N,g\phi ){)}^2{]}^{\frac{1}{2}}}{({\pi }_{k-1|k-2}^N,g)({\pi }_{k-1|k-2}^{ },g)}\\ \le \frac{\sqrt{c_{k-1|k-2}^{ }}\Vert g\phi \Vert }{({\tilde{\pi }}_{k-1|k-2}^N,g)\sqrt{N}}+\frac{\mathrm{E}[(({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2}^{ },g\phi ){)}^2{]}^{\frac{1}{2}}}{({\pi }_{k-1|k-2}^N,g)}\end{array}$$

(34)

Since $\Vert g\Vert <\infty$, we have

$$\begin{array}{l}\frac{\sqrt{c_{k-1|k-2}^{ }}\Vert g\phi \Vert }{({\tilde{\pi }}_{k-1|k-2}^N,g)\sqrt{N}}+\frac{\mathrm{E}[(({\pi }_{k-1|k-2}^N,g\phi )-({\pi }_{k-1|k-2}^{ },g\phi ){)}^2{]}^{\frac{1}{2}}}{({\pi }_{k-1|k-2}^N,g)}\\ \le \frac{\sqrt{c_{k-1|k-2}^{ }}\Vert g\phi \Vert }{({\pi }_{k-1|k-2}^N,g)\sqrt{N}}+\frac{\sqrt{c_{k-1|k-2}^{ }}\Vert g\phi \Vert }{({\pi }_{k-1|k-2}^N,g)\sqrt{N}}\\ \le \frac{2\sqrt{c_{k-1|k-2}^{ }}\Vert g\phi \Vert }{({\pi }_{k-1|k-2}^N,g)\sqrt{N}}=\sqrt{c_{k-1|k-1}^N}\frac{\Vert \phi \Vert }{\sqrt{N}}\end{array}$$

(35)

where $\sqrt{c_{k-1|k-1}^N}=\frac{2\sqrt{c_{k-1|k-2}^{ }}\Vert g\Vert }{({\pi }_{k-1|k-2}^N,g)}$.

Equation (32) follows by substituting (35) into (34) and further applying the square operation. Thus, the proof of (32) is completed.

Theorem 1 shows that the initialization process of the CCPF is converged when $N\to \infty$. □

Theorem 2.

If $\Vert \varphi \phi \Vert \le \Vert \phi \Vert$ holds, the importance sampling process of the CCPF yields

$$E[(({\pi }_{k|k-1}^N,\phi )-({\pi }_{k|k-1}^{ },\phi ){)}^2]\le c_{k|k-1}^N\frac{{\Vert \phi \Vert }^2}{N}$$

(36)

Proof. 

Using (30), we can obtain

$$\begin{array}{l}E[(({\pi }_{k|k-1}^N,\phi )-({\pi }_{k|k-1}^{ },\phi ){)}^2{]}^{\frac{1}{2}}\\ =E[(({\pi }_{k|k-1}^N,\phi )-({\pi }_{k-1|k-1}^N,\varphi \phi )+({\pi }_{k-1|k-1}^N,\varphi \phi )-({\pi }_{k|k-1}^{ },\phi ){)}^2{]}^{\frac{1}{2}}\\ \le E[(({\pi }_{k|k-1}^N,\phi )-({\pi }_{k-1|k-1}^N,\varphi \phi ){)}^2{]}^{\frac{1}{2}}+E[({\pi }_{k-1|k-1}^N,\varphi \phi )-({\pi }_{k-1|k-1}^{ },\varphi \phi ){)}^2{]}^{\frac{1}{2}}\\ =E[(({\pi }_{k-1|k-1}^N,\varphi {\phi }^2)-({\pi }_{k-1|k-1}^N,\varphi \phi ){)}^2{]}^{\frac{1}{2}}+E[({\pi }_{k-1|k-1}^N,\varphi \phi )-({\pi }_{k-1|k-1}^{ },\varphi \phi ){)}^2{]}^{\frac{1}{2}}\end{array}$$

(37)

According to Theorem 1 and letting ${(1+\sqrt{c_{k-1|k-1}^{ }})}^2=c_{k|k-1}^N$, and when $\Vert \varphi \phi \Vert \le \Vert \phi \Vert$ holds, we can have

$$\begin{array}{l}E[(({\pi }_{k-1|k-1}^N,\varphi {\phi }^2)-({\pi }_{k-1|k-1}^N,\varphi \phi ){)}^2{]}^{\frac{1}{2}}+E[({\pi }_{k-1|k-1}^N,\varphi \phi )-({\pi }_{k-1|k-1},\varphi \phi ){)}^2{]}^{\frac{1}{2}}\\ \le \frac{ }{\sqrt{N}}+\sqrt{c_{k-1|k-1}^{ }}\frac{\Vert \phi \Vert }{\sqrt{N}}\\ =(1+\sqrt{c_{k-1|k-1}^{ }})\frac{\Vert \phi \Vert }{\sqrt{N}}\end{array}$$

(38)

Equation (36) follows by substituting (38) into (37) and further applying the square operation. Thus, the proof of Theorem 2 is completed.

Theorem 2 shows that the importance sampling process of the CCPF is converged when $N\to \infty$. □

The resampling process of the CCPF involves the adjustment of the particle weights. In theory, the prior distribution should be close to the importance sampling distribution. Then, using (13), we have $w_k^j\propto g$. From (25) and $\left|(\frac{w_k^{j_{\max }}}{N})\cdot \sin (\frac{{\mathit{L}}^j}{{\mathit{L}}_{\max }}\cdot \frac{\pi }{2})\right|<1$, it is readily known that the particle weights are bounded. By normalization, we can readily have the particle weights within [0, 1]. Without the loss of generality and for the concise description purpose, we write $g\to Fg$, where F is a linear transformation, and $Fg$ is bounded. Thus, we can derive the following theorem.

Theorem 3.

For the resampling process of the CCPF, we have

$$E[(({\tilde{\pi }}_{k|k}^N,\phi )-({\pi }_{k|k},\phi ){)}^2]\le c_{k|k}^N\frac{{\Vert \phi \Vert }^2}{N}$$

(39)

Proof. 

Using (31) and $g\to Fg$, we can obtain the following:

$$\begin{array}{l}E[(({\tilde{\pi }}_{k|k}^N,\phi )-({\pi }_{k|k}^{ },\phi ){)}^2{]}^{\frac{1}{2}}\\ =E[(({\tilde{\pi }}_{k|k}^N,\phi )-({\tilde{\pi }}_{k|k}^N,\phi )+({\tilde{\pi }}_{k|k}^N,\phi )-({\pi }_{k|k}^{ },\phi ){)}^2{]}^{\frac{1}{2}}\\ =E[(\frac{({\tilde{\pi }}_{k|k-1}^N,(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}+\frac{({\pi }_{k|k-1},(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1},(Fg)\phi )}{({\pi }_{k|k-1}^{ },Fg)}{)}^2{]}^{\frac{1}{2}}\\ =E[((\frac{({\tilde{\pi }}_{k|k-1}^N,(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1},(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)})+(\frac{({\pi }_{k|k-1},(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1},(Fg)\phi )}{({\pi }_{k|k-1},Fg)}){)}^2{]}^{\frac{1}{2}}\\ \le E[(\frac{({\tilde{\pi }}_{k|k-1}^N,(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}{)}^2{]}^{\frac{1}{2}}+E[(\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\pi }_{k|k-1}^{ },Fg)}{)}^2{]}^{\frac{1}{2}}\end{array}$$

(40)

According to Theorem 2 and the letting constant $c_{k|k}^N=\frac{2\sqrt{c_{k|k-1}}\Vert Fg\Vert }{({\tilde{\pi }}_{k|k-1}^N,Fg)}$, we have

$$\begin{array}{l}E[(\frac{({\tilde{\pi }}_{k|k-1}^N,(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}{)}^2{]}^{\frac{1}{2}}+E[(\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}-\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\pi }_{k|k-1}^{ },Fg)}{)}^2{]}^{\frac{1}{2}}\\ =E[(\frac{({\tilde{\pi }}_{k|k-1}^N,(Fg)\phi )-({\pi }_{k|k-1}^{ },(Fg)\phi )}{({\tilde{\pi }}_{k|k-1}^N,Fg)}{)}^2{]}^{\frac{1}{2}}+E[(\frac{({\pi }_{k|k-1}^{ },(Fg)\phi )(({\pi }_{k|k-1}^{ },Fg)-({\tilde{\pi }}_{k|k-1}^N,Fg)}{({\tilde{\pi }}_{k|k-1}^N,Fg)({\pi }_{k|k-1}^{ },Fg)}{)}^2{]}^{\frac{1}{2}}\\ \le \frac{\sqrt{c_{k|k-1}^{ }}\Vert Fg\Vert \cdot \Vert \phi \Vert }{({\tilde{\pi }}_{k|k-1}^N,g)\sqrt{N}}+\frac{\Vert \phi \Vert \cdot \mathit{E}[(({\pi }_{k|k-1}^{ },Fg)-({\tilde{\pi }}_{k|k-1}^N,Fg){)}^2{]}^{\frac{1}{2}}}{({\tilde{\pi }}_{k|k-1}^N,g)}\\ \le \frac{2\sqrt{c_{k|k-1}^{ }}\Vert Fg\Vert \cdot \Vert \phi \Vert }{({\tilde{\pi }}_{k|k-1}^N,Fg)\sqrt{N}}=\sqrt{c_{k|k}^N}\frac{\Vert \phi \Vert }{\sqrt{N}}\end{array}$$

(41)

Since $\Vert Fg\Vert <\infty$, (39) follows by substituting (41) into (40) and further applying the square operation. Thus, the proof of (39) is completed.

Theorem 3 shows that the resampling process of the CCPF is converged when $N\to \infty$. □

From Theorems 1–3, it can be inferred that the posterior probability function of CCPF will be converged when $N\to \infty$, which indicates the convergence of the proposed CCPF.

4. Experimental Results

4.1. GNSS/DR Vehicle Navigation System

Experiments on a GNSS/DR-integrated navigation system of a sports car were conducted to evaluate the performance of the proposed CCPF. The state vector of the GNSS/DR-integrated navigation system is defined as follows:

$$\mathit{x}(t)={\left[\begin{array}{cccccccc}{p}_E& v_E& a_E& p_N& v_N& a_N& \epsilon & \psi \end{array}\right]}^T$$

(42)

where $p_E$, ${\mathit{v}}_E$, and $a_E$ are the position, velocity, and acceleration in east; $p_N$, $v_N$, and $a_N$ are the position, velocity, and acceleration in the north; $\epsilon$ is the gyro drift error; and $\psi$ is the DR calibration coefficient.

The system state equation is as follows [44,45]:

$$\dot{\mathit{x}}=\left[\begin{array}{cccccccc}0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& -\frac{1}{{\tau }_{a_E}}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& -\frac{1}{{\tau }_{a_N}}& 0& 0\\ 0& 0& 0& 0& 0& 0& \frac{1}{{\tau }_{\epsilon }}& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\mathit{x}(\mathit{t})+\mathit{u}+\mathit{v}$$

(43)

where $\mathit{v}$ is the process noise with covariance $\mathit{R}$; ${\tau }_{a_E}$ and ${\tau }_{a_N}$ are the correlation time constants for the variation rates of the accelerations in the east and north; ${\tau }_{\epsilon }$ is the correlation time constant for the first-order Markov process in the gyro drift; and $\mathit{u}$ is the control input, defined as

$$\mathit{u}={\left[\begin{array}{cccccccc}0& 0& \frac{{\overline{a}}_E}{{\tau }_{a_E}}& 0& 0& \frac{{\overline{a}}_N}{{\tau }_{a_N}}& 0& 0\end{array}\right]}^T$$

(44)

where ${\overline{a}}_E$ and ${\overline{a}}_N$ are the means of the accelerations in the east and north at the current time.

The measurement vector $\mathit{y}$ is defined as

$$\mathit{y}={\left[\begin{array}{cccc}{p}_E& p_N& \omega & s\end{array}\right]}^T$$

(45)

where $p_E$ and $p_N$ represent the positions in the east and north from the GNSS receiver, $\omega$ is the angular rate from the gyroscope, and $s$ is the output distance from the DR.

The system measurement equation is described as follows:

$$\mathit{y}=h\left(\mathit{x}\right)+\mathit{n}$$

(46)

where $h(\cdot )$ is the nonlinear measurement function, and $\mathit{n}$ is the measurement noise with covariance $\mathit{Q}$.

$$h\left(\mathit{x}\right)=\{\begin{array}{c}{p}_E\\ p_N\\ \frac{v_N{a}_E-v_E{a}_N}{v_E^2+v_N^2}+\epsilon \\ \psi T\sqrt{v_E^2+v_N^2}\end{array}$$

(47)

where $\epsilon$ is the first-order Markov process component of the gyro drift error, and $T$ is the sampling period.

According to the road conditions restricting the system state, the car travelling direction was constrained on the road to the east by the angle $\theta$. The constraint equation is expressed as follows:

$$\begin{gathered} \mathit{D}x=d=0 \\ \mathit{D}=\left[\begin{array}{cccccccc}-tg\theta & 0& 0& 1& 0& 0& 0& 0\\ 0& -tg\theta & 0& 0& 1& 0& 0& 0\end{array}\right] \end{gathered}$$

(48)

4.2. Experimental Setup

The experimental setup is shown in Figure 2. The high-precision differential GNSS receiver UT-206 with the positioning accuracy less than 10 cm was used to obtain the actual vehicle position as the reference to calculate the estimation error.

Figure 2. Experimental setup.

The GNSS data update rate was 1 Hz. The GNSS single-point L1/L2 accuracy was 1.5 m in the vertical direction and 1 m in the horizontal direction. The GNSS receiver accuracy was 0.5 m in the vertical direction and 0.3 m in the horizontal direction. The GNSS velocity accuracy was 0.05 m/s. During the test, at least seven navigation satellite signals were available for the GNSS measurement. The experimental data were collected from the car, which was travelling within a continuous time of 1000 s. After initialization for 1 min, the car moved stably at an average velocity of 40 km/h. The car travelling direction was constrained to the east by $\theta =65°$. The calibration coefficient of DR was $\psi =0.5$. The drift error of the gyroscope was 0.1°/h, and ${\tau }_E={\tau }_N=300 \mathrm{s}$. The car’s initial position, velocity, and acceleration were (0 m, 0 m), (5 m/s, 5 m/s), and (0 m/s², 0 m/s²). The total mileage was 15 km. $\mathit{R}=\mathit{diag}[{\begin{array}{cccccccc}{(10 \mathrm{m})}^2& {(1 \mathrm{m}/\mathrm{s})}^2& {(0.1 \mathrm{m}/{\mathrm{s}}^2)}^2& {(10 \mathrm{m})}^2& {(1 \mathrm{m}/\mathrm{s})}^2& {(0.1 \mathrm{m}/{\mathrm{s}}^2)}^2& {({0.1}^{\circ }/\mathrm{h})}^2& (1 \mathrm{m})\end{array}}^2]$, and Q = diag[(20 m)² (20 m)² (4 m/s)² (4 m/s)²].

For comparison purposes, experiments were conducted using the PF, CPF, and CCPF to estimate the car’s position and velocity errors. The root mean squared error (RMSE) was used as the metric for accuracy evaluation. The RMSE is defined as follows:

$$\mathrm{RMSE}=\sqrt{\frac{1}{F}{\displaystyle \sum _{i=1}^F({\widehat{\mathit{x}}}_i-{\mathit{x}}_{ref}})}$$

(49)

where F is the number of Monte Carlo runs, and ${\mathit{x}}_{ref}$ is the reference value. For the experimental analysis, 1000 Monte Carlo runs were conducted for the RMSE calculation. The overall RMSE is defined as follows:

$${\mathrm{RMSE}}_{overall}^{ }=\sqrt{{\mathrm{RMSE}}_E^2+{\mathrm{RMSE}}_N^2}$$

(50)

where ${\mathrm{RMSE}}_E$ and ${\mathrm{RMSE}}_N$ denote the RMSEs in the east and north.

Figure 3, Figure 4 and Figure 5 show the position errors obtained by the PF, CPF, and CCPF. It can be seen that the position error curves of all the three methods involve large fluctuations within the initial 100 s due to the system initialization. After the initial 100 s, the PF curves of position error still fluctuate greatly, leading to a position error of (−13.1 m, 14.0 m) in the north and a position error of (−14.5 m, 16.1 m) in the east. The CPF improves the PF with the cubature rules for particle sampling, leading to the position errors within (−13.0 m, 11.1 m) the north and (−13.0 m, 12.1 m) east. However, since the CPF still suffers from particle degradation and does not involve constraints in the state estimation, its improvement is limited. In contrast, since the CCPF optimizes the CPF sampling procedures, its position error is (−8.1 m, 9.6 m) in the north and (−7.0 m, 7.1 m) in the east. Table 1 lists the position RMSEs of the PF, CPF, and CCPF. The position RMSEs of the PF, CPF, and CCPF are 4.1719 m, 3.7231 m, and 2.7061 m in the north and 4.7618 m, 3.7504 m, and 2.5168 m in the east. The overall position RMSEs of the PF, CPF, and CCPF are 6.3308 m, 5.2846 m, and 3.6956 m, showing that the accuracy of the CCPF is 41.6% higher than that of the PF and 30.1% higher than that of the CPF.

Figure 3. Position error obtained using the PF.

Figure 4. Position error obtained using the CPF.

Figure 5. Position error obtained using the CCPF.

Table 1. Position RMSEs of the PF, CPF, and CCPF.

Filter	RMSE in East (m)	RMSE in North (m)	Error Range (m)	Overall RMSE (m)
PF	4.1719	4.7618	(−13.1, 14.0), (−14.5, 16.1)	6.3308
CPF	3.7231	3.7504	(−13.0, 11.1), (−13.0, 12.1)	5.2846
CCPF	2.7061	2.5168	(−8.1, 9.6), (−7.0, 7.1)	3.6956

Figure 6, Figure 7 and Figure 8 illustrate the velocity errors obtained by the PF, CPF, and CCPF. The velocity errors of the three methods have a similar trend to their position errors shown in Figure 3, Figure 4 and Figure 5. The velocity errors of the PF, which are (−0.24 m/s, 0.22 m/s) in the north and (−0.25 m/s, 0.21 m/s) in the east, is the largest. The CPF slightly improves the PF, leading to velocity errors of (−0.11 m/s, 0.16 m/s) in the north and (−0.18 m/s, 0.19 m/s) in the east. In contrast, the velocity errors of the CCPF, which are (−0.14 m/s, 0.15 m/s) (north) and (−0.10 m/s, 0.15 m/s) (east), are the smallest. Table 2 summarizes the velocity RMSEs of the three methods. The velocity RMSEs of the PF, CPF, and CCPF are 0.0912 m/s, 0.0892 m/s, and 0.0769 m/s in the north and 0.1023 m/s, 0.0887 m/s, and 0.0674 m/s in the east. The overall velocity RMSEs of the PF, CPF, and CCPF are 0.1371 m/s, 0.1258 m/s, and 0.1022 m/s, showing that the accuracy of the CCPF is 25.5% higher than that of the PF and 18.8% higher than that of the CPF.

Figure 6. Velocity error obtained using the PF.

Figure 7. Velocity error obtained using the CPF.

Figure 8. Velocity error obtained using the CCPF.

Table 2. Velocity RMSEs of the PF, CPF, and CCPF.

Filter	RMSE in East (m/s)	RMSE in North (m/s)	Error Range (m/s)	Overall RMSE (m/s)
PF	0.0912	0.1023	(−0.24, 0.22), (−0.25, 0.21)	0.1371
CPF	0.0892	0.0887	(−0.11, 0.16), (−0.18, 0.19)	0.1258
CCPF	0.0769	0.0674	(−0.14, 0.15), (−0.10, 0.15)	0.1022

From the above results, it can be concluded that the CCPF can achieve more accurate state estimations under constrained conditions than the CPF and PF for GNSS/DR-integrated navigation. It is also shown that the filtering solution of CCPF is bounded, which further verifies the theoretical convergence analysis of the CCPF (described in Section 3.3).

The above RMSEs, together with the theoretical convergence analysis described in Section 3.3, demonstrate the consistent performance of the proposed CCPF. To further evaluate the filter’s consistency, Figure 9 illustrates quantile–quantile plots of the position residuals derived from the use of the PF, CPF, and CCPF. As shown in Figure 9a, it is obvious that the quantile of the position residuals of the PF does not coincide with its normal quantile. Although the residual quantiles of both the CPF and CCPF are in good agreement with their normal quantiles, as shown in Figure 9b,c, the residual quantile of the CCPF is much closer to its normal quantile compared to that of the CPF. Therefore, it is clear that the proposed CCPF possesses the consistency for state estimation.

Figure 9. Quantiles of the position residuals derived from the use of (a) the PF, (b) the CPF, and (c) the CCPF. The quantiles of the position residuals are indicated by the black dots and “+”, and the normal quantiles are indicated by the dashed red line.

5. Conclusions

Nonlinear systems with constraints and non-Gaussian uncertainties are commonly encountered in vehicle navigation. To address this issue, this paper proposes a new CCPF to estimate system state parameters for vehicle navigation. It enhances the CPF importance sampling process using constraints to improve the importance density distribution. Subsequently, it improves the CPF resampling process by using the Euclidean distance of the measurement residual to adjust particle weights to ensure the diversity of particles, thus decreasing particle degradation. Theories were also established to prove the convergence of the proposed CCPF. The experimental results and the results derived from our comparative analysis of GNSS/DR-integrated vehicle navigation demonstrate that the proposed CCPF can effectively estimate system state parameters in constrained environments, resulting in it having enhanced accuracy compared to the PF and CPF.

Our future research work will focus on the improvement of the proposed CCPF. The proposed CCPF will be combined with advanced artificial intelligence technologies and genetic algorithms to achieve nonlinear state estimation in the environments of more complex constraints.