Robust Bluetooth AoA Estimation for Indoor Localization Using Particle Filter Fusion

Abstract

With the growing demand for positioning services, angle-of-arrival (AoA) estimation or direction-finding (DF) has been widely investigated for applications in fifth-generation (5G) technologies. Many existing AoA estimation algorithms only require the measurement of the direction of the incident wave at the transmitter to obtain correct results. However, for most cellular systems, such as Bluetooth indoor positioning systems, due to multipath and non-line-of-sight (NLOS) propagation, indoor positioning accuracy is severely affected. In this paper, a comprehensive algorithm that combines radio measurements from Bluetooth AoA local navigation systems with indoor position estimates is investigated, which is obtained using particle filtering. This algorithm allows us to explore new optimized methods to reduce estimation errors in indoor positioning. First, particle filtering is used to predict the rough position of a moving target. Then, an algorithm with robust beam weighting is used to estimate the AoA of the multipath components. Based on this, a system of pseudo-linear equations for target positioning based on the probabilistic framework of PF and AoA measurement is derived. Theoretical analysis and simulation results show that the algorithm can improve the positioning accuracy by approximately 25.7% on average.

1. Introduction

Indoor positioning is a critical component of a positioning system [1] that provides information about the position of a moving target at any given moment. Global navigation satellite systems (GNSSs), including the Global Positioning System (GPS), GLONASS, and Galileo, perform well in outdoor environments but poorly in indoor environments because the GNSS signals cannot enter buildings [2,3]. Due to the limitations of GNSSs, the indoor positioning problem has attracted much attention from the industry and academia. Three-dimensional indoor localization is in urgent demand, and in order to solve the indoor localization problem, several localization systems have been proposed and implemented using different techniques and approaches, including the utilization of multi-sensor-based localization-related techniques [4,5]. Although many existing systems try to solve this problem using a wide variety of techniques, indoor positioning systems based on wireless sensor networks (WSNs) are one of the most adopted solutions, including some specific IEEE standard radio signals such as Wi-Fi, ZigBee, Bluetooth, UWB, and RFID [6,7,8,9], which have been widely used in many real-world applications such as medical monitoring, target tracking, navigation, emergency call services, and transport. The vast majority of solutions for such systems are solved by standard-ranging techniques such as Received Signal Strength (RSS) [10], Time of Arrival (TOA), Time Difference of Arrival (TDOA), and AoA [11].

In the application of fifth-generation (5G) technology, Bluetooth Low Energy (BLE) is a key wireless technology for IoT systems. It is widely used in consumer electronic devices due to its advantages in terms of cost, efficiency, and availability. The Bluetooth Core Specification v5.1 introduces new capabilities that support high-accuracy direction-finding. The Angle of Departure (AoD) represents the direction from which the signal is sent from the transmitting device, while the Angle of Arrival (AoA) represents the direction from which the signal reaches the receiving device. The article mentions that direction of arrival (DoA) positioning technology is based on the 2D plane to obtain the angle $\theta$, while AoA mostly represents three-dimensional space angle estimation $(\theta ,\phi )$. The Constant Tone Extension (CTE) [12] has been added to BLE packets. With the help of the Switched Antenna Array (SAA), BLE devices can estimate the direction of the incident wave by utilizing the phase difference information of the signals received by different sensors. This effectively enables Angle of Arrival (AoA) estimation, eliminating the reliance on signal time synchronization without excessive hardware cost. Multi-signal classification (MUSIC) algorithms based on subspace methods, as the most referenced approach [13], can provide highly accurate estimation under high signal-to-noise ratio (SNR) conditions. However, it still faces challenges associated with high computational complexity and sensitivity to noise due to multipath and non-line-of-sight (NLOS) propagation. To overcome this challenge, various improved MUSIC algorithms [14,15,16] and advanced subspace methods have been explored. For example, the root-MUSIC algorithm estimates the direction of arrival (DoA) by finding polynomial roots and exhibits superior complexity and resolution compared to MUSIC [17]. The literature [18] proposes a sectoral You music method to reduce the algorithm complexity and efficiently realize fast DoA estimation. A previous study [19] used the Support Vector Machine (SVM) algorithm to improve DoA estimation accuracy with a small sample size. Wang et al. [20] introduced a novel sparse Bayesian learning method that improves the estimation accuracy with a low signal-to-noise ratio and a limited number of snapshots. Another study [21] used support vector regression (SVR) for adaptive target detection to obtain a system model related to target azimuth and radar data features. In [22], a deep neural network (DNN) was used to achieve higher DoA estimation accuracy with lower algorithmic complexity. This study demonstrated the generalization and effectiveness of the algorithm in various scenarios. Barthelme and Utschick [23] used a neural network and subarray covariance matrices to estimate the covariance matrix of the entire array. This approach produced additional DoA estimation results for multiple targets. Of the above methods, the traditional subspace methods rely heavily on a priori knowledge, such as the number of targets, and face limitations in the absence of such knowledge. The Music and Capon algorithms lack effective handling of low signal-to-noise ratio signals highly susceptible to high noise components, which affects the accuracy of target DoA estimation. Algorithms such as SVM, SVR, and Bayesian methods achieve target direction estimation by fitting a complex mapping relationship between the incident direction of the signal and the signal features. However, as the signal-to-noise ratio decreases, noise significantly affects the signal, complicating the relationship between the incident angle and the signal. This situation requires many samples for accurate function fitting, which is challenging. As the sample size increases, the information redundancy intensifies, making machine learning algorithms unable to estimate the DoA accurately. These issues also hinder the effectiveness of large-scale models such as NNs and DNNs in DoA estimation. In most cellular localization systems, AoA is usually not used as a primary localization parameter but as a secondary one.

