# Robust Bluetooth AoA Estimation for Indoor Localization Using Particle Filter Fusion
## 3 Methods
### 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 \(\Delta T\), the true azimuth angle of the target at the sampling moment \(k\Delta T\) is \(\theta(k)\), 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{split}\theta(k+1)&=\theta(k)+\dot{\theta}( k)\Delta T+1/2\ddot{\theta}(k)\Delta T^{2}+1/6\
u\Delta T^{3}\\ \dot{\theta}(k+1)&=\dot{\theta}(k)+\ddot{\theta}( k)\Delta T+1/2\
u\Delta T^{2}\\ \ddot{\theta}(k+1)&=\ddot{\theta}(k)+\Delta T\mathcal{ V}\end{split}  \tag{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:  
\[ \begin{bmatrix}\theta(k+1)\\ \dot{\theta}(k+1)\\ \ddot{\theta}(k+1)\end{bmatrix} = \begin{bmatrix}1&\Delta T&1/2\Delta T^{2}\\ 0&1&\Delta T\\ 0&0&1\end{bmatrix}\begin{bmatrix}\theta(k)\\ \dot{\theta}(k)\\ \ddot{\theta}(k)\end{bmatrix} + \begin{bmatrix}1/6\Delta T^{3}\\ 1/2\Delta T^{2}\\ \Delta T\end{bmatrix} \mathcal{V} \tag{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=\Big{[}\theta,\dot{\theta},\ddot{\theta},\varphi,\dot{\varphi},\ddot{\varphi }\Big{]}^{T} \tag{21}\]  
The state vector at time \(k\) is expressed as:  
\[x_{k}=Ax_{k-1}+B\
u \tag{22}\]Among them, the state transition matrix is:  
\[A= \begin{bmatrix}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{bmatrix} ,\;\;B= \begin{bmatrix}1/6\Delta T^{3}\\ 1/2\Delta T\\ \Delta T\\ 1/6\Delta T^{3}\\ 1/2\Delta T^{2}\\ \Delta T\end{bmatrix}  \tag{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(x_{k},\omega)\] (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.