Batch Cyclic Posterior Selection Particle Filter and Its Application in TRN

Abstract

Terrain referenced navigation (TRN) determines position by comparing terrain height measurements with digital elevation maps (DEMs). However, terrain fluctuations create multimodal observation distributions, introducing significant nonlinearity that challenges fusion positioning algorithms. To address this, we propose a novel data fusion approach: batch cyclic posterior selection particle filter (BCPS-PF), applied to TRN. Our algorithm consists of two primary mechanisms. First, the batch cycle particle generation mechanism continuously generates particles conforming to the prior distribution. This is achieved by decomposing the state transition function and the state noise model during the prediction step. Particles from the previous time step are transformed via the state transition function, and noise sequences generated by the state noise model are added, forming batch cycle particles. Second, a particle selection mechanism filters particles to match the posterior distribution. This involves an update step in the fusion process, utilizing a rejection sampling technique. The batch cycle mechanism can be terminated by limiting the number of particles, and state estimation is derived by calculating the mean of these particles. Simulations demonstrate that our method improves positioning accuracy by over 10% compared with existing methods.

1. Introduction

Terrain referenced navigation (TRN) determines position by comparing terrain height measurements with digital elevation maps (DEMs) [1]. This method is particularly robust against electromagnetic interference and adverse weather conditions, making it suitable for applications in missiles, military aircraft, and underwater equipment [2,3,4].

Despite its advantages, TRN faces significant challenges due to the multimodal distribution of elevation observations, especially in areas with anomalous terrain profiles [5]. Traditional filtering methods often struggle to capture these complex features, leading to potential filter degeneracy and localization failure.

To address the issues associated with multimodal distributions in TRN, nonlinear filtering methods have gained increasing attention. Among these, particle filtering (PF) stands out due to its flexibility in handling nonlinear problems [6,7]. The PF models dynamic systems with nonlinear and non-Gaussian properties, making it a well-accepted estimator for TRN [8]. It implements recursive Bayesian estimation using Monte Carlo simulation to infer states from observations. The posterior probability density function (PDF) is represented by several weighted particles, with the weights calculated based on the conditional likelihood of each particle given the current observations [7,9].

However, PF encounters a unique challenge in the TRN application. The likelihood distribution presents a multimodal feature due to the undulating topography [10]. This feature does not always pose a problem; however, it will pose a serious degeneracy problem when the high probability region of the prior distribution is far away from that of the likelihood distribution [11]. This intermittent degeneracy problem greatly affects the stability of fusion localization, but this problem has not received enough attention from scholars.

1.1. Motivation

Due to the irregular terrain and sharp elevation changes, the observations provided by terrain referenced navigation (TRN) systems exhibit strong nonlinear and non-Gaussian characteristics. Therefore, researchers commonly employ sequential Monte Carlo methods (such as the particle filter (PF)) to fuse TRN observations.

However, a key challenge in achieving high-quality fusion of TRN observations—which is often overlooked in standard PF implementations—lies in ensuring that the multiple high-probability regions (modes) of the multimodal posterior distribution have sufficient non-degenerate particles to accurately represent the distribution’s characteristics. When the prior particles fall into low-likelihood regions, the traditional sequential importance resampling (SIR) PF suffers from the particle degeneracy problem, making accurate state estimation impossible.

To address this fundamental issue, we propose the batch cyclic posterior selection particle filter (BCPS-PF) algorithm. This fusion method aims to indirectly generate particles from the true posterior distribution, thereby ensuring that the high-probability regions of the posterior distribution, which play a crucial role in state estimation, have a sufficient number of high-quality particles to accurately describe their distribution characteristics.

The procedure of the BCPS-PF can be mainly divided into two parts as shown in Figure 1, directly reflecting the prediction and update steps of the Bayesian filter:

(1) Batch cyclic particle generation mechanism (prediction step). This mechanism is designed by mathematically decomposing the state transition function and the state noise model in the prediction step. Its purpose is to generate a diverse batch of proposal particles that conform to the prior distribution $p\left(x_k\left|z_{k-1}\right.\right)$.

➢

It first generates one-step transition particles based on the particles representing the posterior distribution at the previous time step using the deterministic part of the state transition function.

➢

Subsequently, it adds a noise sequence generated from the state noise model to each one-step transition particle in a cyclic manner, thus continuously generating a large batch of proposed particles that correctly conform to the prior distribution.

(2) Posterior particle selection mechanism (update step). This mechanism is designed by integrating the Bayesian update step with the rejection sampling (RS) technique. Its purpose is to select particles that accurately conform to the posterior distribution $p\left(x_k\left|z_k\right.\right)$.

➢

It first matches the batch cycle particles with the digital elevation map (DEM) and calculates the likelihood weights by combining the observation and observation noise model.

➢

Then, it performs acceptance/rejection sampling on the batch cycle particles based on the magnitude of the likelihood weights. This selection mechanism effectively completes the fusion of prior and likelihood information and obtains a new particle set that statistically conforms to the posterior distribution.

Finally, we can terminate the cycle mechanism by limiting the number of accepted particles and obtain the state estimation by calculating the mean of the selected particles. This approach eliminates the explicit need for the computationally expensive and potentially biased resampling and weight propagation steps found in conventional SIR-PF.

1.2. Related Works

TRN has been widely used in the field of missiles, military aircraft, underwater equipment, and so on since the 1970s [12]. After nearly 60 years of development, TRN algorithms can be roughly divided into two categories [3,13]: one is the terrain contour matching algorithm based on a correlation analysis, such as the TERCOM [14,15]; the other is the Bayesian filtering based-inertial terrain referenced navigation algorithm, such as the Sandia inertial terrain-aided navigation (SITAN) algorithm [16,17]. With the continuous application of nonlinear filtering theory in fusion positioning technology, researchers, inspired by TERCOM and SITAN, merged these two architectures to enhance the robustness of the TRN system [18,19,20,21]. However, a measurement ambiguity problem arises in TRN when there are similar terrain profiles surrounding the aircraft, which might potentially cause filter divergence, ultimately resulting in localization failure.