Particle filters (PFs), introduced [24] as a numerical approximation to the nonlinear Bayesian filtering problem, can robustly track objects moving with accurate trajectories while automatically maintaining tracking associations. It can approximate the system state’s posterior distribution even if the posterior’s exact form is unknown. It may require a large number of particles and a large number of computations to achieve acceptable accuracy. Although particle-filtering algorithms have a broad application background in the target tracking problem, the research on tracking and navigation for a pure direction angle is still in the exploratory stage. Most of the moving targets in space are localized by coordinate positions, and there is a nonlinear problem in the transformation of coordinate positions and direction angles in space. For the nonlinear interference factors, particle filtering provides a good solution.

In this paper, a spatial multipath array model is proposed by fusing particle filtering algorithms and AoA measurements, and the mathematical model is built in a single reflection scenario where the locations of the localization station and the reflector are known. The system combines a robust adaptive angle estimation algorithm with a priori knowledge of target motion into a probabilistic framework of PF to localize a moving target. The shortcoming of the traditional AoA algorithm, which only localizes the stationary signal source, has been overcome. Then, a weight calculation function based on the Gaussian function is designed to reasonably calculate the weights of the particles and reduce the particle scarcity. It is verified that our algorithm can realize the tracking navigation of the target, and the algorithm is robust to the nonlinear model of the moving target.

2. Array Model

The realm of Bluetooth signal transmission through wireless channels is a labyrinth of complexity. A comprehensive understanding of the physical environment is a prerequisite for establishing a mathematical model, a task that often proves challenging. To derive a practical parameterized model, we must resort to simplifying assumptions for signal transmission, primarily encompassing array assumptions and spatial source signal assumptions.

(1) Assumption of signal array:

The array elements are independent of each other and have the same characteristics. The signal received by each array element has nothing to do with its size. It is only related to its position, and each array element contains white noise independent of the signal.

(2) Assumption of spatial signal source:

The signal propagates in a straight line in a uniform medium with the same direction, and there is no multipath effect. The signal waveform can be regarded as a beam of plane waves compared with the array. Azimuth and elevation angles are commonly used to indicate the direction of the beam coming in three dimensions. Currently, commonly used signal arrays for 3D spatial direction angle estimation generally use L-type arrays, cross-cross arrays, face arrays, etc. In this paper, we mainly estimate the spatial direction angle of the signal source based on the L-type array.

Assume that $K$ uncorrelated narrowband signals with the same carrier frequency act on the x- and y-axes of a homogeneous surface array of $M$ array elements, and the line array is vertically formed. The isotropic array elements are aligned along the x- and y-axes in the direction of the arriving wave to form an equally spaced linear array. Among them, the spacing between the Bluetooth signal array elements is $d$, as shown in Figure 1.

$\left({\theta }_i,{\varphi }_i\right),(i=1,\dots ,K)$ is the angle of incidence, where ${\theta }_i$ and ${\varphi }_i$ represent the azimuth and elevation angles of the i-th signal, respectively. For narrowband signals, the time delay can be converted to a phase shift, and the time delay of an array element at position $x_m\left(m=1,2,\dots ,M\right)$ concerning the first array element is

$${\tau }_{mk}={\displaystyle \frac{1}{c}}\left(x_m{\sin \varphi }_k\right)$$

(1)

and the spacing between every two neighboring array elements is $d$, for which there is Equation (2):

$${\tau }_{mk}={\displaystyle \frac{d{\sin \varphi }_k}{c}}\left(m-1\right)$$

(2)

In the array, the first array element is located at the origin. Taking the position of the first array element as the reference point, at time t, the spatial signal representation of the i-th signal arriving at the sensor of the reference array element is as follows:

$$s_i\left(t\right)=z_i\left(t\right)e^{j{\omega }_{0}t},i=1,\dots ,K$$

(3)

where $z_i\left(t\right)$ denotes the i-th signal complex envelope containing the angle information of the spatial signal at the time t. $e^{j{\omega }_{0}t}$ represents the source carrier. When the signal is a narrowband acoustic source signal, it satisfies $z_i(t-\tau )\approx z_i\left(t\right)$. Then $s_i(t-\tau )$ can be expressed as:

$$s_i\left(t-\tau \right)=z_i\left(t-\tau \right)e^{j{\omega }_0\left(t-\tau \right)}\approx z_i\left(t\right)e^{j{\omega }_0\left(t-\tau \right)},i=1,\dots ,K$$

(4)

The vector that models the phase shift of each antenna based on AoA is generally referred to as the array streamer vector or the steering vector. $x_m\left(t\right)$ is denoted as the data received by the m-th array element in the array, as shown below:

$$x_m(t)=\sum _{i=1}^K{s}_i\left(t-{\tau }_{mi}\right)+n_m(t)$$

(5)

where ${\tau }_{mi}$ represents the time delay from the i-th signal to the m-th array element with respect to the reference array element, and $n_m\left(t\right)$ represents the additive noise of the m-th array element. From the above, the received signals of the array on the x-axis and y-axis are, respectively:

$$\begin{array}{c}X=A_{x}S+N_x\\ Y=A_{y}S+N_y\end{array}$$

(6)

where $S\in {\mathbb{C}}^{1\times N}$ represents the array signal data. $N_x\in {\mathbb{C}}^{1\times N},N_y\in {\mathbb{C}}^{1\times N}$ are the received noise, and the value of the orientation matrix $A_x\in {\mathbb{C}}^{M\times 1}$ is:

$$A_x=\left[\begin{array}{cccc}1& 1& \dots & 1\\ e^{j2\pi d{\cos \theta }_1{\sin \varnothing }_1/\lambda }& e^{j2\pi d{\cos \theta }_2{\sin \varnothing }_2/\lambda }& \dots & e^{j2\pi d{\cos \theta }_K{\sin \varnothing }_K/\lambda }\\ ⋮& ⋮& \ddots & ⋮\\ e^{j2\pi (M-1)d{\cos \theta }_1{\sin \varnothing }_1/\lambda }\dots & e^{j2\pi (M-1)d{\cos \theta }_2{\sin \varnothing }_2/\lambda }& & e^{j2\pi (M-1)d{\cos \theta }_K{\sin \varnothing }_K/\lambda }\end{array}\right]$$

(7)

Since the x-axis and y-axis share the array at the origin, the orientation matrix $A_y\in {\mathbb{C}}^{(M-1)\times 1}$ is represented as:

$$A_y=\left[\begin{array}{cccc}1& 1& \dots & 1\\ e^{j2\pi d{\cos \theta }_1{\sin \varnothing }_1/\lambda }& e^{j2\pi d{\cos \theta }_2{\sin \varnothing }_2/\lambda }& \dots & e^{j2\pi d{\cos \theta }_K{\sin \varnothing }_K/\lambda }\\ ⋮& ⋮& \ddots & ⋮\\ e^{j2\pi (M-1)d{\cos \theta }_1{\sin \varnothing }_1/\lambda }\dots & e^{j2\pi (M-1)d{\cos \theta }_2{\sin \varnothing }_2/\lambda }& & e^{j2\pi (M-1)d{\cos \theta }_K{\sin \varnothing }_K/\lambda }\end{array}\right]$$

(8)

$A_x$ and $A_y$ are combined, as indicated below:

$$A=\left[\begin{array}{c}{A}_x\\ A_y\end{array}\right]=\left[a_1,a_2,\dots ,a_K\right]$$

(9)

Jointly, the corresponding representation of the signals received by the whole array can be obtained as follows:

$$G=AS\left(t\right)+N\left(t\right)$$

(10)

where $S\left(t\right)$ and $N\left(t\right)$ represent the K source signals and additive noise at time t, denoted as follows:

$$\begin{array}{c}S\left(t\right)={\left[s_1\left(t\right),s_2\left(t\right),\dots ,s_K\left(t\right)\right]}^T\\ N\left(t\right)={\left[n_1\left(t\right),n_2\left(t\right),\dots ,n_M\left(t\right)\right]}^T\end{array}$$

(11)

3. Methods

3.1. MVDR

As one of the core tasks of array signal processing technology, beam forming can realize functions such as beam steering, interference and noise suppression, target identification and positioning, etc. The MVDR beamforming proposed by Capon is a commonly used algorithm [25], which weights and sums the output of the array and focuses the gain in one direction. The appropriate filter coefficients are chosen to minimize the average power at the array’s output under the constraint of the desired signal without distortion. Figure 2 shows the basic structure of the algorithm.

