Robust Bluetooth AOA Estimation for Indoor Localization Using Particle Filter Fusion

1. Introduction

Indoor positioning is a key component of a positioning system [1] that provides information about the position of a moving target at any given moment. Global navigation satellite systems (GNSS), 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 GNSS, the problem of indoor positioning has attracted much attention from both industry and academia. 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 [4,5,6,7] 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 common ranging techniques such as Received Signal Strength (RSS) [8], Time of Arrival (TOA), Time Difference of Arrival (TDOA) and AoA [9].

In the application of the fifth generation (5G) technology, Bluetooth Low Energy (BLE) is a key wireless technology for IoT systems, which is widely used in consumer electronic devices due to its advantages in terms of cost, efficiency and availability. In order to improve the positioning accuracy, Constant Tone Extension (CTE) has been added to BLE packets since Bluetooth standard 5.1 [10]. With the help of Switched Antenna Array (SAA), BLE devices can use the information of the phase difference of the signals received by different sensors to estimate the direction of the incident wave in order to easily perform the Angle of Arrival (AoA) estimation, which eliminates the reliance on signal time synchronization without excessive hardware cost. Multi-signal classification (MUSIC) algorithms based on subspace methods as the most referenced approach [11] 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 [12,13,14] and advanced subspace methods have been explored. For example, the root-MUSIC algorithm estimates DoA by finding polynomial roots and exhibits superior complexity and resolution compared to MUSIC [15]. In the literature [16], a sectoral You music method is proposed to reduce the algorithm complexity and efficiently realize fast DOA estimation. Literature [17] used Support Vector Machine (SVM) algorithm to improve DOA estimation accuracy with small sample size. Wang et al. [18] introduced a novel sparse Bayesian learning method which improves the estimation accuracy with low signal-to-noise ratio and limited number of snapshots. Literature [19] used support vector regression (SVR) for adaptive target detection to obtain a system model related to target azimuth and radar data features. In [20], 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 [21] used a neural network and subarray covariance matrices to estimate the covariance matrix of the entire array, an approach that 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 and are 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 a large number of samples for accurate function fitting. However, obtaining a large number of samples is challenging, and as the sample size increases, the information redundancy intensifies, resulting in machine learning algorithms being unable to accurately estimate the DOA. These issues also hinder the effectiveness of large-scale models such as NNs and DNNs to achieve DOA estimation. In most cellular localization systems, AOA is usually not used as a primary localization parameter, but as a secondary localization parameter.

Particle filters (PFs), introduced [22] as a numerical approximation to the nonlinear Bayesian filtering problem, are capable of robustly tracking objects moving with real trajectories while automatically maintaining tracking associations. It is able to approximate the posterior distribution of the system state even if the exact form of the posterior is unknown. It may require a large number of particles as well as a large number of computations to achieve acceptable accuracy. Although particle filtering algorithms have a wide application background in the target tracking problem, the research on tracking and navigation for 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 angle of arrival (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 that only localizes the stationary signal source is overcome. Then in order to reasonably calculate the weights of the particles as well as to reduce the particle scarcity, a weight calculation function based on Gaussian function is designed. 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 situation of Bluetooth signal transmission through wireless channels is very complicated. A complete description of the physical environment is the prerequisite for establishing a mathematical model, but this approach is often difficult to achieve. In order to obtain a useful parameterized model, simplifying assumptions need to be made for signal transmission, mainly including: 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 the size of the array element. It is only related to the position of the array element, and each array element contains white noise that is 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. Compared with the array, the signal waveform can be regarded as a beam of plane waves. 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 and face arrays, etc. In this paper, we mainly estimate the spatial direction angle of the signal source by 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,...,M\right)$ with respect to the first array element is

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

(1)

and the spacing between every two neighbouring array elements is $d$, there is the equation (2):