Scholars have made significant efforts in improving filtering methods for addressing the issue of ambiguous measurement [2,22,23]. Linear filtering methods were widely used in the early stages [24,25]. Recently, with the advancement of computer hardware capabilities, many nonlinear filtering algorithms capable of providing better approximations to analytical solutions of Bayesian filtering (which involve slightly higher computational demands) have gradually been applied in TRN systems, achieving favorable results [26,27,28]. Refs. [6,29] treat TRN as a nonlinear filtering problem and employ point mass filtering (PMF) to overcome the linear truncation errors. Particle filtering (PF), driven by Monte Carlo methods, has the capability to describe and propagate arbitrary distributions. Therefore, it has received extensive research in TRN algorithms [30,31,32]. Ref. [33] conducts a comparative analysis of PFs with different resampling methods in TRN, and assesses their performance. Ref. [34] decomposes the linear and nonlinear components of the INS using marginal PF (MPF). It employs Kalman filtering (KF) and particle filtering (PF) for estimating the linear and nonlinear parts, respectively. This approach enhances the approximation of the analytical solution of Bayesian filtering in TRN fusion. Ref. [35] uses gradient-fitted results to correct the likelihood distribution obtained from TRN measurements in PF. Furthermore, [36] presents a new constrained cubature particle filter (CCPF) for the difficulty in nonlinear filtering. Ref. [37] adopts spherical simplex unscented transformation (SSUT) to approximate the probability distribution for achieving high accuracy. Overall, the evolution of TRN algorithms has always revolved around a central objective: approximating the analytical solution of the Bayesian method within a nonlinear filtering framework, thereby obtaining the optimal estimate in the sense of minimum mean square error (MMSE).

PF is highly suitable for addressing TRN challenges due to its flexibility in nonlinear filtering problems. However, an impoverishment problem can potentially lead to filter divergence in TRN, in situations of complex terrain or areas with multiple similar terrain features. Dealing with the issues above, the likelihood distribution associated with observations provided by the TRN may exhibit bimodal or even multimodal characteristics. The ability to fully describe such complex likelihood distribution depends on whether there are sufficient particles at each peak’s position, which is determined by the proposal distribution. When the high-probability regions of the proposal distribution do not encompass one peak of likelihood, the particles selected based on the proposal distribution are highly likely to miss the information of likelihood in this area [37]. This phenomenon can lead to filter divergence or result in incorrect position estimates within some local steps. In [38], out-of-sequence measurement (OOSM) was proposed to overcome this issue. The strategy of the proposed method is to skip the measurement update at the time step when an ambiguous measurement update is detected. This method can effectively reduce the impact of incorrect position estimates on the overall flight localization.

The difficulty of determining the shape of the likelihood distribution and the possible multimodal distribution are typical features in TRN. This feature puts forward higher requirements for the multi-source fusion algorithm. Specifically, better balance mechanisms are needed to deal with the degeneracy and impoverishment problems for the PF algorithm. It is important to note that the optimal proposal distribution is theoretically a posterior distribution, as rigorously proven in a previous work [39]. However, the actual posterior distribution is not directly accessible.

1.3. Our Contributions

This paper introduces a novel particle filter using the posterior distribution as the proposal distribution, namely the batch cyclic posterior selection particle filter (BCPS-PF), designed to address the specific challenges above. The BCPS-PF aims to improve the proposal distribution, ensuring a sufficient number of particles are in high-probability regions of the posterior distribution, enhancing accuracy. The algorithm generates batch cycle particles that adhere to the prior state distribution through state model decomposition. It then incorporates rejection sampling (RS) in the update step of the fusion process to select particles that conform to the posterior distribution, resulting in a precise state estimation. The main contributions of this paper are as follows:

(1)

Developing a mechanism to generate batch cycle particles that form cyclic continuous batches conforming to the prior distribution. This involves decomposing the state model into a state transition function and a state noise model. The state transition function transfers particles, while the state noise model ensures particle diversity, resulting in batch cycle particles that describe the prior distribution.

(2)

Designing a particle selection mechanism that integrates the update step in the fusion process with the RS technique, obtaining selected particles that conform to the posterior distribution.

(3)

Proposing the BCPS-PF algorithm, which indirectly uses the posterior distribution as the proposal distribution, avoiding the need for resampling and recursive weight transmission, thus effectively balancing degeneracy and impoverishment problems, ensuring filter accuracy and stability.

(4)

Evaluating the algorithm in inertial measurement units (IMUs) with TRN fusion positioning scenarios, demonstrating significant performance improvements.

The remainder of this paper is structured as follows: Section 2 presents the TRN principle and the fundamental concepts of data fusion. Section 3 analyzes the impact of TRN on fusion positioning during IMU/TRN fusion and introduces the motivation and framework for the BCPS-PF algorithm. Section 4 delves into the BCPS-PF, providing a detailed explanation of its two primary mechanisms: batch cycle particle generation and particle selection. Section 5 discusses the performance evaluation of the BCPS-PF in IMU/TRN fusion positioning scenarios. Finally, Section 6 concludes this paper.

2. TRN Principle and System Model

TRN utilizes digital elevation models (DEMs) to indirectly estimate the horizontal position of unmanned aerial vehicles (UAVs) by matching measured UAV height. These position estimates are then integrated with horizontal position data obtained from the IMU to enhance accuracy. This section outlines the methodology for deriving horizontal position estimates from both the IMU and TRN. Following this, we discuss the fusion localization process based on traditional PF.