Assuming that the desired signal is undistorted, appropriate filter coefficients must be selected to minimize the average power of the array output. The MVDR weight optimization problem can be expressed as:

$$\underset{w}{\mathrm{m}\mathrm{i}\mathrm{n}}{w}^H{R}_{i+n}w\mathrm{s}.\mathrm{t}.w^H{a}_0=1$$

(12)

The above MVDR optimization problem can be solved using the Lagrange multiplier method as follows:

$$w_{opt}={\displaystyle \frac{R_{i+n}^{-1}{a}_0}{a_0^H{R}_{i+n}^{-1}{a}_0}}$$

(13)

where $w={(w_1,\dots w_M)}^T$ is the complex weighting vector of the beamformer and $R_{i+n}^{-1}$ and $a_0$ represent the ideal interference plus noise covariance matrix and the precise desired signal steering vector. In practice, they are usually unavailable, so the sampled covariance matrix ${\tilde{R}}^{-1}$ and the nominal guiding vector ${\overline{a}}_0$ based on the array geometry computed are commonly used instead [26], and in this case, the MVDR beamformer is also referred to as the sampled covariance matrix inversion (SMI) beamformer [11]:

$$w_{SMI}=a{\tilde{R}}^{-1}{\overline{a}}_0={\displaystyle \frac{{\tilde{R}}^{-1}{\overline{a}}_0}{{\overline{a}}_0^H{\tilde{R}}^{-1}{\overline{a}}_0}}$$

(14)

$a={\displaystyle \frac{1}{{\overline{a}}_0^H}}$ The output sequence of the beamformer is:

$$y\left(k\right)=w_{SMI}^{H}x\left(k\right)$$

(15)

The output SINR of the beamformer is:

$$\begin{array}{c}SINR\triangleq {\displaystyle \frac{E\left[w^H{x}_s\left(k\right)x_s^H\left(k\right)w\right]}{E\left[w^H{x}_{i+n}\left(k\right)x_{i+n}^H\left(k\right)w\right]}}\\ ={\displaystyle \frac{w^H{R}_{s}w}{w^H{R}_{i+n}w}}={\displaystyle \frac{{\sigma }_0^2{\left|w^H{a}_0\right|}^2}{w^H{R}_{i+n}w}},\end{array}$$

(16)

${\sigma }_0^2=E\left[s_i\left(k\right)s_i(k{)}^H\right]$ is the expected signal power. The array pattern of the Capon beamformer represents the response relationship between the complex weighted vector and the incident signal in a given direction.

Also known as the beam response, it can be expressed as:

$$B\left(\theta \right)=w^{H}a\left(\theta \right)$$

(17)

The overall execution process steps of the MVDR algorithm, designed with a systematic approach, are as follows:

(1)

Conduct L snapshot observations of the signal source at time t and use the formula $R_k={\displaystyle \frac{1}{L}}{\displaystyle \sum ^{\underset{L}{i=1}}}{G}_{ki}{G}_{ki}^H$ to construct a covariance matrix from the 2M − 1 signal data received by the array.

(2)

Calculate the inverse matrix or pseudo-inverse matrix of the covariance matrix to represent the relationship between signals.

(3)

Calculate the weight vector, which is the inverse matrix (or pseudo-inverse matrix) of the covariance matrix and the received signal.

(4)

Sort according to the size of the eigenroots, take the eigenvectors corresponding to the first K larger eigenvalues to form the signal subspace, and the remaining eigenvectors are the noise subspace.

(5)

Change $(\theta ,\varphi )$, and calculate the spectral function according to the formula to find the position of the maximum value, and then determine the source’s azimuth angle and elevation angle.

3.2. Particle Filter

Particle filtering (PF) stands out as a unique approach where a probability distribution is represented by a set of random samples, known as particles, drawn from the distribution. This method’s strength lies in its ability to easily model nonlinear transformations of random variables, making it a perfect fit for target-tracking problems. Particle filtering is a fusion of the Monte Carlo method and Bayesian estimation theory, serving as an approximate algorithm of Bayesian estimation based on sampling theory. The basic idea is to find a group of random samples with weights in the state space to approximate the conditional posterior probability density $p\left(x_k\mid z_{1:k}\right)$, and replace $E\left(x_k\mid z_{1:k}\right)$ with the sample mean to obtain the minimum variance estimate of the state.

Furthermore, ${\left\{x_k^j,W_k^j\right\}}_{j=1}^N$ (with N as the number of particles and $W_k$ as the weight of the particles) is the random variable describing the posterior probability $p\left(x_k\mid z_{1:k}\right)$ and is the normalized weight of the particle. The state estimation result at time k can be expressed as:

$${\overline{x}}_k=\sum ^{\underset{N}{j=1}}{x}_k^j{\tilde{W}}_k^j$$

(18)

The update process of particles is divided into two parts; the update of $x_k^j$ is sampled from the important density function $q\left(x_k\mid x_{1:k-1}^j,z_{1:k}\right)$. The update of $u_k^j$ is obtained by a one-step transfer of the first-order Markov chain.

Using the prior probability density as the important density function, even if $q\left(x_k^J\mid x_{k-1}^J,z_{1:k}\right)=p\left(x_k^j\mid x_{k-1}^j\right)$, the weight updating process can be described as $W_k^j\propto {\tilde{W}}_{k-1}^{j}p\left(z_k\mid x_k^j\right)$.

3.3. MVDR+PF Nonlinear Dynamical System Modeling

The particle filter algorithm needs to build a model of the system in order to predict its state. When the system model is challenging to obtain, an accurate model can be constructed by constructing an approximate model. This paper mainly implements the system model construction of moving targets in angle estimation navigation in the form of state equations and measurement equations.

• Establishment of state equations

The motion of the target object generally includes uniform velocity motion and variable velocity motion. To better describe the operation of the object, the state of the target is described by the angular position quantity, angular velocity quantity, and angular acceleration quantity in this study. Assuming that the target object moves along a straight line in a short period, the sampling time is $\mathsf{\Delta }T$, the true azimuth angle of the target at the sampling moment $k\mathsf{\Delta }T$ is $\theta \left(k\right)$, the target’s movement speed is $\dot{\theta }(k)$, and the acceleration is $\ddot{\theta }(k)$. The formula for the change in the target’s angular state can be expressed as:

$$\begin{array}{c}\theta (k+1)=\theta (k)+\dot{\theta }(k)\mathsf{\Delta }T+1/2\ddot{\theta }(k)\mathsf{\Delta }{T}^2+1/6\nu \mathsf{\Delta }{T}^3\hfill \\ \dot{\theta }(k+1)=\dot{\theta }(k)+\ddot{\theta }(k)\mathsf{\Delta }T+1/2v\mathsf{\Delta }{T}^2\hfill \\ \ddot{\theta }(k+1)=\ddot{\theta }(k)+\mathsf{\Delta }T𝒱\hfill \end{array}$$

(19)

Among them, $𝒱$ represents the process noise of the system, which is independent of the observation noise and satisfies the Gaussian distribution of $𝒱\sim N(0,Q)$. Noise causes fluctuations in angle, angular velocity, and angular acceleration. The state space model of the target motion is obtained by combining the expressions, and the expression is as follows:

$$\left[\begin{array}{c}\theta (k+1)\\ \dot{\theta }(k+1)\\ \ddot{\theta }(k+1)\end{array}\right]=\left[\begin{array}{ccc}1& \mathsf{\Delta }T& 1/2\mathsf{\Delta }{T}^2\\ 0& 1& \mathsf{\Delta }T\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\theta (k)\\ \dot{\theta }(k)\\ \ddot{\theta }(k)\end{array}\right]+\left[\begin{array}{c}1/6\mathsf{\Delta }{T}^3\\ 1/2\mathsf{\Delta }{T}^2\\ \mathsf{\Delta }T\end{array}\right]𝒱$$

(20)

We expand the signal source into three-dimensional space, and the target direction angle includes the azimuth angle and the elevation angle. The state x is expressed as:

$$x=[\theta ,\dot{\theta },\ddot{\theta },\phi ,\dot{\phi },\ddot{\phi }{]}^T$$

(21)

The state vector at time $k$ is expressed as:

$$x_k=A{x}_{k-1}+B\nu$$

(22)

Among them, the state transition matrix is:

$$A=\left[\begin{array}{ccc}1& \mathsf{\Delta }T& 1/2\mathsf{\Delta }{T}^2\\ 0& 1& \mathsf{\Delta }T\\ 0& 0& 1\\ 1& \mathsf{\Delta }T& 1/2\mathsf{\Delta }{T}^2\\ 0& 1& \mathsf{\Delta }T\\ 0& 0& 1\end{array}\right],B=\left[\begin{array}{c}1/6\mathsf{\Delta }{T}^3\\ 1/2\mathsf{\Delta }{T}^2\\ \mathsf{\Delta }T\\ 1/6\mathsf{\Delta }{T}^3\\ 1/2\mathsf{\Delta }{T}^2\\ \mathsf{\Delta }T\end{array}\right]$$

(23)

• Establishment of observation model