$${\tau }_{mk}=\frac{d{\mathit{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 steering vector. $x_m\left(t\right)$ is denoted as the data received by the $m-th$array element in the array is [M1] :

$$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, respectively, 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{\mathit{cos}\theta }_1{\mathit{sin}\varnothing }_1/\lambda }& e^{j2\pi d{\mathit{cos}\theta }_2{\mathit{sin}\varnothing }_2/\lambda }& \dots & e^{j2\pi d{\mathit{cos}\theta }_K{\mathit{sin}\varnothing }_K/\lambda }\\ ⋮& ⋮& \ddots & ⋮\\ e^{j2\pi (M-1)d{\mathit{cos}\theta }_1{\mathit{sin}\varnothing }_1/\lambda }\dots & e^{j2\pi (M-1)d{\mathit{cos}\theta }_2{\mathit{sin}\varnothing }_2/\lambda }& & e^{j2\pi (M-1)d{\mathit{cos}\theta }_K{\mathit{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{\mathit{cos}\theta }_1{\mathit{sin}\varnothing }_1/\lambda }& e^{j2\pi d{\mathit{cos}\theta }_2{\mathit{sin}\varnothing }_2/\lambda }& \dots & e^{j2\pi d{\mathit{cos}\theta }_K{\mathit{sin}\varnothing }_K/\lambda }\\ ⋮& ⋮& \ddots & ⋮\\ e^{j2\pi (M-1)d{\mathit{cos}\theta }_1{\mathit{sin}\varnothing }_1/\lambda }\dots & e^{j2\pi (M-1)d{\mathit{cos}\theta }_2{\mathit{sin}\varnothing }_2/\lambda }& & e^{j2\pi (M-1)d{\mathit{cos}\theta }_K{\mathit{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)

The [∙] in the above equation represents the transpose operation of the matrix.

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 [23], which weights and sums the output of the array and focuses the gain in one direction. The appropriate filter coefficients are chosen such that the average power at the output of the array is minimized under the constraint of the desired signal without distortion.

Figure 2. Beam forming basic structure.

Figure 2. Beam forming basic structure.

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

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

(12)

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

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

(13)

Where $w={(w_1,\mathrm{...}{w}_M)}^T$ is the complex weighting vector of the beamformer, $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 ${\bar{a}}_0$ based on the array geometry computed are commonly used instead [50], and in this case the MVDR beamformer is also referred to as the sampled covariance matrix inversion (SMI) beamformer [9]:

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

(14)

$a=\frac{1}{{\bar{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 \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]}\\ =\frac{w^H{R}_{s}w}{w^H{R}_{i+n}w}=\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 are as follows:

(1)

Conduct L snapshot observations of the signal source at time t, and use formula $R_k=\frac{1}{L}\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 azimuth angle and elevation angle of the source.

3.2. Particle Filter

In a particle filter (PF), a probability distribution is represented by a set of random samples, called particles, drawn from the distribution. Because it can easily model nonlinear transformations of random variables, this makes it particularly suitable for target tracking problems. Particle filtering combines the Monte Carlo method and Bayesian estimation theory. It is 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 {\mathit{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.

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

$${\bar{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 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)$ then 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

In order to predict the state of the system, the particle filter algorithm needs to build a model of the system. When the system model is difficult to obtain, a real 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.

1. Establishment of state equations

The motion of the target object generally includes uniform velocity motion and variable velocity motion, in order 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 the study. Assuming that the target object moves along a straight line in a short period of time, the sampling time is $\Delta T$，the true azimuth angle of the target at the sampling moment $k\Delta T$ is $\theta \left(k\right)$, the target’s movement speed is $\dot{\theta }\left(k\right)$ and the acceleration is $\ddot{\theta }\left(k\right)$. The formula for the change of the target’s angular state can be expressed as:

$$\begin{gathered} \theta \left(k+1\right)=\theta \left(k\right)+\dot{\theta }\left(k\right)\Delta T+\frac{1}{2\ddot{\theta }\left(k\right)\Delta T^2}+\frac{1}{6\nu \Delta T^3} \\ \dot{\theta }\left(k+1\right)=\dot{\theta }\left(k\right)+\ddot{\theta }\left(k\right)\Delta T+\frac{1}{2v\Delta T^2} \\ \ddot{\theta }(k+1)=\ddot{\theta }\left(k\right)+\Delta T\mathcal{V} \end{gathered}$$

(19)

Among them, $\mathcal{V}$ represents the process noise of the system, which is independent of the observation noise and satisfies the Gaussian distribution of $\mathcal{V}\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& \Delta T& 1/2\Delta T^2\\ 0& 1& \Delta T\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\theta \left(k\right)\\ \dot{\theta }\left(k\right)\\ \ddot{\theta }\left(k\right)\end{array}\right]+\left[\begin{array}{c}1/6\Delta T^3\\ 1/2\Delta T^2\\ \Delta T\end{array}\right]\mathcal{V}$$

(20)

Expand the signal source into three-dimensional space, the target direction angle includes azimuth angle and 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:

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

(23)

2. 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 is mainly to solve the matching degree between the predicted state and the observed state of the moving target, and then update 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 information 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 is

$$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, in order to determine the effectiveness of the collected signal and the accuracy of the algorithm, the signal is often collected in multiple snapshots. Assuming that the signal is observed in snapshots at time $k$, the observation output matrix of the array is:

$$G_k=\left[G\right(kL+1),\hspace{1em}G(kL+2),\hspace{1em}\cdots ,\hspace{1em}G(kL+L)$$

(26)

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

$$R_k=\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, namely the observed value, the predicted value and the real value. The true value represents the true direction position of the target source, the predicted 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 true 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 basic particle filter algorithm process as the basic implementation framework. The algorithm design is carried out through the setting of state conditions and parameters, which can be carried out in the following steps.

Figure 4. Flowchart of the algorithm for MVDR+PF.

Figure 4. Flowchart of the algorithm for MVDR+PF.

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\left(i\right)\sim U[0,\pi ]$;

The pitch angle ${\phi }_0\left(i\right)\sim U[0,\pi /2],{\dot{\phi }}_0\left(i\right),\dot{{\theta }_0}\left(i\right)\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\left(i\right)$ and ${\ddot{\phi }}_0\left(i\right)$ are initialized to 0, and the weight value is 1/N;

2. Iteration: $\mathrm{for}\mathrm{i}=1:Y$

(a) Obtain observation data

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

(b) Importance sampling stage

For 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)=\frac{W_k\left(x_k^{\left(i\right)}\right)}{\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=\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)=\frac{1}{N}$

3. End

4. Simulation Experiments and Analyses

For the navigation and positioning problem based on moving targets, In order to verify the effectiveness of the proposed algorithm, a simulation experiment was conducted to test it. In the simulation, the main purpose is to verify the influence of the signal-to-noise ratio, the number of snapshots, the number of particles and the interference signal source on the effectiveness of the algorithm proposed in this article under the uniform motion of the moving target object.

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, 4 array elements on each X-axis and Y-axis, 0.2m between array elements (the distance between array elements is greater than half the wavelength of the signal) The initial azimuth and elevation angles are (45°, 45°); The wave signal is assumed to be a parallel wave; the number of snapshots is 256; particles are 800; Observation noise is introduced with a signal-to-noise ratio of 10 dB.

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

It can be seen from the results of the figure that, except for the starting position of the target, there is a large deviation in the direction angle of the target object predicted by the algorithm, 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 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. Modify the initial velocity of each particle to be 0.3°⁄ s, The acceleration is randomly transformed between the range [-0.5,0.5]; in order 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.

In Figure 6(a) plot shows the deviation of the target object orientation trajectory and the algorithm-predicted trajectory in terms of spatial orientation angle in the variable speed condition. Figure 6(b) shows the prediction results of the azimuth and elevation angles of the algorithm on the true angle of the target under the variable speed condition. As can be seen from the change in the spatial angular position of the target in Figure 6(a), the target object experienced accelerated motion, decelerated motion process, and a reverse motion state during the whole moving process. In the simulation process, the trend of the direction change of the predicted curve and the trend of the target curve change are consistent, and the azimuth and elevation angles fluctuate around the real value with a small error. 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 of 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 real 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=\frac{1}{n}\sqrt{\frac{1}{50}{\displaystyle {\sum }_{j=1}^{50}\left[{(P{\theta }_i-T{\theta }_i)}^2+{(P{\varphi }_i-T{\varphi }_i)}^2\right]}}$$

(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 azimuth and dip angle in step $i$ ；$T{\theta }_i$ and $T{\varphi }_i$represent the real values of azimuth and dip angle. The simulation results of the experiment are shown in Figure 7.

From Figure 7, it can be seen that with the increase of the number of particles, the error between the navigation trajectory and the real trajectory becomes smaller and smaller, and the localization accuracy is gradually improved. Comparing the rms 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 obvious 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 with respect to 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. Examine the variation of RMSE of the algorithm proposed in this paper with the number of snapshots. The number of snapshots is between [50,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 real 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 -5 dB to 30 dB and the number of snapshots changes from 50 to 2000, and the estimation accuracy of the algorithm is getting higher and higher as the signal-to-noise ratio and the number of snapshots increase. This is due to the fact that 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, and 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, it can be seen that the localization performance of the proposed algorithm is better than the other two localization algorithms. The proposed method enables 90% of the users to locate within 3 meters, while for the other single two localization algorithms, the error is more than 5.6 meters, the algorithm can improve the positioning accuracy by about 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 model of the system, the behavioral information of the target object and the transient information of the signal source are combined to overcome the defects of the traditional DOA algorithm that only localizes the fixed signal source. Then, in order to reasonably calculate the weights of the particles and reduce the particle scarcity, a particle filtering weight calculation function based on Gaussian function is designed to resample the function. The effectiveness of the algorithm is verified through the form of simulation. It is verified that the algorithm can realize the tracking navigation of the target and has strong robustness to the nonlinear model of the moving target.

6. Patents

This section is not mandatory but may be added if there are patents resulting from the work reported in this manuscript.