IMU measures an object’s acceleration and angular velocity, typically using accelerometers and gyroscopes on three axes. Based on these measurements and an initial position, IMU can estimate a UAV’s position. This paper focuses on fusing IMU and TRN data to obtain a UAV’s horizontal position. To achieve this, we simplify the IMU mathematical model by considering only the two horizontal position parameters as the state model, as shown below:

$$x_k={\left[p_k^x,p_k^y\right]}^T$$

(1)

where $p_k^x$ and $p_k^y$ are the position of a UAV in two directions (horizontal direction) at time step $k$.

The dynamic models adopted in this work are:

$$x_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}{v}_{k-1}^x\Delta t\\ v_{k-1}^y\Delta t\end{array}\right]+u_{k-1}$$

(2)

where $v_k^x$ and $v_k^y$ are real velocities corresponding to these two position directions (provided by the IMU) at time step $k$. $u_{k-1}$ is the zero-mean Gaussian process characterizing the IMU measurement noise. Equation (2) is considered the state model.

The principle of obtaining observation data by the TRN system is illustrated in Figure 2. The UAV utilizes barometer altimeter and radar altimeter to measure its current barometric altitude and radar altitude at the current position, respectively. Then, the current ground altitude at that location is obtained as an observation. In addition, this observation can be obtained by matching the current horizontal position estimated by IMU with DEM. Therefore, the dynamic measurement model characterizing the TRN system is as follows:

$$Z_k=h\left(x_k\right)+v_k$$

(3)

where $Z_k$ denotes the results of the terrain elevation measurement. The real terrain elevation can be obtained from the DEM stored on the UAV. $v_k$ is a zeros-mean Gaussian processes characterizing the elevation measurement noise arising from the barometric altimeter and radar altimeter. Equation (3) is considered the observation model.

3. From Bayesian Estimation to Particle Filter

State Equation (2) can be considered as the state transition probability $p\left({\mathit{x}}_k\left|{\mathit{x}}_{0:k-1}\right.\right)$, whereas measurement Equation (3) represents the likelihood probability $p\left({\mathit{z}}_{0:k}\left|{\mathit{x}}_k\right.\right)$.

The goal of the recursive Bayesian estimation is to obtain the expectation of the posterior PDF at the current time step based on Bayes’ theorem, as follows:

$${\overline{\mathit{x}}}_k=E\left[p\left({\mathit{x}}_k\left|{\mathit{z}}_{1:k}\right.\right)\right]$$

(4)

where $p\left({\mathit{x}}_k\left|{\mathit{z}}_{1:k}\right.\right)$ denotes the posterior PDF at time step $k$. To achieve the recursive Bayesian estimation, the filter process is usually divided into two steps: prediction and update.

Prediction: Obtaining the prior PDF using the state transition probability and the posterior probability at the last time step, as follows:

$$p\left({\mathit{x}}_k\left|{\mathit{z}}_{k-1}\right.\right)={\displaystyle \int p\left({\mathit{x}}_k\left|{\mathit{x}}_{k-1}\right.\right)p\left({\mathit{x}}_{k-1}\left|{\mathit{z}}_{k-1}\right.\right)d{\mathit{x}}_{k-1}}$$

(5)

Update: Fusion of the prior PDF and the likelihood distribution, as follows:

$$p\left({\mathit{x}}_k\left|{\mathit{z}}_k\right.\right)={\displaystyle \frac{p\left({\mathit{z}}_k\left|{\mathit{x}}_k\right.\right)p\left({\mathit{x}}_k\left|{\mathit{z}}_{k-1}\right.\right)}{p\left({\mathit{z}}_k\left|{\mathit{z}}_{k-1}\right.\right)}}$$

(6)

In this step, the fusion of the measurement is accomplished by calculating weights to particles: ${\left\{{\tilde{\omega }}_k^i\right\}}_{i=1}^N={\left\{p\left({\mathit{z}}_k\left|{\mathit{x}}_k^i\right.\right)\right\}}_{i=1}^N$. At this point, Equation (6) will become an integral difficulty problem due to the nonlinear characteristic in (3), which makes it impossible to obtain an analytical solution of the posterior PDF. In PF, the posterior distribution is approximated using weighted particles, as follows:

$$p\left({\mathit{x}}_k\left|{\mathit{z}}_k\right.\right)\approx {\displaystyle \sum _{i=1}^N{\omega }_k^i\delta \left({\mathit{x}}_k-{\mathit{x}}_k^i\right)}$$

(7)

where $\delta$ denotes the Dirac delta function; $N$ denotes particle number; ${\omega }_k^i$ is the weight of the i-th particle. Although weighted samples can approximate the posterior distribution, a degeneracy problem can arise in the recursive process, leading to filter divergence. To address this issue, resampling techniques are introduced. All particles obtained through resampling have equal weights (taking SIR as an example). Therefore, Equation (7) can be rewritten as follows:

$$p\left({\mathit{x}}_k\left|{\mathit{z}}_k\right.\right)\approx {\displaystyle \frac{1}{N}}{\displaystyle \sum _{i=1}^N\delta \left({\mathit{x}}_k-{\mathit{x}}_k^i\right)}$$

(8)

A pseudo-code description of the SIR algorithm is given in Algorithm 1.

Algorithm 1. Particle filter algorithm.

Initialization: Particle sampling is performed according to the pdf of the initial position.${\left\{X_0\right\}}_N~p\left(x_0\right)$ and obtain observation $z_1$