The observation model is another important model in the particle filter framework, which is expressed in the form of observation equations. The observation equation mainly solves the matching degree between the predicted state and the observed state of the moving target, and then updates the state prediction estimate of the target. Most observation models are obtained in the form of data collected by sensors. The DOA navigation and tracking method for moving targets collects signals containing status and noise information of moving targets through array sensors and builds a system observation model through the collected data. The corresponding representation is as follows:

$$G_k=f\left(x_k,\omega \right)$$

(24)

Among them, $x_k$ represents the status information of the target at the time $k$ of the system, and $\omega$ represents the observation noise, which is independent of the process noise. The establishment of the observation model of moving objects is based on the signal data received by the array. In this paper, the L-shaped array model is used as the signal model, and the original data of signal observation obtained are

$$G=AS\left(t\right)+N\left(t\right)$$

(25)

where $S\left(t\right)$ is the source signal, $A$ is the steering matrix of the array, and $N\left(t\right)$ is the observation noise of the array elements in the array. In actual situations, the signal is often collected in multiple snapshots to determine the effectiveness of the collected signal and the algorithm’s accuracy. Assuming that the signal is observed in snapshots at the time $k$, the observation output matrix of the array is:

$$G_k=\left[G\right(kL+1), G(kL+2), \dots , G(kL+L)$$

(26)

The covariance matrix of the original data can be obtained from the above formula, expressed as follows:

$$R_k={\displaystyle \frac{1}{L}}{G}_k{G}_k^H$$

(27)

The above equation represents the observation equation of the system. To further explain the quantities in the moving target DOA navigation and tracking algorithm based on the particle filter algorithm, the entire algorithm design mainly includes the relationship between three quantities: the observed value, the predicted value, and the actual value. The proper value represents the true direction position of the target source, the expected value represents the result value obtained by the particle filter algorithm, and the observed value is the measured value of the observation equation that reflects the size of the actual value. The relationship between the three is shown in Figure 3.

In summary, the equations of the system model in the moving target DOA navigation and tracking algorithm based on the particle filter algorithm represent the prior probability and conditional probability expressions in the particle filter algorithm. Among them, $p\left(x_k\mid x_{k-1}\right),p\left(y_k\mid x_k\right)$ essentially represents the state equation and observation equation in this section. The robust angle estimation fusion algorithm proposed in this article uses the primary particle filter algorithm process as the basic implementation framework. The algorithm design is carried out by setting state conditions and parameters, which can be carried out in Figure 4.

1. Initialization

Assume there are particles $N$, the number of snapshots is $L$, and the total number of iterations is $Y$;

For $i=1:N$, initialize the particle state, azimuth angle ${\theta }_0(i)\sim U[0,\pi ]$;

The pitch angle ${\phi }_0(i)\sim U[0,\pi /2],{\dot{\phi }}_0(i),\dot{{\theta }_0}(i)\sim N\left({\mu }_0,{\epsilon }_0^2\right),i=1,\dots ,N,U\left[e_1,e_2\right]$ is evenly distributed, $N\left(\mu ,{\epsilon }^2\right)$ normal distribution, ${\ddot{\theta }}_0(i)$ and ${\ddot{\phi }}_0(i)$ are initialized to 0, and the weight value is 1/N;

2. Iteration: for $i=1:Y$

(a) Obtain observation data

For $i=1:L;$ obtain the observation data of the system through the array at time K, obtain the data covariance matrix according to $R_k={\displaystyle \frac{1}{L}}{G}_k{G}_k^H$;

(b) Importance sampling stage

For the transfer equation according to status $q\left(x_k\left(i\right)\mid x_{0:k-1}\left(i\right),Y_{1:k}\right)=p\left(x_k\left(i\right)\mid x_{k-1}\left(i\right)\right)$ carry out sampling $\left({\theta }_k\left(i\right),{\phi }_k\left(i\right)\right)$,

For $i=1:N$; update the weight of the particle based on the weight calculation formula $W_k\left(x_k^{\left(i\right)}\right)=W_{k-1}\left(x_{k-1}^{\left(i\right)}\right)p\left(y_k\mid x_k\right)$

Normalize the weights of particles according to ${\tilde{W}}_k\left(x_k^{\left(i\right)}\right)={\displaystyle \frac{W_k\left(x_k^{\left(i\right)}\right)}{{\displaystyle \sum ^{\underset{N}{i=1}}}{W}_k\left(x_k^{\left(i\right)}\right)}}$

(c) Algorithm resampling stage

Resample the particle set $\left\{x_k^{\left(i\right)},w_k\left(x_k^{\left(i\right)}\right)\right\}$ and reinitialize the weight values.

Get new particle set $\left\{x_k^{\left(i\right)},w_k\left(x_k^{\left(i\right)}\right)\right\}$

(d) Result output

Output the prediction results according to $x_k={\displaystyle \sum ^{\underset{N}{i=1}}}{w}_k\left(x_k^{\left(i\right)}\right)x_k^{\left(i\right)}$

(e) Reinitialize weights

For $i=1:N$, reset the weight of the particles $w_k\left(x_k^{\left(i\right)}\right)={\displaystyle \frac{1}{N}}$

3. End.

4. Simulation Experiments and Analyses

For the navigation and positioning problem based on moving targets, a simulation experiment was conducted to verify the effectiveness of the proposed algorithm. In this chapter, we tested the effectiveness of an improved Bluetooth AoA navigation algorithm. This algorithm combines particle filtering and the MUSIC algorithm, and we evaluated it through experimental simulation. The main aim of the simulation was to assess how the algorithm performs under different signal-to-noise ratios, numbers of snapshots, and numbers of particles when the target object moves at either a uniform or variable speed. The simulation experiment showed that, with a certain number of particles and snapshots, the improved algorithm can accurately and consistently navigate and track targets with uniform and variable speeds in space.

1. Simulation experiment on the impact of the target object’s uniform motion on the particle filter-based DOA navigation and tracking algorithm for moving targets.

Experimental conditions: L-shaped acoustic array, four array elements on each X-axis and Y-axis, and 0.2 m between array elements (the distance between array elements is greater than half the wavelength of the signal). The initial azimuth and elevation angles were set at (45°, 45°), and the wave signal was assumed to be a parallel wave. The number of snapshots was 256, the number of particles was 800, and observation noise was introduced with a signal-to-noise ratio of 10 dB.

The entire process was iterated over 50 steps. Figure 5a shows the deviation between the real trajectory of the target object in the spatial direction and the trajectory predicted by the algorithm. Figure 5b,c shows the relationship between the predicted angle and the true angle of each iteration from two angles: the azimuth angle and the overhead angle.

The results of the figure show that, except for the target’s starting position, the direction angle of the target object predicted by the algorithm deviated significantly, and the deviation between the predicted angle and the true angle at the remaining time is small. Therefore, it is concluded through simulation that under uniform speed conditions, the moving target DOA navigation and tracking algorithm based on particle filtering has good navigation and tracking performance for moving targets.

2. The effect of the target object under variable speed motion on the improved DOA navigation algorithm for Bluetooth signals based on particle filtering algorithm.

Experimental conditions: Assuming that the target object is stationary at the initial moment based on the conditions of Experiment 1, the initial velocity of each particle is modified to be 0.3°⁄s, and the acceleration is randomly transformed in the range of [−0.5, 0.5]. To better observe the tracking effect of the algorithm, the initial position of the particles is initialized as the real position. The simulation results are shown in Figure 6.

Figure 6a shows the deviation of the target object orientation trajectory and the algorithm-predicted trajectory in terms of the spatial orientation angle in the variable-speed condition. Figure 6b shows the prediction results of the algorithm’s azimuth and elevation angles on the target’s true angle under the variable speed condition. As can be seen from the change in the spatial angular position of the target in Figure 6a, the target object experienced accelerated motion, a decelerated motion process, and a reverse motion state during the whole moving process. In the simulation process, the trend of the direction change in the predicted curve and the trend of the target curve change is consistent, and the azimuth and elevation angles fluctuate around the actual value with small errors. It is concluded that in the case of variable speed motion of the target object, the algorithm can still predict the direction of the target correctly and track the target accurately.

3. The variation in RMSE with particles is assumed to be between the number of particles [100, 1000], the number of iterations is 20, and the number of particles increases by 50 each time compared to the previous one. In order to better observe the tracking effect of the algorithm, the initial position of the particle is initialized to be the actual position. The effect of the algorithm is judged by the root mean square (RMS) [24], and the corresponding formula for RMS is shown in Equation (28).

$$Rmes={\displaystyle \frac{1}{n}}\sqrt{{\displaystyle \frac{1}{50}}{\sum }_{j=1}^{50}[{\left(P{\theta }_i-T{\theta }_i\right)}^2+{\left(P{\varphi }_i-T{\varphi }_i\right)}^2]}$$

(28)

where $n$ represents the number of iterations the algorithm has gone through. $P{\theta }_i$ and $P{\varphi }_i$ represent the predicted values of the azimuth and dip angles in step $i$; $T{\theta }_i$ and $T{\varphi }_i$ represent the real values of the azimuth and dip angles. The simulation results of the experiment are shown in Figure 7.

From Figure 7, it can be seen that with the increase in the number of particles, the error between the navigation trajectory and the actual trajectory becomes smaller and smaller and the localization accuracy is gradually improved. Comparing the RMSE values of the algorithm under the uniform speed condition and the variable speed condition, the accuracy of the algorithm under the uniform speed condition is higher, which means that the algorithm under the uniform speed condition is more likely to converge, which is in line with the actual situation. In the uniform speed condition, the algorithm results have been stabilized when the number of particles is 400. Under the variable speed condition, the algorithm starts to stabilize when the number of particles is 800, and the fluctuation of the algorithm is evident when the number of particles is less than 800. Therefore, the number of particles in Experiment 1 and Experiment 2 is chosen to be 800.

4. Examining the variation of RMSE of the algorithm proposed in this paper concerning the signal-to-noise ratio, the number of snapshots is taken as 1000, and the signal-to-noise ratio is varied from −5 dB to 30 dB; the number of particles is fixed at 800.

5. We examine the variation of RMSE of the algorithm proposed in this paper with the number of snapshots. The number of snapshots is between [50 and 2000], the number of iterations is 20, and the number of snapshots increases by 100 each time compared with the previous one. In order to better observe the tracking effect of the algorithm, the initial position of the particle is initialized to be the actual position; the remaining conditions are the same as in Experiment 1.

From Figure 8, it can be seen that the algorithm has good estimation accuracy when the signal-to-noise ratio ranges from 15 dB to 30 dB and the number of snapshots changes from 50 to 2000. The estimation accuracy of the algorithm becomes higher and higher as the signal-to-noise ratio and the number of snapshots increase. This is because the RMSE of the algorithm converges to the CRB bound as the signal-to-noise ratio and the number of snapshots increase.

6. The algorithm proposed in this paper is compared with two algorithms, single particle filter localization and angle estimation localization. Figure 9 shows the cumulative error distribution curves of the three algorithms. The CDF for horizontal localization is used as a performance metric for localization evaluation, defining the localization error as:

$$e=\sqrt{{\left(\widehat{x}-x_{true}\right)}^2+{\left(\widehat{y}-y_{true}\right)}^2}$$

(29)

From the figure, the localization performance of the proposed algorithm is better than that of the other two localization algorithms. The proposed method enables 90% of the users to locate within 3 m, while for the other two localization algorithms, the error is more than 5.6 m; the algorithm can improve the positioning accuracy by approximately 25.7% on average. From the CDF distribution curve, it can be seen that 47% of the target points are in LOS propagation. The proposed algorithm can fully estimate the target source location with the highest positioning accuracy.

5. Conclusions

Some of the attractive advantages of PF include its generality and ability to handle multimodal probability distributions and nonlinear functions. The system incorporates mvdr, a robust AoA estimation algorithm with a priori knowledge of the target’s motion model, into the probabilistic framework of the PF to achieve localization of a moving target. The system is first modeled from the target’s state equations as well as the observation equations. Through the system model, the target object’s behavioral information and the signal source’s transient information are combined to overcome the defects of the traditional DoA algorithm that only localizes the fixed signal source. Then, to reasonably calculate the weights of the particles and reduce the particle scarcity, a particle filtering weight calculation function based on the Gaussian function is designed to resample the function. The effectiveness of the algorithm is verified through simulation. It is verified that the algorithm can realize the tracking navigation of the target and is strongly robust to the nonlinear model of the moving target.

Although the algorithm’s feasibility is demonstrated through MATLAB simulation in this article, there are still several shortcomings in the algorithm’s research when compared to real-world conditions. There are numerous issues that need to be addressed in the future. The article outlines the future research directions as follows:

(1) In this paper, a particle filter-based algorithm for mobile target Bluetooth AoA navigation and tracking is proposed to address the challenge of tracking the movable trajectory of the target. It recognizes that coherent signal sources can interfere with Bluetooth AoA navigation and tracking technology. Therefore, it suggests that future work should consider the impact of coherent signals on the algorithm and propose effective methods to address this issue.

(2) Additionally, the paper examines the relationship between the spatial coordinates of the target object and its directional angle in space. It constructs a typical motion navigation trajectory for the target object and verifies the navigation and tracking algorithm through simulations in typical scenarios. Although the results demonstrate basic navigation and tracking of the target trajectory, the algorithm’s performance is less than satisfactory when the directional trajectory of the target object undergoes significant changes.

(3) When designing the particle filter-based mobile target Bluetooth AoA navigation tracking algorithm, it is assumed that the signal of a single target object exists. However, there is insufficient research on effectively extracting valid data when there are multiple targets, and real experimental verification is lacking. Real-world scenarios present more influencing factors compared to simulation scenarios, thus potentially affecting the algorithm in unexplored ways. To enhance the algorithm’s robustness, it is essential to incorporate more influencing factors and carry out real experimental tests.