# Robust Bluetooth AoA Estimation for Indoor Localization Using Particle Filter Fusion
## 3 Methods
### MVDR+PF Nonlinear Dynamical System Modeling
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(t)+N(t) \tag{25}\]  
where \(S(t)\) is the source signal, \(A\) is the steering matrix of the array, and \(N(t)\) 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}=[G(kL+1),\;\;\;G(kL+2),\;\;\;\ldots,\;\;\;G(kL+L) \tag{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} \tag{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(x_{k}\mid x_{k-1}),p(y_{k}\mid x_{k})\) 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 \(\varphi_{0}(i)\sim U[0,\pi/2]\), \(\dot{\varphi}_{0}(i),\dot{\theta}_{0}(i)\sim N\big{(}\mu_{0},\epsilon_{0}^{2} \big{)},i=1,\ldots,N,U[e_{1},e_{2}]\) is evenly distributed, \(N\big{(}\mu,\epsilon^{2}\big{)}\) normal distribution, \(\tilde{\theta}_{0}(i)\) and \(\tilde{\varphi}_{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}=\frac{1}{L}G_{k}G_{k}^{H}\);  
(b) Importance sampling stage  
For the transfer equation according to status \(q(x_{k}(i)\mid x_{0k-1}(i),Y_{1:k})=p(x_{k}(i)\mid x_{k-1}(i))\) carry out sampling \((\theta_{k}(i),\varphi_{k}(i))\),  
For \(i=1:N\); update the weight of the particle based on the weight calculation formula \(W_{k}\Big{(}x_{k}^{(i)}\Big{)}=W_{k-1}\Big{(}x_{k-1}^{(i)}\Big{)}p(y_{k}\mid x _{k})\)  
Normalize the weights of particles according to \(\widetilde{W}_{k}\Big{(}x_{k}^{(i)}\Big{)}=\frac{w_{k}\big{(}x_{k}^{(i)}\big{)} }{\frac{i! }{\sum\limits^{N}W_{k}\big{(}x_{k}^{(i)}\big{)}}}\)  
(c) Algorithm resampling stage  
Resample the particle set \(\Big{\{}x_{k}^{(i)},w_{k}\Big{(}x_{k}^{(i)}\Big{)}\Big{\}}\) and reinitialize the weight values. Get new particle set \(\Big{\{}x_{k}^{(i)},w_{k}\Big{(}x_{k}^{(i)}\Big{)}\Big{\}}\)  
(d) Result output  
Figure 4: Flowchart of the algorithm for MVDR+PF. Figure 3: Relationship between the three measures.