//Overall time steps: For $k\leftarrow 1$ to $K$ do (1): Draw ${\left\{{\widehat{X}}_k\right\}}_N\sim p\left(x_k\left|z_{k-1}\right.\right)$ according to (2), respectively; (2): ${\left\{{\widehat{X}}_k\right\}}_N$ are matched with the DEM to generate the predicted observations ${\left\{z_{k,pre}\right\}}_N$. (3): Calculate the deviation ${\left\{{\epsilon }_{k,pre}\right\}}_N={\left\{z_{k,pre}\right\}}_N-z_k$ (4): Calculate ${\left\{{\widehat{\omega }}_k\right\}}_N={\left\{p\left({\epsilon }_{k,pre}\left|{\widehat{X}}_k\right.\right)\right\}}_N$, respectively; (5): Normalization of the weights: ${\left\{{\omega }_k\right\}}_N={\left\{{\displaystyle \frac{{\widehat{\omega }}_k}{sum\left({\left\{{\widehat{\omega }}_k\right\}}_N\right)}}\right\}}_N$, respectively; (6): Resampling [11], and obtain ${\left\{X_k\right\}}_N$ (7): State estimation: ${\widehat{x}}_k=mean\left({\left\{X_k\right\}}_N\right)$ End

4. The BCPS-PF Algorithm

4.1. Batch Cycle Particle Generation Mechanism in Prediction Step

Suppose random variable ${\chi }_{k-1}~p\left(x_{k-1}\left|z_{k-1}\right.\right)$. After passing through the state model, we can obtain a new random variable ${\widehat{\chi }}_k$, as shown in Figure 3. Then, ${\widehat{\chi }}_k~p\left(x_k\left|z_{k-1}\right.\right)$ according to the prediction step [27].

While $X_{k-1}$ is a set of particles that generates from ${\chi }_{k-1}$, then the distribution characteristic of these particles can be used to represent $p\left(x_{k-1}\left|z_{k-1}\right.\right)$ approximately. At this point, we can easily get a new set of particles ${\widehat{X}}_k$, where ${\widehat{X}}_k=f\left(X_{k-1}\right)+u_0$ and $u_0$ denotes the noise sequence generated by the state noise model. ${\widehat{X}}_k$ is a set of particles satisfied by $p\left(x_k\left|z_{k-1}\right.\right)$. When the number of particles is larger, the description of $p\left(x_{k-1}\left|z_{k-1}\right.\right)$ by $X_{k-1}$ is more accurate. At the same time, the description of $p\left(x_k\left|z_{k-1}\right.\right)$ by ${\widehat{X}}_k$ is more accurate. However, the number of particles is always limited in a real-time PF system.

In this subsection, our goal is to obtain a particle flow that can be used to reflect $p\left(x_k\left|z_{k-1}\right.\right)$ as accurately as possible under the condition that the particle number of $X_{k-1}$ is fixed. For this purpose, we have decomposed the state transition function with the state noise model, as shown in Figure 4. ${\tilde{\chi }}_k$ is the random variable obtained by the result of the state transition function without the state noise model, called the one-step transfer random variable. $p\left(f\left(x_{k-1}\right)\left|z_{k-1}\right.\right)$ is defined as the PDF of ${\tilde{\chi }}_k$, called the one-step transfer distribution. When ${\tilde{\chi }}_k$ passes through the state noise model, it is defined as the PDF of ${\tilde{\tilde{\chi }}}_k$.

Furthermore, the one-step transfer particle ${\tilde{X}}_k$ can be used to describe $p\left(f\left(x_{k-1}\right)\left|z_{k-1}\right.\right)$, where ${\tilde{X}}_k=f\left(X_{k-1}\right)$.

Theorem 1.

In the nonlinear system of Figure 4, $p\left({\tilde{\chi }}_k\left|z_{k-1}\right.\right)={\displaystyle {\int }_{{ℝ}_{k-1}}p\left({\chi }_{k-1}\left|z_{k-1}\right.\right)d{\chi }_{k-1}}$ ,where ${ℝ}_{k-1}=\left\{{\chi }_k:{\tilde{\chi }}_k=f\left({\chi }_{k-1}\right)\right\}$.

Theorem 2.

In the nonlinear system of Figure 4, $p\left({\tilde{\tilde{\chi }}}_k\left|z_{k-1}\right.\right)=p\left({\widehat{\chi }}_k\left|z_{k-1}\right.\right)$ , where $p\left({\chi }_k\left|z_{k-1}\right.\right)$ is described in Figure 3.

From Theorem 1 we can see that the random variable ${\tilde{\chi }}_k$ depends on $p\left({\chi }_{k-1}\left|z_{k-1}\right.\right)$ and the nonlinear system $f\left(•\right)$; from Theorem 2 we can see that the random variable ${\widehat{\chi }}_k$ can be used to describe the prior distribution $p\left({\chi }_k\left|z_{k-1}\right.\right)$. When particles ${\left\{X_{k-1}\right\}}_{N_{k-1}}$, the posteriori distribution at time step $k-1$ is obtained. We are able to obtain a deterministic set of particles ${\left\{{\tilde{X}}_k\right\}}_{N_{k-1}}$ used to describe the random variable ${\tilde{\chi }}_k$ due to the determinacy of nonlinear system $f\left(•\right)$, where ${\tilde{X}}_k=f\left(X_{k-1}\right)$. Consider the noise model is known, as shown in Figure 5, we can generate a particle stream ${\left\{{\tilde{\tilde{X}}}_k\right\}}^i$ that represents the batch ith particle noise model and ${\tilde{\tilde{\chi }}}_k~p\left({\tilde{\tilde{x}}}_k\left|z_{k-1}\right.\right)$, where

$${\left\{{\tilde{\tilde{X}}}_k\right\}}^i={\tilde{X}}_k+{\left\{u\right\}}^i$$

(9)

4.2. Particle Selection Mechanism in Update Step

The fusion of the prior and the posterior distribution is one of the most important steps. This step is equivalent to the mathematical equation as follows:

