# A Robust Tracking Algorithm Based on a Probability Data Association for a Wireless Sensor Network
## 3 Proposed Method
### Model Matching  
_Prediction:_ The mixed state estimates and the mixed covariance matrix estimates obtained after the interaction are provided as initial estimates to the two model-matched filters for prediction:  
\[\hat{X}_{j}(k|k-1)=A\hat{X}_{0j}(k-1|k-1) \tag{12}\]  
\[P_{j}(k|k-1)=AP_{0j}(k-1|k-1)A^{T}+CQ(k)C^{T} \tag{13}\]  
where \(\hat{X}_{j}(k|k-1)\) is the state prediction of the LOS/NLOS model at time step \(k\) and \(P_{j}(k|k-1)\) is the error covariance matrix prediction of the LOS/NLOS model at time step \(k\). _Update:_ When \(j=1\), the state estimate update and the error covariance matrix estimate update are performed using the extended Kalman filter. Furthermore:  
\[H_{1}(k)=\frac{\partial h(X(k))}{\partial X(k)}\bigg{|}_{X(k)=\hat{X}_{1}(k|k- 1)}, \tag{14}\]  
\[v_{1}(k)=D(k)-h\big{(}\hat{X}_{1}(k|k-1)\big{)}, \tag{15}\]  
\[S_{1}(k)=H_{1}(k)P_{1}(k|k-1)H_{1}^{T}(k)+R_{1}^{*}(k), \tag{16}\]  
where \(H_{1}(k)\) is the Jacobian matrix at time step \(k\). \(v_{1}(k)\) denotes the innovation at time step \(k\).\(S_{1}(k|k-1)\) is the innovation covariance matrix. \(R_{1}^{*}(k)\) is the covariance matrix of the measurement errors vector. Furthermore:  
\[K_{1}(k)=P_{1}(k|k-1)H_{1}^{T}(k)S_{1}^{-1}(k|k-1), \tag{17}\]  
\[\hat{X}_{1}(k|k)=\hat{X}_{1}(k|k-1)+K_{1}(k)v_{1}(k), \tag{18}\]  
\[P_{1}(k|k)=(I_{4}-K_{1}(k)H_{1}(k))P_{1}(k|k-1), \tag{19}\]  
\[\Lambda_{1}(k)=N(v_{1}(k);0,S_{1}(k)), \tag{20}\]  
where \(K_{1}(k)\) is the extended Kalman gain at time \(k\), \(\hat{X}_{1}(k|k)\) is the updated state estimate, \(P_{1}(k|k)\) denotes the updated error covariance matrix, and \(\Lambda_{1}(k)\) is the likelihood function. When \(j=2\), the improved MPDA filter is used to update the state estimation prediction and error covariance matrix prediction in the NLOS model. The proposed method divides the range measurements into \(N=\left( \begin{array}{c}M\\ 3\end{array} \right)\) different subgroups, which are used together with the positions of the corresponding beacon nodes to determine the position of the mobile node, and each subgroup uses the least squares estimation to obtain the corresponding position estimation \(z_{n}(k)\). The position estimation prediction \(\hat{z}(k|k-1)\) of the mobile node is given as:  
\[\hat{z}(k|k-1)=B\hat{X}_{2}(k|k-1), \tag{21}\]  
\[v_{2,n}(k)=z_{n}(k)-\hat{z}(k|k-1),\ n=1,\ \ldots,\ N, \tag{22}\]  
where \(B=\left[ \begin{array}{ccc}1&0&0&0\\ 0&1&0&0\end{array} \right]\) is the observation matrix, \(v_{2,n}(k)\) is the innovation for the position estimate from subgroup \(n\) at time step \(k\). Then, we perform the NLOS detection for the position estimation obtained to determine the correct position estimation. _NLOS detection:_ If all the innovations do not exist as NLOS errors, then there are:  
\[v_{2,n}(k)\sim N(0,S_{2,n}(k)),\ n=1,\ \ldots,\ N. \tag{23}\]In order to validate Equation (21), we define \(N\) hypotheses and alternatives:  
\[\zeta_{0,n}:v_{2,n}(k)\,\sim N(0,S_{2,n}(k)),\ n=1,\ \ldots,\ N \tag{24}\]  
\[\zeta_{1,n}:\ \text{not}\ \zeta_{0,n},\ n=1,\ \ldots,\ N \tag{25}\]  
If the beacon nodes from subgroup \(n\) are in the LOS condition, the hypothesis \(\zeta_{0,n}\) holds true, and the position estimate \(z_{n}(k)\) from subgroup \(n\) falls into the validation region; otherwise, the hypothesis \(\zeta_{1,n}\) holds true. The test statistic \(T_{n}(k)\) can be expressed as:  
\[T_{n}(k)=v_{2,n}^{T}(k)S_{2,n}^{-1}(k)v_{2,n}(k). \tag{26}\]  
\(T_{n}(k)\) is verified with the validation gate \(\gamma\) to determine whether the position estimate \(z_{n}(k)\) is within the validation region. If \(T_{n}(k)\) is not larger than the validation gate \(\gamma\), the hypothesis \(\zeta_{0,n}\) holds true, and \(z_{n}(k)\) is accurate. If \(T_{n}(k)\) is larger than the validation gate \(\gamma\), the hypothesis \(\zeta_{0,n}\) is rejected, and \(z_{n}(k)\) is discarded due to it being an NLOS error.