## 3 Localization Algorithm
### The Dynamic Reference Selection-Based Self-Localization Algorithm
#### 3.2.1 The Prediction-Based Self-Localization Algorithm  
Considering the mobility model and the observation model of DUANs, we apply the extension Kalman filter (EKF) method to predict the node's position and the mobility pattern in DUANs [31; 32].  
Mobility model:  
\[\widehat{\Gamma}_{j}(k+1)=\left[\begin{array}{c}X_{j}(k+1)\\ v_{j}(k+1)\\ a_{j}(k+1)\end{array}\right]=F\widehat{\Gamma}_{j}(k)+\omega_{j}(k) \tag{6}\]  
where \(v_{j}(k+1)\) is the velocity of node \(j\) at time \(k+1\), \(a_{j}(k+1)\) is the acceleration of node \(j\) at time \(k+1\), \(F\) is the state transformation matrix which can be expressed as  
\[F=\left[\begin{array}{cccccc}1&0&0&T_{0}&(-1+\alpha t_{0}+e^{-\alpha T_{0}} )/\alpha^{2}\\ 0&1&0&T_{0}&(-1+\alpha t_{0}+e^{-\alpha T_{0}})/\alpha^{2}\\ 0&0&1&T_{0}&(-1+\alpha t_{0}+e^{-\alpha T_{0}})/\alpha^{2}\\ 0&0&0&1&(1-e^{-\alpha t_{0}})/\alpha\\ 0&0&0&0&e^{-\alpha t_{0}}\end{array}\right]\]  
\[\omega_{j}(k)=\left[\begin{array}{c}\omega_{1}(k)\\ \omega_{2}(k)\\ \omega_{3}(k)\\ \omega_{4}(k)\\ \omega_{5}(k)\end{array}\right]\]\[X_{j}(k+1)=\left[\begin{array}{c}x_{j}(k+1)\\ y_{j}(k+1)\\ z_{j}(k+1)\end{array}\right]\]  
where \(\omega_{j}(k)\) is the mobility noise vector, \(\alpha\) is the model factor. \(t_{0}\) is the sampling period.  
Observation model:  
The observation model is derived from the range measurement as shown in Equations (7) and (8).  
\[Z_{j}(k+1)=\left[\begin{array}{c}Z_{(1,j)}(k+1)\\ Z_{(2,j)}(k+1)\\ Z_{(3,j)}(k+1)\\ Z_{(i,j)}(k+1)\end{array}\right] \tag{7}\]  
\[Z_{(l,j)}(k+1) =h_{(l,j)}(X_{l}(k+1),X_{j}(k+1))+\mu_{(l,j)}(k+1) \tag{8}\] \[=\parallel X_{l}(k+1)-X_{j}(k+1)\parallel+\mu_{(l,j)}(k+1),l=1,2,3,i\]  
where \(Z_{(l,j)}(k+1)\) is the range measurement between node \(l\) and node \(j\) at time \(k+1\). \(\mu_{(l,j)}(k+1)\) is the noise vector, \(\mu_{(l,j)}(k+1)\sim N(0,R(k))\), \(R(k)\) is the variance of observation noise.  
Since the nonlinearity of the observation model as in Equation (8), we use EKF method to predict the mobile node's position and the mobility parameter simultaneously.  
\[\widehat{\Gamma}_{j}(k+1\mid k)=F\widehat{\Gamma}_{j}(k\mid k)\]  
\[P(k+1\mid k+1)=FP(k\mid k)F^{T}+Q(k+1)\]  
\[K(k+1)=P(k+1\mid k)f_{j}^{T}[J_{j}P(k+1\mid k)f_{j}^{T}+R(k+1)] \tag{9}\]  
\[\widehat{\Gamma}_{j}(k+1\mid k+1)=\widehat{\Gamma}_{j}(k+1\mid k)+K(k)[Z(k)-h (\widehat{\Gamma}_{j}(k+1\mid k))]\]  
\[P(k+1\mid k+1)=P(k+1\mid k)-K(k)[J_{j}P(k)\mid k]^{T}+R(k+1)]K^{T}(k+1)\]  
where \(P\) is the state covariance matrix, \(F\) is the state transformation matrix, \(Q\) is the state transformation covariance matrix, \(R\) is the observation noise covariance matrix and \(J_{(l,j)}\) is the Jacobians of the nonlinear function \(h_{(l,j)}\).  
\[J_{j}=\left[\begin{array}{c}J_{(1,j)}(k+1)\\ J_{(2,j)}(k+1)\\ J_{(3,j)}(k+1)\\ J_{(i,j)}(k+1)\end{array}\right] \tag{10}\]  
\[J_{(l,j)} =\frac{\partial h^{(l,j)}(X_{l}(k+1),X_{j}(k+1))}{\partial X_{l}( k+1)} \tag{11}\] \[=\left[\begin{array}{cc}\frac{x_{j}(k+1)-x_{j}(k+1)}{d_{(l,j)}( k+1)}&\frac{y_{j}(k+1)-y_{j}(k+1)}{d_{(l,j)}(k+1)}&\frac{z_{j}(k+1)-z_{j}(k+1)}{d_{( l,j)}(k+1)}\end{array}\right],l=1,2,3,i\]