$$p\left(x_k\left|z_k\right.\right)={\displaystyle \frac{p\left(z_k\left|x_k\right.\right)p\left(x_k\left|z_{k-1}\right.\right)}{p\left(z_k\left|z_{k-1}\right.\right)}}$$

(10)

where $p\left(z_k\left|x_k\right.\right)$ denotes the likelihood distribution and $p\left(x_k\left|z_{k-1}\right.\right)$ denotes the posterior distribution. $p\left(z_k\left|z_{k-1}\right.\right)$ is the normalization constant.

Compared with the batch cycle particle generation mechanism, the prediction step in the PF algorithm is equivalent to obtain ${\left\{{\tilde{\tilde{X}}}_k\right\}}^0$. Then the weights corresponding to particles ${\left\{{\tilde{\tilde{X}}}_k\right\}}^0$ can be easily calculated, as follows:

$${\left({\tilde{\tilde{\omega }}}_k\right)}^0=p\left(z_k\left|{\left\{{\tilde{\tilde{X}}}_k\right\}}^0\right.\right)$$

(11)

After that, the posteriori distribution can be described by the weighted particles $\left\{{\left({\tilde{\tilde{\omega }}}_k\right)}^0,{\left\{{\tilde{\tilde{X}}}_k\right\}}^0\right\}$. However, in the TRN system, when the observation exhibits a multimodal distribution, particles sampled from the proposal distribution are likely to miss certain states of the observation due to the limited number of particles, as shown in Figure 6. In this situation, part of the likelihood information can easily be lost.

In this subsection, we propose a particle selection mechanism whose core is the acceptance and rejection of weighted prior particles. This process allows us to directly acquire particles satisfying the posterior distribution and complete the state estimation.

In (5), consider $p\left(x_k\left|z_k\right.\right)$ and $p\left(x_k\left|z_{k-1}\right.\right)$ are the target distribution and proposal distribution, respectively. We have obtained prior particles ${\left\{{\tilde{\tilde{X}}}_k\right\}}^i$ in Section 4.1, which can be used to describe $p\left(x_k\left|z_{k-1}\right.\right)$. According to the principle of RS construction of the acceptance probability, we can get the expression of acceptance probability as follows:

$$\xi <{\displaystyle \frac{p\left(x_k\left|z_k\right.\right)}{Mp\left(x_k\left|z_{k-1}\right.\right)}}$$

(12)

where $\xi ~U\left(0,1\right)$ and $M$ is a constant used to ensure that:

$$\begin{array}{l}Mp\left(x_k\left|z_{k-1}\right.\right)\ge p\left(x_k\left|z_k\right.\right) \\ \forall x_k\in dom\left(p\left(x_k\left|z_{k-1}\right.\right)\right)\cap dom\left(p\left(x_k\left|z_k\right.\right)\right)\end{array}$$

(13)

Accept the sample if the result obtained by substituting the particle into the right side of (7) is greater than a randomly generated $\xi$. Otherwise, reject the sample. Obtaining an explicit expression for $p\left(x_k\left|z_k\right.\right)$ directly is difficult, but substitute (5) into (11), and we have:

$$\xi <{\displaystyle \frac{p\left(z_k\left|x_k\right.\right)p\left(x_k\left|z_{k-1}\right.\right)}{Mp\left(x_k\left|z_{k-1}\right.\right)p\left(z_k\left|z_{k-1}\right.\right)}}$$

(14)

Consider $p\left(z_k\left|z_{k-1}\right.\right)$ denotes the normalization constant and simplify (9) to get:

$$\xi <{\displaystyle \frac{p\left(z_k\left|x_k\right.\right)}{L}}$$

(15)

where $L=Mp\left(z_k\left|z_{k-1}\right.\right)$. Then, the size of $L$ is to ensure:

$$L\ge p\left(z_k\left|x_k\right.\right) \forall x_k\in dom\left(p\left(z_k\left|x_k\right.\right)\right)$$

(16)

In TRN data fusion, since the expression of likelihood distribution is obvious, the selection of $L$ can be calculated simply as follows:

$$L=\max \left(p\left(z_k\left|x_k\right.\right)\right)$$

(17)

Consequently, the principle of the particle selection method is shown in Figure 7. When we obtain particle $x_k^j$ from the prior particles ${\left\{{\tilde{\tilde{X}}}_k\right\}}^i$, we first compute the ratio of its weight to $L$. We then compare this ratio to a random number $\xi$ to determine whether to reject or accept the particle. The collection of accepted particles forms a particle set $X_k$, which reflects the current posterior distribution $p\left(x_k\left|z_k\right.\right)$.

Finally, the state estimation is as follows:

$${\widehat{x}}_k={\displaystyle \frac{1}{N_k}}{\displaystyle {\sum }_{i=1}^{N_k}{x}_k^i}$$

(18)

where $X_k\equiv {\left[\begin{array}{cccc}{x}_k^1& x_k^2& \dots & x_k^{N_k}\end{array}\right]}^T$, $x_k^j$ denotes a single accepted particle. The detailed BCPS-PF steps are summarized in Algorithm 2.

Algorithm 2. The BCPS-PF algorithm.

Initialization: Set sample number $N_0$ Generate the initial sample ${\left\{X_0\right\}}_{N_0}$ according to the initial PDF $p\left(x_0\right)$

//Overall time steps: For $k\leftarrow 1$ to $K$ do (1): Obtain the prior particles without state noise model according to ${\tilde{X}}_k=f\left(X_{k-1}\right)$; (2): Set $i=1;N_k=0;X_k=\varnothing$ While $N_k<0.9*N_0$: (3): Obtain a batch of prior particles ${\left\{{\tilde{\tilde{X}}}_k\right\}}^i$ according to Equation (4); (4): ${\left\{{\tilde{\tilde{X}}}_k\right\}}^i$ are matched with the DEM to generate the predicted observations ${\left\{z_{k,pre}\right\}}^i$; (5): Calculate the deviation ${\left\{{\epsilon }_{k,pre}\right\}}^i={\left\{z_{k,pre}\right\}}^i-z_k$; (6): Calculate the likelihood weights ${\left({\tilde{\tilde{\omega }}}_k\right)}^i$ of each particle according to (10); (7): Generate $N_{k-1}$ random number ${\left(\xi \right)}^i$ where ${\left({\xi }^j\right)}^i\sim U\left[0,1\right]$; For $j\leftarrow 1$ to $N_{k-1}$ (8): if ${\left({\tilde{\tilde{\omega }}}_k^j\right)}^i>{\left({\xi }^j\right)}^i$: $X_k=\left[X_k,{\left({\tilde{\tilde{x}}}_k^j\right)}^i\right]$, $N_k=N_k+1$; End $i=i+1$ End (9): State estimation according to Equation (13). End

Remark 1.

In the practical application of The RN system, the observation model is a simple one-dimensional nonlinear system, and the observation noise model is assumed to be a Gaussian noise model. For convenience, the expression of likelihood weights is as follows:

$${\left({\tilde{\tilde{\omega }}}_k\right)}^i=\exp \left\{-{\displaystyle \frac{{\left(z_k-{\left({\tilde{\tilde{z}}}_{k,pre}\right)}^i\right)}^2}{2{\sigma }^2}}\right\}$$

(19)

where $\sigma$ is the standard deviation of the observation noise; $z_k$ is the observation; ${\left({\tilde{\tilde{z}}}_{k,pre}\right)}^i$ is the predicted observation based on the ith batch of particles. It is easy to know that $\max \left\{{\left({\tilde{\tilde{\omega }}}_k\right)}^i\right\}=1$ ; therefore, we can set $L=1$.

Remark 2.

Due to the uncertainty in selecting the number of particles through RS, it is difficult to precisely obtain $N_{k-1}$ particles from the entire cycle of one batch particles. Therefore, the number of particles cannot be fixed in the recursive processing. In this situation, we choose $0.9\times N_0$ as the termination condition for the batch cycle. When the number of accepted particles exceeds $0.9\times N_0$, we terminate the batch cycle particle generation mechanism and use the obtained particles as a description of the posterior distribution.

4.3. Computational Complexity Analysis

In terms of computational complexity, BCPS-PF requires $O\left(KN\left(d_x+d_y\right)\right)$ operations, where $N$ is the target particle count, $d_x$ and $d_y$ are the state and observation dimensions, and $K$ is the number of acceptance-sampling batches needed to reach a predefined fraction of accepted particles. When the transition prior already overlaps well with the posterior $\left(K\approx 1\right)$, BCPS-PF runs in $O\left(N\left(d_x+d_y\right)\right)$, matching the cost of SIS-PF. By contrast, EKF-PF and UPF each invoke $O\left(N{d}_x^3\right)$ per particle (for linearization or sigma-point calculations), resulting in $O\left(N{d}_x^3\right)$ complexity. Thus, BCPS-PF offers a middle ground: for modest $K$, its workload remains linear in N and avoids the cubic cost of EKF-PF/UPF, while maintaining better posterior fidelity than SIR-PF.

5. Simulation Experiment

To assess the proposed BCPS-PF algorithm, we consider two representative scenarios: a strongly nonlinear one-dimensional benchmark model and TRN scenarios.

5.1. Kitagawa’s Nonlinear Model

The system model is as follows:

$$\left\{\begin{array}{c}{x}_t=0.5x_{t-1}+{\displaystyle \frac{25x_{t-1}}{1+x_{t-1}^2}}+8\cos \left(1.2\left(t-1\right)\right)+u_{t-1}\\ y_t=0.05x_t^2+v_t\end{array}\right.$$

where the process and observation noise are the Gaussian white noise with zero mean, $u_t~N\left(0,{\sigma }_u^2\right)$, $v_t~N\left(0,{\sigma }_v^2\right)$.

The parameters are set as follows: simulation time $T=50$; ${\sigma }_v^2=0.1$. In addition, we set ${\sigma }_u^2$ to two different values—1.0 and 0.1—for evaluating the robustness of BCPS-PF. The initial state is $x_0=5$, and particles are initialized from a Gaussian prior $N\left(x_0,{\sigma }_{prior}^2\right)$ with ${\sigma }_{prior}^2=2.0$.

We consider five particle counts: $N=\left\{10,30,50,100,500\right\}$. For each configuration, 500 independent Monte Carlo trials are conducted. Competing algorithms include SIR-PF, EPF, UPF, and APF. BCPS-PF includes a safeguard: if no particles are accepted in a given batch, the particle with the highest likelihood within that batch is retained. This prevents the filter from stalling and ensures continuity in the estimation process under sparse likelihood support. and the number of candidate batches is capped at 50.

When the variances of process noise and observation noise are at different scales, they can significantly impact the performance of nonlinear filtering algorithms [40]. Figure 8 illustrates the trends in state estimation accuracy for different filtering algorithms as the number of particles increases, under two process noise variance conditions.

As can be seen from Figure 8, the state estimation accuracy of EPF and UPF, which construct their proposal distributions based on linear Gaussian approximations, is weaker than that of SIR-PF, APF, and the proposed BCPS-PF in Kitagawa’s nonlinear model. This is attributed to the fact that, under strong nonlinear conditions, approximations of the noise model often lead to larger errors. Further analysis of the algorithms’ performance under varying process noise covariances reveals instability in the APF compared with the SIR-PF. Specifically, when ${\sigma }_u^2=0.1$, the APF improves the filtering accuracy slightly by incorporating likelihood information, demonstrating a superior estimation performance over SIR-PF under equivalent particle counts. However, when ${\sigma }_u^2=1.0$, the filtering accuracy of APF paradoxically falls below that of SIR-PF. This performance degradation can be attributed to the dominance of errors introduced by the likelihood information when there is a significant discrepancy between process noise and observation noise, thereby leading to a reduction in filtering accuracy.

In sharp contrast with the aforementioned algorithms, the proposed algorithm is specifically designed to overcome these limitations. It introduces a rejection sampling technique, which indirectly enables the use of an approximate posterior distribution as the proposal distribution. This fundamental approach strictly adheres to the Bayesian filtering framework, eliminates the need for potentially performance-degrading resampling steps, and demonstrates improved convergence in both tested scenarios. Crucially, the BCPS-PF demonstrates significantly improved and consistent estimation accuracy across both tested process noise covariance scenarios. Its stability, regardless of the significant variations in process noise, highlights its robustness against the changing nonlinearity strength and the discrepancy between process and observation noise, unlike the unstable behavior observed in the APF.

Figure 9 illustrates the computational time required for a single simulation run of different algorithms. As shown in the figure, the computational overhead of the proposed algorithm increases linearly with the increasing number of particles, being slightly higher than that of the SIR-PF algorithm and comparable to APF.

5.2. TRN Scenarios

In this section, simulation experiments are conducted to verify the performance of the BCPS-PF in TRN scenarios.

5.2.1. Simulation Model and Condition

The simulation scenario aims to simulate the process of a UAV flying along a specified trajectory (a figure-eight pattern) in an environment with a loaded DEM. In this scenario, IMU/TRN fusion positioning is performed. The simulation scenario includes the following modules:

(1)

DEM data: The scenario utilizes a pre-loaded DEM with a resolution of 30 m (DEM) [37]. These DEM data are represented by a 4801 × 1000 matrix. The UAV’s initial position for the flight is set at row 87, column 94 within this matrix.

(2)

IMU data: As detailed in Section 2, the mathematical model incorporates errors associated with the IMU’s position and velocity estimations. These errors are modeled as zero-mean Gaussian distributions.

(3)

TRN observation: The TRN observation is obtained by subtracting the barometer altimetry reading from the radar altimetry reading. During this process, independent zero-mean Gaussian distributed errors are introduced for both barometer and radar altimetry measurements.

(4)

UAV trajectory: The UAV commences its flight from the designated starting point, traveling due east for 86 s at a constant speed. It then performs a northward circular flight for 43 s. Furthermore, it executes another southward circular flight for 43 s. Finally, it flies straight towards the east for 128 s. The total flight time is 300 s. Throughout the entire flight, the UAV maintains a constant speed. The trajectory of the UAV flying over the DEM is shown in Figure 10. The relevant parameters are shown in

Table 1. The parameter configuration of simulation.

Condition	Value
Simulation duration	300 s
Velocity	100 m/s
The flight altitude	900 m
Initial position error	40 m
The std of position error estimated by the IMU	5
The std of velocity error estimated by the IMU	2
IMU sampling frequency	1 Hz
The flight altitude	900 m
Observation sampling frequency	1 Hz
The std of observation error obtained by barometer altimetry	15
The std of observation error obtained by radar altimetry	4.71

.

5.2.2. Method

To evaluate the performance of the proposed IMU/TRN fusion localization algorithm, we employed the aforementioned simulation scenario. In this scenario, the state transition function is provided by the IMU, and the observation equation is determined by the observation and the matching status with the DEM. See Section 2 for details. The specific matching process is as follows: First, the prior position estimation result was provided by the IMU, and the data of the two adjacent grids (60 m extension) around the position were selected as the DEM submap based on the prior position, and the submap was linearly interpolated as the matching object, as shown in Figure 11. Additionally, we compared the BCPS-PF against four nonlinear filtering methods: PF, auxiliary PF (APF), mixture particle filter (MPF), and OOSM-PF. All particle filter-based algorithms utilized the sampling importance resampling (SIR) method and set the number of particles to 500. For BCPS-PF, it was difficult to precisely maintain a fixed number of particles during the multi-batch particle selection process. Therefore, we set the initial number of particles to 500 and employed a threshold of 450 particles for termination in the recursive iteration. When the number of particles exceeded 450, the loop was terminated, and state estimation was performed. Moreover, the BCPS-PF directly generated particles that satisfied the posterior distribution diversity, eliminating the need for resampling. Finally, we conducted 100 Monte Carlo experiments for the IMU/TRN fusion localization process and employed the minimum mean squared error (MSE) as the metric to evaluate state estimation accuracy, as follows:

$$RMS{E}_{p^x}=\sqrt{{\displaystyle \frac{1}{Monter}}{\displaystyle {\sum }_{m=1}^{Monter}{\left(p_k^x-{\widehat{p}}_k^x\right)}^2}}$$

(20)

$$RMS{E}_{p^y}=\sqrt{{\displaystyle \frac{1}{Monter}}{\displaystyle {\sum }_{m=1}^{Monter}{\left(p_k^y-{\widehat{p}}_k^y\right)}^2}}$$

(21)

$$RMSE=\sqrt{{\displaystyle \frac{1}{Monter}}{\displaystyle {\sum }_{m=1}^{Monter}\left[{\left(p_k^x-{\widehat{p}}_k^x\right)}^2+{\left(p_k^y-{\widehat{p}}_k^y\right)}^2\right]}}$$

(22)

where $RMS{E}_{p^x}$ and $RMS{E}_{p^y}$ denote the RMSE of east–west and north–south directions, respectively. $p_k^x$ and $p_k^y$ denote the real position on east–west and north–south directions at time step $k$, respectively. ${\widehat{p}}_k^x$ and ${\widehat{p}}_k^y$ denote the results of the state estimation of position on east–west and north–south directions at time step $k$, respectively. $Monter$ denotes the number of Monte Carlo runs.

5.2.3. Results

Figure 12 and Figure 13 present the outcomes of a simulation experiment conducted within this scenario. As illustrated in Figure 12, our proposed algorithm and the four comparison algorithms successfully achieve IMU/TRN data fusion. Figure 13 highlights the posterior particle distributions of the different algorithms after fusion at the 251st time step, while the ✯ represent the true position. In this figure, the pentagram indicates the UAV’s actual position at the current moment, and subplot (f) shows the real posterior distribution. It is evident that the high-probability region of the posterior distribution deviates significantly from the real position due to the substantial observation noise at this time, which creates a disparity between the likelihood distribution and the prior distribution. This discrepancy results in a high-probability region of the posterior distribution that is far from the real position. Nevertheless, the goal remains to have particles that more accurately reflect the posterior distribution to achieve an optimal state estimation from a probabilistic statistical perspective.

Subplots (a) to (e) provide further insights: the APF algorithm’s particles are more concentrated in the upper right because this method emphasizes the weight of observation information, leading the particles to cluster in the high-probability region of the likelihood distribution. Although the PF algorithm does not emphasize the weight of observation information, the significant difference between the likelihood distribution and the prior distribution causes many particles to replicate in the high-probability region of the likelihood distribution. The MPF algorithm’s particles show a tendency to migrate toward the high-probability region of the posterior distribution; however, since its proposal distribution is not the accurate posterior distribution, the particles remain more dispersed. The OOSM-PF algorithm’s state estimation is closest to the true state because it detects when the likelihood distribution deviates significantly from the prior distribution and considers such observations as ambiguous, thus discarding them. However, from a statistical standpoint, a significant deviation between the likelihood observation and the prior distribution does not necessarily indicate reduced observation quality. Therefore, this strategy may not always be optimal. In contrast, the particle distribution of our proposed algorithm aligns most closely with the true posterior distribution. By utilizing particle flow generation and particle flow selection mechanisms, the proposed algorithm can directly generate particles that satisfy the posterior distribution without being affected by the degradation or lack of diversity related to the proposal distribution’s proximity to the posterior distribution.

The results of 100 Monte Carlo simulations for this scenario are presented in Figure 14, Figure 15 and Figure 16. Figure 14 and Figure 15 show the root mean square error (RMSE) of the positioning results for five filter algorithms in two directions. Compared with other PF-based algorithms, the APF relies more heavily on the observation model. However, the observation model of TRN generates a multimodal likelihood distribution, which statistically reduces the fusion accuracy of the APF. Both PF and the MPF algorithm achieve similar fusion accuracy because the clustering mode based on mixed Gaussians used in MPF is not well-suited for multimodal distributions in TRN environments. Consequently, the enhancements introduced by MPF over PF do not provide significant benefits in this scenario. OOSM-PF achieves slightly higher fusion accuracy than MPF and PF due to its better adaptability to the characteristics of TRN scenarios. The BCPS-PF, which directly generates particles that satisfy the posterior distribution, results in superior fusion accuracy compared with the benchmark algorithms.

Figure 16 presents a statistical analysis of the positioning errors from 100 rounds of Monte Carlo simulations. The interquartile range (IQR) of the upper and lower quartiles for all five methods is relatively similar, with the proposed algorithm having a slightly smaller IQR. The number and range of outliers for APF and MPF are significantly higher compared with the other three methods. This further validates that the improvements introduced by APF and MPF to particle filtering are not well-suited for TRN scenarios, leading to decreased system stability. Additionally, the number and magnitude of outliers observed for PF, OOSM-PF, and the proposed algorithm are relatively similar. The superior fusion accuracy of the proposed algorithm is attributed to its smaller IQR, resulting from its particles providing a more accurate representation of the posterior distribution.

Table 2. The performance of difference of filter algorithms in terms of RMSE and time cost.

Method	RMSE			Times(s)
	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}\mathit{Y}$	--
PF	22.5276	25.0796	33.7886	0.0463
APF	26.2716	27.4077	38.0666	0.0924
MPF	23.0830	25.8360	34.7122	0.9856
OOSM	22.0741	24.0055	32.6898	0.0736
BCPS-PF	20.0280	22.4909	30.1838	0.0838

presents the RMSE results for different algorithms over the entire time steps. The novel particle filter using the posterior distribution as the proposal distribution demonstrates a superior RMSE performance compared with the other filter algorithms. When ambiguous observations are detected and excluded, the RMSE performance of the OOSM algorithm slightly surpasses that of the standard particle filter. In contrast, the APF algorithm shows the poorest performance due to its excessive reliance on observations. The standard particle filter has the lowest time cost, while the time costs of the APF and the novel particle filter are similar. Notably, the positioning accuracy of the novel particle filter is improved by 10% compared with the standard particle filter.

6. Conclusions

In this paper, we addressed the challenges associated with the multimodal distribution of elevation observations in terrain referenced navigation (TRN). The fluctuating nature of terrain introduces uncertain noise probability distributions, making nonlinear filtering methods more suitable for fusing TRN and IMU data. Traditional PF often suffers from poor accuracy and robustness due to the difficulty in balancing particle degradation and diversity. To tackle these issues, we developed BCPS-PF, which indirectly uses the posterior distribution as the proposal distribution. This approach eliminates the need for resampling and bypasses the requirement to propagate weights during the recursive Bayesian update. Instead, it relies solely on particles that satisfy the posterior distribution to accurately estimate the current state. Our proposed algorithm was specifically adapted for TRN applications. Simulations demonstrated its effectiveness, showing a 10% improvement in positioning accuracy compared with existing algorithms. This significant enhancement underscores the algorithm’s potential for improving the reliability and precision of TRN systems.